SourceFile002EV3.c

#pragma config(Sensor, S1,     eyes,           sensorEV3_IRSensor)
#pragma config(Sensor, S2,     redButton,      sensorNone)
#pragma config(Motor,  motorA,          motorEyes,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorRight,    tmotorEV3_Large, PIDControl, reversed, driveRight, encoder)
#pragma config(Motor,  motorC,          motorLeft,     tmotorEV3_Large, PIDControl, reversed, driveLeft, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// min, max et modulo(negatifs?) ?
// movement ligne 226 (operateur |=)
// fabs = valeur absolue

#include "config.c"
#include "position.c"
#include "movement.c"
#include "sonar.c"

task displayPos() {
	struct PosData pos;
	while (false) {
		getPosition(&pos);
		displayBigTextLine(1, "X : %6.2f", pos.x);
		displayBigTextLine(3, "Y : %6.2f", pos.y);
		displayBigTextLine(5, "O : %6.2f", pos.orientation);
		writeDebugStreamLine("X:%5.1f Y:%5.1f O:%5.1f", pos.x, pos.y, pos.orientation);
		wait1Msec(50);
	}
}


task emergencyStop() {
	while (SensorValue[redButton] == 0) { wait1Msec(20); }
	stopAllTasks();
}




task avoidObstaclesPerso()
{
	int i = 1;
	float target = -45;
	int robotSpeed = 40;
	int eyesSpeed = 60;


	while(true){
		setMotorSpeed(motorLeft, robotSpeed);
		setMotorSpeed(motorRight, robotSpeed);

		while(getIRDistance(eyes) > 50) {

			setMotorTarget(motorEyes, target, eyesSpeed);

			//Holds program flow until the motor on port A comes to a complete stop.
			waitUntilMotorStop(motorEyes);

			target += 10*i;
			if (abs(target) >= 45)
				i *= -1;

		}

		setMotorSpeed(motorRight, 0);
		setMotorSpeed(motorLeft, 0);


		while (getIRDistance(eyes)<60) {

		}

	}
}


task main(){
	initPosition(true);	//void initPosition(bool isGreenSide)
	startTask(avoidObstaclesPerso);
	startTask(displayPos);
	startTask(emergencyStop);

	moveTo_forward(100,0);
	wait10Msec(1000);
	moveTo_forward(0,0);


	while (getMovementState() != NOMVT) {wait1Msec(20);}
	stopAllTasks();


}