@Test
public void testRotateRightMotor(){
float r=1;
int x=10;
int y=10;
simPilot = new SimPilot(new SimPosition(0,x,y), 2*r, 2);
simPilot.rotateRightMotor(90); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 45 Grad
assertEquals(315, simPilot.getHeading());
assertTrue(simPilot.getPosX() > x);
assertTrue(simPilot.getPosY() < y);
simPilot = new SimPilot(new SimPosition(0,x,y), 2*r, 2);
simPilot.rotateRightMotor(180); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 90 Grad
assertEquals(270, simPilot.getHeading());
assertEquals(x+r*5, simPilot.getPosX(), 0.1);
assertEquals(y-r*5, simPilot.getPosY(), 0.1);
simPilot = new SimPilot(new SimPosition(0,x,y), (2*r), 2);
simPilot.rotateRightMotor(270); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 135 Grad
assertEquals(225, simPilot.getHeading());
assertTrue(simPilot.getPosX() > x);
assertTrue(simPilot.getPosY() < y-r*5);
simPilot = new SimPilot(new SimPosition(0,x,y),(2*r), 2);
simPilot.rotateRightMotor(360); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 180 Grad
assertEquals(180, simPilot.getHeading());
assertEquals(x, simPilot.getPosX(), 0.1);
assertEquals(y-2*r*5, simPilot.getPosY(), 0.1);
simPilot = new SimPilot(new SimPosition(0,x,y), (2*r), 2);
simPilot.rotateRightMotor(450); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 225 Grad
assertEquals(135, simPilot.getHeading());
assertTrue(simPilot.getPosX() < x);
assertTrue(simPilot.getPosY() < y-r*5);
simPilot = new SimPilot(new SimPosition(0,x,y), (int)(2*r), 2);
simPilot.rotateRightMotor(540); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 270 Grad
assertEquals(90, simPilot.getHeading());
assertEquals(x-r*5, simPilot.getPosX(), 0.1 );
assertEquals(y-r*5, simPilot.getPosY(), 0.1 );
simPilot = new SimPilot(new SimPosition(0,x,y),(2*r), 2);
simPilot.rotateRightMotor(630); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 315 Grad
assertEquals(45, simPilot.getHeading());
assertTrue(simPilot.getPosX() < x);
assertTrue(simPilot.getPosY() < y);
simPilot = new SimPilot(new SimPosition(0,x,y), (int)(2*r), 2);
simPilot.rotateRightMotor(720); // ergibt bei den vorliegenden Daten einen Drehwinkel des Robots von 360 Grad
assertEquals(0, simPilot.getHeading());
assertEquals(x, simPilot.getPosX(), 0.1 );
assertEquals(y, simPilot.getPosY(), 0.1 );
}