127 lines
4.6 KiB
C#
127 lines
4.6 KiB
C#
///-----------------------------------------------------------------
|
|
/// Namespace: <CozmoHelpers>
|
|
/// Class: <CozmoMovementTester>
|
|
/// Description: <Used to measure the time for rotating and moving the virtual cozmo to copy the
|
|
/// real cozmos movement behaviour.>
|
|
/// Author: <Tobias Hassel> Date: <29.07.2019>
|
|
/// Notes: <>
|
|
///-----------------------------------------------------------------
|
|
///
|
|
|
|
using Cozmo;
|
|
using System.Diagnostics;
|
|
using UnityEngine;
|
|
|
|
namespace CozmoHelpers
|
|
{
|
|
public enum TestStates { MovementSpeed, RotationSpeed } // enum to determine test scenarios
|
|
|
|
[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 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;
|
|
|
|
[Tooltip("Start and stop testing from the inspector.")]
|
|
public bool isTesting = false;
|
|
|
|
|
|
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 (isTesting)
|
|
{
|
|
if (testState == TestStates.MovementSpeed)
|
|
{
|
|
TestMovementSpeed();
|
|
}
|
|
else if (testState == TestStates.RotationSpeed)
|
|
{
|
|
TestRotationSpeed();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Measure time for rotation
|
|
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);
|
|
}
|
|
|
|
isTesting = false;
|
|
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);
|
|
}
|
|
}
|
|
|
|
// Measure time for movement
|
|
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);
|
|
}
|
|
|
|
isTesting = false;
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|