コード例 #1
0
ファイル: main.cpp プロジェクト: IdanShamash/Robotika
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);
		}
	}
}
コード例 #2
0
double getDistance(double xOrigin, double yOrigin, Position2dProxy & position){
	return sqrt(pow((position.GetXPos() - xOrigin), 2.0) + pow((position.GetYPos() - yOrigin), 2.0));
}
コード例 #3
0
void debugCode(Position2dProxy &position){
	cout<< "debug";
	cout<<position.GetXPos()<<", "<<position.GetYPos()<<", "<<rtod(position.GetYaw())<< endl;
}