Add CozmoMovementTest script to find out rotation and movement speed in the simulation

Assets/Scripts/Movemnet/CozmoMovement.cs

@@ -4,17 +4,17 @@ using UnityEngine;
 
 public class CozmoMovement : MonoBehaviour
 {
-  
+
     public float m_Speed = 12f;                 // How fast the tank moves forward and back.
     public float m_TurnSpeed = 180f;            // How fast the tank turns in degrees per second.
-  
+
 
     private string m_MovementAxisName;          // The name of the input axis for moving forward and back.
     private string m_TurnAxisName;              // The name of the input axis for turning.
     private Rigidbody m_Rigidbody;              // Reference used to move the tank.
     private float m_MovementInputValue;         // The current value of the movement input.
     private float m_TurnInputValue;             // The current value of the turn input.
-  
+
 
     private void Awake()
     {
@@ -43,9 +43,9 @@ public class CozmoMovement : MonoBehaviour
     private void Start()
     {
         // The axes names are based on player number.
-        m_MovementAxisName = "Vertical"; 
-        m_TurnAxisName = "Horizontal"; 
-       
+        m_MovementAxisName = "Vertical";
+        m_TurnAxisName = "Horizontal";
+
     }
 
 
@@ -53,9 +53,9 @@ public class CozmoMovement : MonoBehaviour
     {
         // Store the value of both input axes.
         m_MovementInputValue = Input.GetAxis(m_MovementAxisName);
-        m_TurnInputValue = Input.GetAxis(m_TurnAxisName);       
+        m_TurnInputValue = Input.GetAxis(m_TurnAxisName);
     }
-    
+
 
     private void FixedUpdate()
     {
@@ -64,21 +64,34 @@ public class CozmoMovement : MonoBehaviour
         Turn();
     }
 
-
-    private void Move()
+    public void Move(float directionValue)
     {
         // Create a vector in the direction the tank is facing with a magnitude based on the input, speed and the time between frames.
-        Vector3 movement = transform.forward * m_MovementInputValue * m_Speed * Time.deltaTime;
+        Vector3 movement = directionValue * transform.forward * m_Speed * Time.deltaTime;
 
         // Apply this movement to the rigidbody's position.
         m_Rigidbody.MovePosition(m_Rigidbody.position + movement);
     }
 
+    public void Move()
+    {
+        Move(m_MovementInputValue);
+    }
 
-    private void Turn()
+    //public void Move(float[] act)
+    //{
+
+    //}
+
+    public void Turn()
+    {
+        Turn(m_TurnInputValue);
+    }
+
+    public void Turn(float turnValue)
     {
         // Determine the number of degrees to be turned based on the input, speed and time between frames.
-        float turn = m_TurnInputValue * m_TurnSpeed * Time.deltaTime;
+        float turn = turnValue * m_TurnSpeed * Time.deltaTime;
 
         // Make this into a rotation in the y axis.
         Quaternion turnRotation = Quaternion.Euler(0f, turn, 0f);

Assets/Scripts/Movemnet/CozmoMovementTester.cs

@@ -0,0 +1,105 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Diagnostics;
+using UnityEngine;
+
+public enum TestStates { MovementSpeed, RotationSpeed }
+
+[RequireComponent(typeof(CozmoMovement))]
+public class CozmoMovementTester : MonoBehaviour
+{
+
+    private const float DISTANCE_TO_MEASURE_SPEED = 1f; // Distance that is used to calculate cozmos movementspeed (in m)
+    private const float ROTATION_ANGLE = 90f;          // Angle that is used to calculate cozmos rotationspeed 
+
+    private CozmoMovement movement;             // Reference to the movement script of cozmo
+    private Vector3 startPositionCozmo;         // Original position of cozmo
+    private float lastY;                        // Last Y of cozmo
+    private float curAngleX = 0;                // Current angle in the x axis of cozmo
+    private Stopwatch movementWatch;            // Stopwatch to measure the time for the movement usecase
+    private Stopwatch rotationWatch;            // Stopwatch to measure the time for the rotation usecase  
+
+    [Tooltip("Choose which speed of the virtual cozmo should be tested.")]
+    public TestStates testState = TestStates.MovementSpeed;
+
+    void Start()
+    {
+        movement = GetComponent<CozmoMovement>();
+        movementWatch = new Stopwatch();
+        rotationWatch = new Stopwatch();
+
+
+        startPositionCozmo = movement.transform.position;   // Cache the starting position of Cozmo    
+        lastY = movement.transform.rotation.y;           // Cache the rotation Vector of Cozmo    
+    }
+
+    private void FixedUpdate()
+    {
+        if (testState == TestStates.MovementSpeed)
+        {
+            TestMovementSpeed();
+        }
+        else if (testState == TestStates.RotationSpeed)
+        {
+            TestRotationSpeed();
+        }
+    }
+
+    private void TestRotationSpeed()
+    {
+
+        float currentY = movement.transform.rotation.eulerAngles.y;
+
+        // If distance between start and current position of cozmo reached the  DISTANCE_TO_MEASURE_SPEED stop the stopwatch
+        if (currentY >= lastY + ROTATION_ANGLE || currentY <= lastY - ROTATION_ANGLE)
+        {
+            if (rotationWatch.IsRunning)
+            {
+                rotationWatch.Stop();
+                UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
+                    "/nElapsed time in milliseconds (Rotation): " + rotationWatch.ElapsedMilliseconds);
+            }
+
+            return;
+        }
+        else
+        {
+            // Start stopwatch to measure the time cozmo needs for a specific rotation
+            if (!rotationWatch.IsRunning)
+            {
+                rotationWatch = Stopwatch.StartNew();
+            }
+
+            // Turn right
+            movement.Turn(1);
+        }
+    }
+
+    private void TestMovementSpeed()
+    {
+        // If distance between start and current position of cozmo reached the  DISTANCE_TO_MEASURE_SPEED stop the stopwatch
+        if (Vector3.Distance(startPositionCozmo, movement.transform.position) >= DISTANCE_TO_MEASURE_SPEED)
+        {
+            if (movementWatch.IsRunning)
+            {
+                movementWatch.Stop();
+                UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
+                    "/nElapsed time in milliseconds (Movement): " + movementWatch.ElapsedMilliseconds);
+            }
+
+            return;
+        }
+        else
+        {
+            // Start stopwatch to measure the time cozmo needs for a specific distance
+            if (!movementWatch.IsRunning)
+            {
+                movementWatch = Stopwatch.StartNew();
+            }
+
+            // Move cozmo in forward direction
+            movement.Move(1);
+        }
+    }
+}

Assets/Scripts/Movemnet/CozmoMovementTester.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 4265d0764c65d26409f0f920e5293c82
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: