Tracking auto

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S3, HTServo,  none,     none,     none)
#pragma config(Sensor, S2,     HTSMUX,              sensorI2CCustom)
#pragma config(Sensor, S4,     RGB,            sensorCOLORFULL)
#pragma config(Motor,  mtr_S1_C1_1,     yellow,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     red,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     blue,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     green,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     arm,           tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     whisk,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S3_C1_1,    autoflipper,          tServoStandard)
#pragma config(Servo,  srvo_S3_C1_2,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S3_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//tracking auto.c

//the SMUX is plugged into PORT 2
//   the gyro is in port 4 of the SMUX
//   the ir is in port 3 of the SMUX

#include "JoystickDriver.c";
#include "statuslight.c";
#include "Sample Programs\NXT\3rd Party Sensor Drivers\drivers\XanderDriverSuite\rdpartyrobotcdr-3.3.1\drivers\hitechnic-sensormux.h"
//regular driver
//#include "Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-gyro.h"

#include "Sample Programs\NXT\3rd Party Sensor Drivers\drivers\XanderDriverSuite\rdpartyrobotcdr-3.3.1\drivers\hitechnic-gyro.h"    //for gyro sensor
//#include "Sample Programs\NXT\3rd Party Sensor Drivers\drivers\XanderDriverSuite\rdpartyrobotcdr-3.3.1\drivers\LEGOUS-driver.h"    //for sonar sensor
#include "Sample Programs\NXT\3rd Party Sensor Drivers\drivers\XanderDriverSuite\rdpartyrobotcdr-3.3.1\drivers\hitechnic-irseeker-v2.h"    //for IR seeker sensor
//#include "Sample Programs\NXT\3rd Party Sensor Drivers\drivers\XanderDriverSuite\rdpartyrobotcdr-3.3.1\drivers\LEGOTS-driver.h"    //for touch sensor

//!!SMUX is connected to port 2 on NXT; gyro is in port 1 on smux and ir is in port 3!!//
#define HTGYRO              msensor_S2_4
//#define sonarSensor             msensor_S4_2
#define INF          msensor_S2_3
//#define touchSensor             msensor_S4_4


int ex = 0;
int correct = 0;


void driveToInf(){
	float firstsight = 0;
	float secondsight = 0;
	nMotorEncoder[yellow] = 0;

	while(nMotorEncoder[yellow] < 6500){
		if (correct == 0){
			motor(red) = -30;
			motor(yellow) = 30;
			motor(green) = -30;
			motor(blue) = 30;
			if (SensorValue[INF] == 5){
				firstsight = nMotorEncoder[yellow];
				if(firstsight !=0 && SensorValue[INF] != 5){
					secondsight = nMotorEncoder[yellow];
				}
			}
		}

		while(nMotorEncoder[yellow] < ((secondsight + firstsight)/2.0)){
			if(nMotorEncoder[yellow] < 1000){
				ex = 350;
				}else if(nMotorEncoder[yellow] < 2000){
				ex = 350;
				}else{
				ex = 50;
			}
		}
	}

	motor(red) = 30;
	motor(yellow) = -30;
	motor(green) = 30;
	motor(blue) = -30;
	wait1Msec(ex);
	motor(blue) = 0;
	motor(green) = 0;
	motor(yellow) = 0;
	motor(red) = 0;
	servo[autoflipper] = 65;
	wait1Msec(2500);
	servo[autoflipper] = 240;
	PlayTone(300,15);
}



void returnto(){
	time1[T1] = 0;
	while(nMotorEncoder[yellow] > 700 && time1[T1] < 6500){
		motor(red) = 60;
		motor(yellow) = -60;
		motor(green) = 60;
		motor(blue) = -60;
	}
	motor[red] = 0;
	motor[yellow] = 0;
	motor[green] = 0;
	motor[blue] = 0;
}


