#include <ArduinoRobot.h>
#include <Wire.h>
#include <SPI.h>

int turnSpeed = 150;
int forwardSpeed = 200;

void setup() {

	Robot.begin();
	Robot.motorsWrite(forwardSpeed, forwardSpeed);
}

void loop()
{
	// Si détection mur face -> Tourne
	if (Robot.analogRead(TK2) < 15){
		Robot.motorsStop();
		// Définition du degrés cible
		int compassInit = Robot.compassRead();
		int compass = compassInit -90;
		if (compass <= 0){
			do {
				turnLeft(turnSpeed);
				delay(100);
			} while (Robot.compassRead() < compassInit);
			compass += 360;
		}
		do {
			turnLeft(turnSpeed);
			delay(100);
		} while (Robot.compassRead() > compass);
		Robot.motorsStop();
	}
	// Sinon on avance
	else {
		Robot.motorsWrite(forwardSpeed, forwardSpeed);
	}
}

void turnLeft(int speed)
{
	Robot.motorsWrite(-speed, speed);
}

void turnRight(int speed)
{
	Robot.motorsWrite(speed, -speed);
}