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