robot_competition_code.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    IR_collector_left, sensorReflection)
#pragma config(Sensor, in2,    IR_collector_right, sensorReflection)
#pragma config(Sensor, dgtl1,  button,         sensorTouch)
#pragma config(Sensor, dgtl6,  rangefinder,    sensorSONAR_cm)
#pragma config(Sensor, dgtl10, CloseLED,       sensorDigitalOut)
#pragma config(Sensor, dgtl11, ReadyLED,       sensorDigitalOut)
#pragma config(Sensor, dgtl12, FarLED,         sensorDigitalOut)
#pragma config(Sensor, I2C_1,  motor_right,    sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  motor_crane,    sensorNone)
#pragma config(Motor,  port3,           motor_right,   tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port2,           motor_left,    tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port10,          motor_crane,   tmotorVex393_HBridge, openLoop, encoderPort, I2C_1)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//flag to store button input
bool button_pushed;
// the max difference of IR to be used to find beacon
int max_diff = 0;
// speed for turning and going forward. Can go higher but sacrifices accuracy
int turn_speed = 60;
int forward_speed = 127;
// acceptable difference between 2 IR sensors
int acceptable_diff = 400;
// target distance is 6cm
const int target_dist = 6;
// time for robot to wait to get stationary
int wait_time = 100;

/*
*  Used to flag button inputs
*       - this avoids errors caused by program recognizing input, taking action, and
*         reading input again before button is released
*/
void monitorInput()
{
  if(SensorValue(button) && !button_pushed)
  {
    button_pushed = true;
  }
}

// helper function to turn all motors off
void motors_off(){
	motor[motor_right] = 0;
	motor[motor_left] = 0;
	motor[motor_crane] = 0;
	// wait for motors to stop spinning
}

// helper functions to get vale of LEDs
int monitorLight_right()
{
	static int minLevelIR = 4096;	// Minimum light level seen by IR sensor
	static int maxLevelIR = 0;			// Maximum light level seen by IR sensor
	static int diffLevelIR = 0;		// Delta between maximum and minimum seen in last 0.1 seconds
	int lightLevel;
	lightLevel = SensorValue[IR_collector_right];
	// Check if 100 msecs have elapsed.
	if ( time1[T1] > 100 )  {
		// 100 msecs have elapsed.  Compute delta of light level.
		diffLevelIR = maxLevelIR - minLevelIR;

		// Reset calculation for next 100 msecs.
		maxLevelIR = 0;
		minLevelIR = 4096;
		clearTimer(T1);

		} else {

		// Check for new minimum/maximum light levels.
		if ( lightLevel < minLevelIR ) {
			minLevelIR = lightLevel;
			} else if ( lightLevel > maxLevelIR) {
			maxLevelIR = lightLevel;
		}
	}
	return(diffLevelIR);
}
int monitorLight_left()
{
	static int minLevelIR = 4096;	// Minimum light level seen by IR sensor
	static int maxLevelIR = 0;			// Maximum light level seen by IR sensor
	static int diffLevelIR = 0;		// Delta between maximum and minimum seen in last 0.1 seconds
	int lightLevel;
	lightLevel = SensorValue[IR_collector_left];
	// Check if 100 msecs have elapsed.
	if ( time1[T2] > 100 )  {
		// 100 msecs have elapsed.  Compute delta of light level.
		diffLevelIR = maxLevelIR - minLevelIR;

		// Reset calculation for next 100 msecs.
		maxLevelIR = 0;
		minLevelIR = 4096;
		clearTimer(T2);

		} else {
		// Check for new minimum/maximum light levels.
		if ( lightLevel < minLevelIR ) {
			minLevelIR = lightLevel;
			} else if ( lightLevel > maxLevelIR) {
			maxLevelIR = lightLevel;
		}
	}
	return(diffLevelIR);
}

// returns maximum difference of IR readings so that they are both equal
int find_max_diff(){
	// rotate 10 seconds (over 360 degrees) and find the lowest IR value (max_diff)
	clearTimer(T3);
	while(time1[T3] < 3500){
		// turn motors on
		motor[motor_right] = turn_speed;
		motor[motor_left] = turn_speed * -1;
		// replace with bigger value if one is found and is roughly equal for the 2 IR sensors
		if (abs(monitorLight_left()-monitorLight_right()) < acceptable_diff  && monitorLight_right() > max_diff){
			max_diff = 	monitorLight_right();
		}
	}
	motors_off();
	// wait a  so robot is stationary
	wait1Msec(wait_time);
	return max_diff;
}

