//Commands with eye tracking
private void SendCommandToRobot(Vector2 controlOutput)
{
// Debug.Log("Sending command to robot");
// Debug.Log("ControlOutput is :" + controlOutput.x +" " + controlOutput.y);
IsDriving = true;
Vector2 movement = new Vector2(controlOutput.y, -controlOutput.x);
// Debug.Log("Intial Linear speed was :" + movement.x + "Initial Angular speed was : " + movement.y);
//if you are not at the dead zone
if (!InsideDeadZone(movement.x, movement.y))
{
//normalize speed and send data
movement = new Vector2(FilterLinearVelocity(movement.x), FilterAngularVelocity(movement.y));
// Debug.Log("Normalized Linear speed was :" + movement.x + "Normalized Initial Angular speed was : " + movement.y);
_rosLocomotionDirect.PublishData(movement.x, movement.y);
_isStopped = false;
}
else
{
// Debug.Log("Inside Dead Zone");
// Debug.Log("Linear speed was :" + 0 + "Angular speed was : " + 0);
_rosLocomotionDirect.PublishData(0, 0);
_isStopped = false;
}
}
public override void MoveDirect(Vector2 command)
{
if (CurrenLocomotionType != RobotLocomotionType.DIRECT)
{
_rosLocomotionWaypointState.PublishData(ROSLocomotionWaypointState.RobotWaypointState.STOP);
}
_rosLocomotionDirect.PublishData(command.y, command.x);
CurrenLocomotionType = RobotLocomotionType.DIRECT;
CurrentRobotLocomotionState = RobotLocomotionState.MOVING;
}
private void PublishData(int leftTicks, int rightTicks)
{
float left = leftTicks * Time.deltaTime;
float right = rightTicks * Time.deltaTime;
float linearSpeed = ((left + right) / 2) * _linearCoefficient;
float angularSpeed = ((left - right) / _wheelbase) * _angularCoefficient;
_locomotionDirect.PublishData(linearSpeed, angularSpeed);
}
void Update()
{
Vector2 vel = Vector2.zero;
if (Input.GetKey(KeyCode.A) || Input.GetKey(KeyCode.LeftArrow))
{
vel += new Vector2(1, 0);
}
if (Input.GetKey(KeyCode.D) || Input.GetKey(KeyCode.RightArrow))
{
vel += new Vector2(-1, 0);
}
if (Input.GetKey(KeyCode.W) || Input.GetKey(KeyCode.UpArrow))
{
vel += new Vector2(0, 1.5f);
}
if (Input.GetKey(KeyCode.S) || Input.GetKey(KeyCode.DownArrow))
{
vel += new Vector2(0, -1.5f);
}
_timer += Time.deltaTime;
if (_timer >= (float)_publishingRateMs / 1000)
{
_timer = 0;
if (vel == Vector2.zero && !_stopped)
{
_stopped = true;
_cmdVel.PublishData(vel.y, vel.x);
}
else if (vel != Vector2.zero)
{
_stopped = false;
_cmdVel.PublishData(vel.y, vel.x);
}
}
}
private void PublishVelocity(TwistMsg twist)
{
_rosLocomotionDirect.PublishData((float)twist._linear._x, (float)twist._angular._z);
}