void comeBack(){
if(totalY == 0) totalY = 1;
if(totalX == 0) totalX = 1;
float rot = atan2(totalX, totalY) * 180 / PI;
if(rot < 0){
rot = 359+rot;
}
nxtDisplayTextLine(2, "Rot: %f", rot);
while(getCompassValue() < rot){
motor[motorB] = 100;
motor[motorC] = -100;
}
nMotorEncoder[motorB] = 0;
float dist = sqrt(totalX*totalX + totalY*totalY);
nxtDisplayTextLine(3, "Dist: %f", dist);
while(nMotorEncoder[motorB] < dist){
moveF();
}
totalX = 0;
totalY = 0;
}
void checkForMove(ubyte data){
switch(data){
case turnRight:
nxtDisplayTextLine(7, "RIGHT");
moveRight();
break;
case turnLeft:
nxtDisplayTextLine(7, "LEFT");
moveLeft();
break;
case moveForward:
nxtDisplayTextLine(7, "FORWARD");
moveF();
break;
case moveBackward:
nxtDisplayTextLine(7, "BACKWARD");
moveBack();
break;
case goBack:
comeBack();
break;
case stopMoving:
nxtDisplayTextLine(7, "!!!!!!STOP!!!!!!!!");
stopAll();
break;
default:
stopAll();
break;
}
}
void move(Vec3d dp) { moveR(dp[0]); moveU(dp[1]); moveF(dp[2]); }
/// Set linear velocity
void move(double dr, double du, double df) { moveR(dr); moveU(du); moveF(df); }
