private void Joystick_PositionChanged(object sender, JoystickPositionEventArgs e)
{
_potisionTextView.Text = $"X:{e.PositionX} Y:{e.PositionY}";
// Determine the Servo direction (Forward, Backward, Stop)
// stop = 0, forward > 0, backward < 0
ServoDirection servoDirection = ServoDirection.Stop;
if (e.PositionY > 0)
{
servoDirection = ServoDirection.Forward;
}
if (e.PositionY < 0)
{
servoDirection = ServoDirection.Backward;
}
SteerRobot(servoDirection, e.PositionX);
}
private void SteerRobot(ServoDirection servoDirection, int turnValue)
{
// If moving FORWARD then ...
if (servoDirection == ServoDirection.Forward)
{
// If turning LEFT
if (turnValue < -150)
{
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.CounterMaxSpeed - 30;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.StopSpeed;
}
// If turning RIGHT
else if (turnValue > 150)
{
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.StopSpeed;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.ClockwiseMaxSpeed + 60;
}
else
{
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.CounterMaxSpeed - 30;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.ClockwiseMaxSpeed + 60;
}
}
// If moving BACKWARD then ...
if (servoDirection == ServoDirection.Backward)
{
// If turning LEFT
if (turnValue < -150)
{
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.StopSpeed;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.CounterMaxSpeed;
}
// If turning RIGHT
else if (turnValue > 150)
{
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.ClockwiseMaxSpeed;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.StopSpeed;
}
else
{
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.ClockwiseMaxSpeed;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.CounterMaxSpeed;
}
}
// If STOPPING then ...
if (servoDirection == ServoDirection.Stop)
{
// If turning LEFT
RobotParameters.ServoA.CurrentRotationPosition = RobotParameters.StopSpeed;
RobotParameters.ServoB.CurrentRotationPosition = RobotParameters.StopSpeed;
}
// Display the final speed
_potisionTextView.Text += $"\r\nA:{RobotParameters.ServoA.CurrentRotationPosition} B:{RobotParameters.ServoB.CurrentRotationPosition}";
// Send the Message to the Robot
string message = RobotMessage.FormatSteerMessage(RobotParameters.ServoA, RobotParameters.ServoB);
sendRobotMessage(RobotAction.Steer, message);
}