void BasicMoveController::forward(double distance)
{
geometry_msgs::Point pos0 = odometry_.pose.pose.position;
while (mtk::distance2D(pos0, odometry_.pose.pose.position) < distance)
{
slowForward();
}
}
task main()
{
bool goesback=false;
initializeRobot();
waitForStart();
distForward(42);wait1Msec(300);
ClearTimer(T1);
while(SensorValue[LSonar]>8 && time1(T1)<2000)
{
standardDrive(50,50);
}
standardDrive(0,0);wait1Msec(300);
if(SensorValue[LSonar]>8)
{
goesback=true;ClearTimer(T1);
while(SensorValue[LSonar]>8 && time1(T1)<1000)
{
standardDrive(-100,-100);
}
}
standardDrive(0,0);wait1Msec(300)
if(!goesback)
{
distForward(15);wait1Msec(200);
}
else
{
distForward(19);wait1Msec(200);
}
turnRSideTo(45);wait1Msec(300);
slowBackward(60);wait1Msec(200);
slowForward(8);wait1Msec(300);
turnRSideTo(90);wait1Msec(300);
slowBackward(24);wait1Msec(300);
standardDrive(0,0);wait1Msec(300);
}
