mazebot

#pragma config(Sensor, in4,    centerLineFollower, sensorLineFollower)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, in3,   gyroSensor,  sensorVirtualCompass)
#pragma config(Sensor, dgtl11,  touchSensor,    sensorTouch)
#pragma config(Sensor, dgtl8,  sonarSensor,    sensorSONAR_cm)
#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)


// This absolute number function is required because the built
// in abs function becomes a float and automatically typecasts
// to the higher possible int when cast to an int
int abso(int num){
	if (num < 0){
		return num * -1;
	}

	return num;
}

// sync up both the right and left motors to be running
// at the same speed, this used for going straight
void motorSync(int speed){
	motor[rightMotor] = speed;
	motor[leftMotor] = speed;
}

// This returns the current value of the gyroscope normalized
// to be within 360 degrees and return a positive angle
// regardless of the negative value
int getGyro(){
	int deg = SensorValue[gyroSensor] % 3600;
	if (deg < 0){
		return 3600 + deg;
	}
	return deg;
}

// This turns the the degree
void turnToDegree(int degs){

	// The gyroscope handles degrees in form of tenths of a degree
	// for that reason the desired degree is multiplied to match
	degs *= 10;

	// invert the motor speed values if negative
	int motorMult = 1;

	// actually check if degree is negative
	int calced = abso(getGyro() - degs);
	if ( calced > 1800){
		motorMult *= -1;
	}

	if (getGyro() > degs){
		motorMult *= -1;
	}

	// set motor speeds
	motor[rightMotor] = -15 * motorMult;
	motor[leftMotor]  =  15 * motorMult;

	// Just keep turning motors until angle is met
	// We add 0.2 degrees of leeway
	while(abso(getGyro() - (degs)) > 2){}

	// Ok now return from subroutine
}

// Move forward until object or line is detected
void moveForward(){
	motorSync(120);
	while(SensorValue[centerLineFollower] < 1500 && SensorValue[touchSensor] == 0){}
	motorSync(-70);
	sleep(500);
}

// Move forward just until object
void moveFinish(){
	motorSync(120);
	while(SensorValue[touchSensor] == 0){}
	motorSync(-70);
	sleep(500);
}

task main()
{

	SensorValue[gyroSensor] = 0;
	wait1Msec(2000);

	// Run predefined movement functions to escape
	int turns[] = {0, 270, 180, 270, 0, 90, 0, 270};

	// Lets get the length of the move count
	int turnCount = (int) sizeof(turns)/ sizeof(int);

	// Lets turn to each angle then go forward until we hit something
	for (int i = 0; i < turnCount; i++){
			turnToDegree(turns[i]);
			if (i == turnCount-1){
				moveFinish();
			} else {
				moveForward();
			}
	}

	//Its over now so turn off the motors
	motor[rightMotor] = 0;
	motor[leftMotor]  = 0;
}