//this should correct if the robot drifts
task correction(){
	float rotSpeed = 0;
	eraseDisplay();
	disableDiagnosticsDisplay();
	float heading = 0;

	// Calibrating


	// Reset the timer.
	time1[T1] = 0;

	while (true)
	{
		// timeout 20ms
		while (time1[T1] < 20)
			wait1Msec(1);

		// Read rotation speed
		rotSpeed = HTGYROreadRot(HTGYRO);

		/* Calculate the new heading by adding the amount of degrees
		we've turned in the last 20ms
		If our current rate of rotation is 100 degrees/second,
		then we will have turned 100 * (20/1000) = 2 degrees since
		the last time we measured.*/

		//varience compinsator for integration with multiple tasks

		heading += rotSpeed * (time1[T1] / 1000.0);
		nxtDisplayCenteredTextLine(2, "Timer: %2.3f", (time1[T1]));
		// Reset timer
		time1[T1]=0;

		// Display degrees from start position

		nxtDisplayCenteredTextLine(3, "Heading: %2.3f", heading);


		//reset if over 360 degrees
		if (heading >= 360){
			heading = (heading - 360);
		}
		//reset if under 360
		if (heading <= 0){
			heading = (heading + 360);
		}
		nxtDisplayCenteredTextLine(4, "Herp");
		if (heading >= 10 /*|| heading <= -10*/){
			nxtDisplayCenteredTextLine(4, "Derp");
			correct = 1;
			//stop all the motors
			motor[yellow] = 0;
			motor[red] = 0;
			motor[blue] = 0;
			motor[green] = 0;
			disableDiagnosticsDisplay();
			nxtDisplayTextLine(1,"Changing Course");
			if (heading >= 10){
				while(heading >= -2){
					PlayTone(622,38);
					//rotate robot until -2 degrees
					motor[yellow] = 20;
					motor[red] = 20;
					motor[blue] = 20;
					motor[green] = 20;
					PlayTone(784,15);
				}
			}
			if (heading <= -10){
				while (heading <= 2){
					motor[yellow] = -20;
					motor[red] = -20;
					motor[blue] = -20;
					motor[green] = -20;
					PlayTone(784,15);
				}
			}
			bDisplayDiagnostics = true;
			correct = 0;

		}
	}
}
task main (){
	disableDiagnosticsDisplay();
	eraseDisplay();
	color1 = red;
	int button = 0;
	int waittime = 0;/*
	nxtDisplayCenteredTextLine(3, "<-- Wait 15 sec");
	nxtDisplayCenteredTextLine(2, "No wait -->");
	while(waittime  == 0){
		if(nNxtButtonPressed == 2){
			waittime = 2;
			button = 2;
			//wait mode
			wait1Msec(10);
		}
		if(nNxtButtonPressed == 1){
			waittime = 1;
			button = 1;
			color1 = blue;
			//no wait
		}
	}
	while(nNxtButtonPressed == button){
		wait10Msec(10);
	}

	int waitstart = 0;
	nxtDisplayCenteredTextLine(3, "<-- Test Mode");
	nxtDisplayCenteredTextLine(2, "Tournament -->");
	while(waitstart  == 0){
		if(nNxtButtonPressed == 2){
			nxtDisplayCenteredTextLine(3, "On test mode");
			waitstart = 1;
			if (waittime == 2){
				wait10Msec(1500);
			}
			//test
		}
		if(nNxtButtonPressed == 1){
			waitstart = 1;
			//tournament
			nxtDisplayCenteredTextLine(2, "Waiting");
			nxtDisplayCenteredTextLine(3, "for Start");
			SensorType[RGB] = sensorCOLORBLUE;

			waitForStart();
			if (waittime == 2){
				wait10Msec(1500);
			}
		}
	}*/

	eraseDisplay();
	bDisplayDiagnostics = true;
	HTGYROstartCal(HTGYRO);
	StartTask(blinker);
	StartTask(correction);
	color1 = "blue";
	color2 = "none";
	while(true){
	}
}