void Roadway1()
{
pc.Read();
pp.SetSpeed(0.2, 0);
while(1)
{
pc.Read();
if ((sp[MID] < 0.6) || (sp[MID_LEFT] < 0.6) || (sp[MID_RIGHT] < 0.6))
{
PrintObjectLocationByPosition(pp.GetXPos(), pp.GetYPos(), pp.GetYaw(), 2, sp[2]);
// TURN
float Angle;
if (sp [LEFT] > sp[RIGHT])
{
Angle = dtor(30);
}
else
{
Angle = dtor(-30);
}
pp.SetSpeed(0, Angle);
while ((sp[MID] < 0.6) || (sp[MID_LEFT] < 0.6) || (sp[MID_RIGHT] < 0.6))
pc.Read();
pp.SetSpeed(0.2, 0);
}
}
}
void MoveDistance(PlayerClient pc, Position2dProxy& pp, double Meters)
{
pc.Read();
int StartPos = pp.GetXPos();
pp.SetSpeed(SPEED, 0);
while ((pp.GetXPos() - StartPos) < 1)
{
pc.Read();
}
pp.SetSpeed(0, 0);
}
void moveRobot(LaserProxy &lp, Position2dProxy &pp, double *newspeed, double *newturnrate){
laserAvoid(lp, newspeed, newturnrate);
pp.SetSpeed(*newspeed, *newturnrate);
}