public void turnRight(int angle)
{
double angleRad = exMath.ToRad(angle);
Engine.getInstance().Info("initial:" + initialHeading);
Engine.getInstance().Info("anglepos:" + angle);
Debug.Print("initialangle:" + initialHeading.ToString());
//Let's suppose the mass center is in the geometrical center of the rover
double lengthRight = angleRad * GlobalVal.width_correction * GlobalVal.distanceBetweenWheels_mm / 2;
double lengthLeft = lengthRight;
turnRight();
while (right.distance_mm < lengthRight && left.distance_mm < lengthLeft && !contingency.alarm)
{
Thread.Sleep(50);
}
this.brake();
//actualPosition.angle += (left.distance_mm + right.distance_mm) / (GlobalVal.width_correction * GlobalVal.distanceBetweenWheels_mm);
actualPosition.angle = this.Relative_MAG_Heading;
Debug.Print("angle:" + this.MAG_Heading.ToString());
Debug.Print("angle_gir:" + this.actualPosition.angle.ToString());
}
public void Act()
{
while (!end)
{
if (alarm)
{
lock (monitor)
{
if (count >= 5)
{
Engine.getInstance().Debug("Contingency count: " + count);
if (count >= 10)
{
navigation.turnLeft(20, (sbyte)90);
}
else if (count >= 15)
{
navigation.turnRight(20, (sbyte)90);
count = 10;
}
else
{
navigation.MoveForward(100, (sbyte)90);
}
}
}
Engine.getInstance().Debug("Solving problem when moving " + movement.ToString());
switch (movement)
{
case Movement.forward:
navigation.MoveBackward(100, (sbyte)40);
break;
case Movement.backward:
navigation.MoveForward(100, (sbyte)40);
break;
case Movement.left:
navigation.turnRight(10, (sbyte)40);
break;
case Movement.right:
navigation.turnLeft(10, (sbyte)50);
break;
}
Engine.getInstance().Restart();
alarm = false;
}
}
}
public void SetYOFF(short _cy)
{
Engine.getInstance().Info("ola que ase2" + _cy);
magnetometer.SetYOFF(_cy);
}
public void SetXOFF(short _cx)
{
Engine.getInstance().Info("Ola que ase" + _cx);
magnetometer.SetXOFF(_cx);
}