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 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);
}
}
}
static bool is_moving(Position2dProxy& pp)
{
return pp.GetXSpeed() || pp.GetYSpeed() || pp.GetYawSpeed();
}
void moveRobot(LaserProxy &lp, Position2dProxy &pp, double *newspeed, double *newturnrate){
laserAvoid(lp, newspeed, newturnrate);
pp.SetSpeed(*newspeed, *newturnrate);
}
double getDistance(double xOrigin, double yOrigin, Position2dProxy & position){
return sqrt(pow((position.GetXPos() - xOrigin), 2.0) + pow((position.GetYPos() - yOrigin), 2.0));
}
void debugCode(Position2dProxy &position){
cout<< "debug";
cout<<position.GetXPos()<<", "<<position.GetYPos()<<", "<<rtod(position.GetYaw())<< endl;
}
