finalProject.c

#pragma config(Sensor, in1,    rightLightSensor, sensorReflection)
#pragma config(Sensor, in2,    leftLightSensor, sensorReflection)
#pragma config(Sensor, dgtl4,  frontLeftBumper, sensorTouch)
#pragma config(Sensor, dgtl5,  frontRightBumper, sensorTouch)
#pragma config(Sensor, dgtl6,  backLeftBumper, sensorNone)
#pragma config(Sensor, dgtl7,  backRightBumper, sensorNone)
#pragma config(Sensor, dgtl11, rightLimitSensor, sensorTouch)
#pragma config(Sensor, dgtl12, leftLimitSensor, sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorVex269, openLoop)
#pragma config(Motor,  port3,           leftMotor,     tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void driveMotors(int leftMotorSpeed,int rightMotorSpeed);
void stopMotors(int stopTime);
void turnRight(int turnTime);
void turnLeft(int turnTime);
void followLeftWall();
void followRightWall();
void alignToLight();


/**********************************************/
/**** ALL TEST PROTOTYPES BELOW THIS POINT ****/
/**********************************************/
void testLightSensor();
void testLimitSwitch();
void testBumperSensors();

task main() {
//Should have a starting task.  After the starting task, some sort of switch statement
//1. Find the Light -- Set orientation to the light.
//2. Drive towards the light until contact with some sensor.
//3. Continue driving in direction of light.  If we turn we are not driving to the light we have to orient ourself to light again. But have to avoid a loop
  //driveMotors(127,-127);
//alignToLight();
//driveMotors(-100,100);
driveMotors(-127,127);
wait1Msec(200000);
//while (1) {
//followRightWall();
//}
}


void driveMotors(int leftMotorSpeed, int rightMotorSpeed) {
	motor[leftMotor] = leftMotorSpeed;
	motor[rightMotor] = rightMotorSpeed;
}

void turnLeft(int turnTime) {
	driveMotors(100,100);
	wait1Msec(turnTime);
	stopMotors(10);
}

void turnRight(int turnTime) {
	driveMotors(-100,-100);
	wait1Msec(turnTime);
	stopMotors(10);
}

void stopMotors(int stopTime) {
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(stopTime);
}

void followLeftWall() {
	/* Invoked when robot is driven and limit switches are activated.
	** If the limit switches are activated then the robot is traveling
	** in a direction that is somewhat close to being parrallel of the wall.
	** We should correct our direction (to avoid a crash) and oscillate on the wall.
	*/
	writeDebugStreamLine("In followLeftWall");
	while(SensorValue(frontRightBumper) == 0) {
		driveMotors(-127,127);
		wait1Msec(1000);
		writeDebugStreamLine("In the first while loop of followLeftWall");
		while ((SensorValue(leftLimitSensor) == 0) && (SensorValue(frontRightBumper) == 0)) {
			writeDebugStreamLine("LEFT LIMIT SENSOR IS 0");
			driveMotors(-127,127);
			wait1Msec(200);
		}
		while ((SensorValue(leftLimitSensor) == 1) && (SensorValue(frontRightBumper) == 0) ) {
			writeDebugStreamLine("LEFT LIMIT SENSOR IS 1");
			stopMotors(300);
			turnRight(1200);
			driveMotors(-127,127);
			wait1Msec(2000);
			stopMotors(300);
			turnLeft(1000);
		}
		driveMotors(-70,70);
		wait1Msec(100);
	}
}

void followRightWall() {
	/* Invoked when robot is driven and limit switches are activated.
	** If the limit switches are activated then the robot is traveling
	** in a direction that is somewhat close to being parrallel of the wall.
	** We should correct our direction (to avoid a crash) and oscillate on the wall.
	*/	
	while(SensorValue(frontRightBumper) == 0) {
		driveMotors(-127,127);
		wait1Msec(100);
		while ((SensorValue(rightLimitSensor) == 0) && (SensorValue(frontRightBumper) == 0)) {
			driveMotors(-70,70);
			wait1Msec(100);
		}
		while ((SensorValue(rightLimitSensor) == 1) && (SensorValue(frontRightBumper) == 0) ) {
			stopMotors(300);
			turnLeft(1200);
			driveMotors(-127,127);
			wait1Msec(2000);
			turnRight(1000);
		}
}
}

void alignToLight() {
	/* At the beginning of the maze, we need to find the light and orient our robot towards the light
	** We will rotate until the strongest light source (which should be the lantern) is detected.
	*/
	float leftLightValue=SensorValue(leftLightSensor);
	float maximumLight = leftLightValue;
	int counter = 0;
	for (int i=0; i<90; i++) {
		// Initial loop to locate the maximum value.  We do not want to locate the maximum value and then continue to turn. So we
		// loop as long as maximumLight is NOT less than the sensor value.
		writeDebugStreamLine("LIGHT VALUE: %d  MaximumLight:  %d", SensorValue(leftLightSensor),maximumLight);
		if (maximumLight > SensorValue(leftLightSensor)) {
				maximumLight=SensorValue(leftLightSensor);
				writeDebugStreamLine("Max Value: %d", maximumLight);
				counter =0;
	  }
		counter++;
		writeDebugStreamLine("Counter: %d",counter);
		turnLeft(100);
	}
	for (int i = counter; i>0; i--) {
		//Should rotate back to the right exactly as much as the robot had turned to the left.
		turnRight(100);
		writeDebugStreamLine("CountDOWN: %d", i);
	}
}


/****************************************/
/**** ALL TEST CODE BELOW THIS POINT ****/
/****************************************/

void testLightSensor() {
	wait1Msec(2000);
	//This will just write the sensors value to the debug screen
	for(int i=0; i<1000; i++) {
	float lval=SensorValue(leftLightSensor);
	writeDebugStreamLine("Light sensors:%d, %d\n",lval);
}
}

void testLimitSwitch() {
	wait1Msec(2000);
	// Loop while robot's sensor is not pressed
	while (SensorValue(leftLimitSensor) == 0 ) {
		motor[leftMotor] = 23;
		motor[rightMotor] = 23;
		writeDebugStreamLine("Right Limit Sensor: %d", SensorValue(rightLimitSensor));
	}
	stopMotors(2000);
}

void testBumperSensors() {
	wait1Msec(2000);
	// Loop while robot's sensor is not pressed
	while (SensorValue(frontRightBumper) == 0 ) {
		motor[leftMotor] = 100;
		motor[rightMotor] = 100;
	}
}