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; }