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