예제 #1
0
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;
}
예제 #2
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);	
}
예제 #3
0
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;
}