void go_to_target(){
	// maximum IR diff (when IR is strongest)
	int max_diff;
	// Define tag for scan_state
	enum T_move_state {
		DETECT_BUTTON = 0,
		SCAN,
		GO_FORWARD,
		MAKE_CONNECTION,
		CLEAR_CABLE,
		SIGNAL_COMPLETION
	};
	// Declare variable to hold state, intialize to DETECT_BUTTON state.
	T_move_state move_state = DETECT_BUTTON;
	// FLOW: DETECT_BUTTON -> SCAN -> GO_FORWARD -> MAKE_CONNECTION -> CLEAR_CABLE -> SIGNAL_COMPLETION
	while(true){
		// This function updates the button_pushed flag.
		monitorInput();
		// Switch the states.
		switch(move_state) {
			// Code for DETECT_BUTTON state:
			case DETECT_BUTTON:
				if (button_pushed) {
						writeDebugStream("Done Detect Button 1 \n ");
					// find value of IR beacon that puts robot in straight line (by having both IRs reading roughly same value)
					max_diff =  1500;
					move_state = SCAN;
					// Clear flag to indicate button processed.
					button_pushed = false;
				}
				break;

			// code for SCAN state:
			case SCAN:
				writeDebugStream("max diff is %d \n ", max_diff);
				//run motors until IR_reading exceeds threshold (found beacon)
				while( monitorLight_right() < max_diff && monitorLight_left() < max_diff){
					motor[motor_right] = turn_speed * -1;
					motor[motor_left] = turn_speed;
				}
				writeDebugStream("Done scan \n");
				// when scan is complete, stop motors and  GO_FORWARD
				motors_off();
				move_state = GO_FORWARD;
				break;

			// code for GO_FORWARD state:
			case GO_FORWARD:
				//run motors until we are close to target
				while(SensorValue[rangefinder] > target_dist){
					int dist = SensorValue[rangefinder];
					// hardcoded distance to maximize speed. To maximize accuracy use find_max_diff() function which scans for maximum IR value and sets it to target
					int diff = 400;
					if (dist < 25){
						forward_speed = 35;
						diff = 800;
					}
					// if right motor is reading a lot more than left one, steer robot left
					if ( monitorLight_right() >   monitorLight_left() + diff){
						motor[motor_left] = forward_speed;
						motor[motor_right] = 0 ;
						writeDebugStream("going more left \n");
					}
					// if left motor is reading a lot more than right one, steer robot right
					else if (monitorLight_left() >  monitorLight_right()  + diff){
						motor[motor_right] = forward_speed;
						motor[motor_left] = 0;
						writeDebugStream("going more right \n");
					}
					// go straight
					 else{
						motor[motor_left] = forward_speed;
						motor[motor_right] = forward_speed;
						writeDebugStream("Going forward \n ");
					}
				}
				// Turn motors off.
				motors_off();
				move_state = MAKE_CONNECTION;
				writeDebugStream("Reached target \n");
				break;

			// code for MAKE_CONNECTION state:
			case MAKE_CONNECTION:
				// move crane down for 0.7s at speed of 20
				clearTimer(T4);
				while(time1[T4] < 1 * 100){
					motor[motor_crane] = 70;
				}
				// turn motors off
				motors_off();
				//wait 0.7 seconds to give the magnet time to connect
				wait1Msec(700);
				//lift arm for 0.4 seconds to clear the beacon at a speed of 60
				clearTimer(T4);
				while(time1[T4] < 1 * 400){
					motor[motor_crane] = -60;
				}
				//turn motors off
					motors_off();

				writeDebugStream("Made Connection \n");
				move_state = CLEAR_CABLE;
				break;

			// code for CLEAR_CABLE state
			case CLEAR_CABLE:
				clearTimer(T3);
				// back up to clear cable for 2.5 seconds
				while(time1[T3] < 1000){
					motor[motor_right] = forward_speed * -1;
						motor[motor_left] = forward_speed * -1 ;
				}
				clearTimer(T3);
				motors_off();
				// wait so robot is stationary
				wait1Msec(wait_time);
				writeDebugStream("cleard cable \n");
				move_state = SIGNAL_COMPLETION;
				break;

			// code for SIGNAL_COMPLETION state
			case SIGNAL_COMPLETION:
			writeDebugStream("Done conneciton. hitting button \n ");
				// hit button
				clearTimer(T4);
				while(time1[T4] < 1 * 700){
					motor[motor_crane] = -80;
				}
				motors_off();
				wait1Msec(wait_time);

				// release button
				clearTimer(T4);
				while(time1[T4] < 1 * 500){
					motor[motor_crane] = 20;
				}
				motors_off();
				// go back to DETECT_BUTTON so the another trial is ready
				move_state = DETECT_BUTTON;
		}	// end switch
	} // end while
} // end function

task main(){
	button_pushed = false;
	motors_off();
	go_to_target();
}