public void Go(float xCm, float yCm)
{
if (Moving)
{
return;
}
motor.ResetEncoder();
if (xCm == 0 && yCm == 0)
{
return;
}
desiredEncoderValue = (int)(xCm != 0 ? xCm : yCm) * (998 / 32);
encoderToWatch = MotorControl.Motors.FrontLeft;
if ((xCm < 0 && yCm > 0) || (xCm > 0 && yCm < 0))
{
desiredEncoderValue = (int)(desiredEncoderValue * 1.414213562373);
encoderToWatch = MotorControl.Motors.FrontRight;
desiredEncoderValue *= -1;
}
else if ((xCm < 0 && yCm < 0) || (xCm > 0 && yCm > 0))
{
desiredEncoderValue = (int)(desiredEncoderValue * 1.414213562373);
}
int xSpeed = (xCm > 0 ? Speed : -Speed);
int ySpeed = (yCm > 0 ? Speed : -Speed);
if (xCm == 0)
{
xSpeed = 0;
}
if (yCm == 0)
{
ySpeed = 0;
}
motor.SetDestination(xSpeed, ySpeed, 0);
Moving = true;
}
public void Rotate(float degree)
{
if (Moving)
{
return;
}
motor.ResetEncoder();
if (degree == 0)
{
return;
}
desiredEncoderValue = (int)(degree * (665 / 35));
encoderToWatch = MotorControl.Motors.FrontLeft;
int wSpeed = (degree > 0 ? 1 : -1);
motor.SetDestination(0, 0, wSpeed);
Moving = true;
}
