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 readCompassSensor(){
//~ int compass_reading = 0;//getCompassValue();
//~ sensor_sensorReadings.positionSensor.compass_direction = 0; //deg_to_rad((double)compass_reading);
//~ return;
check_analog_sensors();
int compass_reading = getCompassValue();
sensor_sensorReadings.positionSensor.compass_direction = deg_to_rad((double)compass_reading);
}
void stopAll(){
if(move){
float rot = getCompassValue();
float x = sinDegrees(rot) * encoder;
float y = cosDegrees(rot) * encoder;
nxtDisplayTextLine(0, "%d %d %d", rot, x, y);
nxtDisplayTextLine(1, "%d", encoder);
totalX+=x;
totalY+=y;
nMotorEncoder[motorB] = 0;
move = false;
}
stopBack();
stopF();
stopLeft();
stopRight();
motor[clawm] = 0;
}
