auto002

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S3, HTServo,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#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, encoder)
#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               !!*//

// auto002.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"

#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/hitechnic-colour-v2.h"   //for color 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

#define HTGYRO              msensor_S2_4
#define INF          msensor_S2_3
float heading = 0;
float heading2 = 4;
int back = 0;
int truncate = 0;
int isthree = 0;

////////////////////////////////////////////Allstop Below//////////////////////////////////////////////////////////////////
void allStop(){
	motor[yellow] = 0;
	motor[green] = 0;
	motor[blue] = 0;
	motor[red] = 0;
}

////////////////////////////////////////////Allstop Above///////////////////////////////////////////////////////////////
////////////////////////////////////////////Ramp Below//////////////////////////////////////////////////////////////////
void ramp(){
	heading = 0;
	nMotorEncoder[yellow] = 0;
	nMotorEncoder[green] = 0;
	while (nMotorEncoder[yellow] < 2000 || nMotorEncoder[green] > -2000 ){//do that straffing thingy to avoid wood. Numbers might be wrong
		motor[red] = -50;
		motor[yellow] = 50;
		motor[green] = -50;
		motor[blue] = 50;
	}
	//old thing
	//motor[blue] = 50;
	//motor[red] = -50;
	//motor[yellow] = 50;
	//motor[green] = -50;
	//wait10Msec(50);

	allStop();
	float localHeading = 0;
	heading2 = 0;

	//drive in front of ramp
	motor[blue] = 50;
	wait10Msec(10);
	motor[red] = -50;
	wait10Msec(200);
	allStop();

	//turn to face ramp with GYRO
	motor[red] = -10;
	motor[yellow] = -10;
	motor[green] = -10;
	motor[blue] = -10;

	localHeading=heading2;
	eraseDisplay();
	while (localHeading > -45){
		PlayTone(622,38);
		nxtDisplayCenteredTextLine(6, "Heading: %2.3f", localHeading);
		localHeading=heading2;
	}
	allStop();

	//old turning thing
	//motor[red] = -10;
	//motor[yellow] = -10;
	//motor[green] = -10;
	//motor[blue] = -10;
	//wait10Msec(130);
	//allStop();

	//drive up on that ramp, man
	nMotorEncoder[yellow] = 0;
	nMotorEncoder[green] = 0;
	time1[T4] = 0;
	while (nMotorEncoder[yellow] > -4000 && nMotorEncoder[green] < 4000 && time1[T4] < 2000 ){//go to apex of ramp
		motor[red] = -100;
		motor[yellow] = -100;
		motor[green] = 100;
		motor[blue] = 100;
	}
	allStop();
	eraseDisplay();
	nxtDisplayCenteredTextLine(3, "All Finished");
	while(true){//pause while on the ramp until the end of autonomous.
		wait10Msec(1);
	}
}

////////////////////////////////////////////Ramp Above////////////////////////////////////////////////////////////////////
////////////////////////////////////////////Recall Below//////////////////////////////////////////////////////////////////
void recall(){
	float localHeading = 0;
	heading2 = 0;
	//drive back to start position
	while (nMotorEncoder[yellow] >= 900 || nMotorEncoder[green] <= -900){
		motor[green] = 30;
		motor[yellow] = -30;
	}
	allStop();
	motor[yellow] = -50;
	motor[red] = -50;
	motor[blue] = 50;
	motor[green] = 50;
	nMotorEncoder[yellow] = 0;
	nMotorEncoder[green] = 0;
	while (nMotorEncoder[yellow] > -750 || nMotorEncoder[green] < 750 ){
	}
	allStop();
	motor[blue] = 50;
	motor[red] = -50;
	wait10Msec(300);
	allStop();
	wait10Msec(1);
	motor[yellow] = -30;
	motor[red] = -30;
	motor[blue] = -30;
	motor[green] = -30;
	heading2 = heading;
	localHeading=heading2;

	while (localHeading > -32){
		PlayTone(622,38);
		nxtDisplayCenteredTextLine(6, "Heading: %2.3f", localHeading);
		localHeading=heading2;
	}
	allStop();
	nMotorEncoder[yellow] = 0;
	nMotorEncoder[green] = 0;
	motor[red] = 100;
	motor[green] = -100;
	motor[blue] = -100;
	motor[yellow] = 100;
	time1[T4] = 0;
	while (nMotorEncoder[green] > -4000 && nMotorEncoder[green] < 4000 && time1[T4] < 2000 ){//go to apex of ramp
	}
	allStop();
	while(true){
	}

}

////////////////////////////////////////////Recall Above///////////////////////////////////////////////////////
////////////////////////////////////////////Gyro Below//////////////////////////////////////////////////////////

task gyro(){
	float rotSpeed = 0;
	float heading = 0;
	time1[T1] = 0;

	while (true){
		while (time1[T1] < 20)
			wait1Msec(1);
		rotSpeed = HTGYROreadRot(HTGYRO);
		heading += rotSpeed * (time1[T1] / 1000.0);
		nxtDisplayCenteredTextLine(2, "Timer: %2.3f", (time1[T1]));
		time1[T1]=0;

		nxtDisplayCenteredTextLine(3, "Heading: %2.3f", heading);/*
		if (heading >= 360)
		heading = (heading - 360);
		if (heading <= -1)
		heading = (heading + 360);*/
		heading2=heading;
	}
}

////////////////////////////////////////////Gyro Above//////////////////////////////////////////////////////////////////
////////////////////////////////////////////Dump Below//////////////////////////////////////////////////////////////////
void dump(){
	int isback;
	isback = back;
	if(isthree == 1){
		while (nMotorEncoder[yellow] > 4500 || nMotorEncoder[green] < -4500 ){
			motor[green] = 30;
			motor[yellow] = -30;
		}
	}
	StartTask(gyro);
	heading = 0;
	heading2 = 0;
	float localHeading = 0;
	allStop();
	PlayTone(622,38);

	//rotate to face buckets
	motor[red] = 20;
	motor[yellow] = 20;
	motor[green] = 20;
	motor[blue] = 20;


	localHeading=heading2;
	while (localHeading < 40){
		nxtDisplayCenteredTextLine(6, "Heading: %2.3f", localHeading);
		localHeading=heading2;
	}
	allStop();

	//stop and dump
	servo[autoflipper] = 65;
	wait10Msec(100);
	servo[autoflipper] = 240;
	//turn back and stop
	motor[red] = -10;
	motor[yellow] = -10;
	motor[green] = -10;
	motor[blue] = -10;
	while (localHeading >= 0){
		localHeading=heading2;
	}
	allStop();
	//if come back was selected

	eraseDisplay();

	if (isback == 1){
		recall();
		nxtDisplayCenteredTextLine(1,"Recalled");
	}

	//keep driving to end and a bit more
	while (nMotorEncoder[yellow] <= 8500 && nMotorEncoder[green] >= -8500 && time1[T3] < 30000){
		motor[green] = -30;
		motor[yellow] = 30;
		nxtDisplayCenteredTextLine(1,"Time: %f", time1[T3]);
	}


	allStop();
	ramp();

	//float ramp_up = heading;

}//END DUMP SECTION

////////////////////////////////////////////Dump Above///////////////////////////////////////////////////////////////////
////////////////////////////////////////////Drive Below//////////////////////////////////////////////////////////////////
void drive(){
	time1[T3] = 0;
	nMotorEncoder[yellow] = 0;
	nMotorEncoder[green] = 0;
	//herro frind
	//===first bucket===
	while (nMotorEncoder[yellow] < 970 || nMotorEncoder[green] > -970 ){//first 1470
		motor[green] = -30;
		motor[yellow] = 30;
	}
	PlayTone(622,38);
	if (HTIRS2readDCDir(INF) == 3){
		dump();
	}
	//===secoend bucket===
	while (nMotorEncoder[yellow] < 2000 || nMotorEncoder[green] > -2000 ){//secoend 2500
		motor[green] = -30;
		motor[yellow] = 30;
	}
	PlayTone(622,38); wait10Msec(2);
	if (HTIRS2readDCDir(INF) == 3){
		dump();
	}
	//===third bucket===
	while (nMotorEncoder[yellow] < 4500 || nMotorEncoder[green] > -4500 ){//third 4800
		motor[green] = -30;
		motor[yellow] = 30;
	}
	PlayTone(622,38);
	if (HTIRS2readDCDir(INF) == 3){
		isthree = 1;
		dump();
	}
	//dump anyway if truncate was selected
	if (truncate == 1){
		isthree = 1;
		dump();
	}
	//===fourth bucket===
	while (nMotorEncoder[yellow] < 5350 || nMotorEncoder[green] > -5350 ){//fourth 5850
		motor[green] = -30;
		motor[yellow] = 30;
	}
	PlayTone(622,38);
	if (HTIRS2readDCDir(INF) == 3){
		dump();
		}else{
		//dump anyway if it hasen't found the beacon
		dump();
	}
}

////////////////////////////////////////////Drive Above/////////////////////////////////////////////////////////////////
////////////////////////////////////////////Main Below//////////////////////////////////////////////////////////////////
task main (){
	int isthree = 0;

	StartTask(blinker);
	color1 = "red";
	color2 = "none";
	HTGYROstartCal(HTGYRO);
	servo[wrist] = 0;
	servo[autoflipper] = 240;
	disableDiagnosticsDisplay();
	eraseDisplay();
	color1 = "red";
	int button = 0;
	int waittime = 0;
	//add opposite side?

	//wait 15 sec

	nxtDisplayCenteredTextLine(3, "<-- Wait 15 sec");
	nxtDisplayCenteredTextLine(2, "No wait -->");
	while(waittime  == 0){
		if(nNxtButtonPressed == 2){
			waittime = 2;
			button = 2;
			//wait mode
		}
		if(nNxtButtonPressed == 1){
			waittime = 1;
			button = 1;
			//no wait
		}
	}
	while(nNxtButtonPressed == button){
		wait10Msec(10);
	}

	//Truncate run after third bin, should probably be combined with the come back part
	button = 0;
	truncate	= 2;
	eraseDisplay();
	nxtDisplayCenteredTextLine(3, "<-- Truncate");
	nxtDisplayCenteredTextLine(2, "Full Run-->");
	while(truncate == 2){
		if(nNxtButtonPressed == 2){
			truncate = 1;
			button = 2;
			//truncate
		}
		if(nNxtButtonPressed == 1){
			truncate = 0;
			button = 1;
			//proceedes with a full run.
		}
	}
	//stops the NXT from skipping ahead
	while(nNxtButtonPressed == button){
		wait10Msec(10);
	}



	//Come back after dump option
	button = 0;
	back = 2;
	eraseDisplay();
	nxtDisplayCenteredTextLine(3, "<-- Come back");
	nxtDisplayCenteredTextLine(2, "Go on -->");
	while(back == 2){
		if(nNxtButtonPressed == 2){
			back = 1;
			button = 2;
			//comes back
			wait1Msec(10);
		}
		if(nNxtButtonPressed == 1){
			back = 0;
			button = 1;
			//goes ahead
		}
	}
	while(nNxtButtonPressed == button){
		wait10Msec(10);
	}

	//test mode or tournament option
	int waitstart = 0;
	nxtDisplayCenteredTextLine(3, "<-- Test Mode");
	nxtDisplayCenteredTextLine(2, "Tournament -->");
	while(waitstart  == 0){
		if(nNxtButtonPressed == 2){
			nxtDisplayCenteredTextLine(3, "On test mode");
			waitstart = 1;
			wait10Msec(100);
			if (waittime == 2){
				wait10Msec(1500);
			}
			//test mode
		}
		if(nNxtButtonPressed == 1){
			waitstart = 1;
			//tournament mode
			nxtDisplayCenteredTextLine(2, "Waiting");
			nxtDisplayCenteredTextLine(3, "for Start");
			color1 = "blue";

			waitForStart();
			if (waittime == 2){
				wait10Msec(1500);
			}
		}
	}
	color1 = "blue";
	PlayTone(322,33);
	drive();
	eraseDisplay();
	while(true){
		nxtDisplayCenteredTextLine(4, "encoder: %2.3f", (nMotorEncoder[yellow]));
		nxtDisplayCenteredTextLine(3, "IR: %i", HTIRS2readDCDir(INF));
		if (HTSMUXreadPowerStatus(HTSMUX))
			nxtDisplayTextLine(7, "SMUX Batt: bad");
		else
			nxtDisplayTextLine(7, "SMUX Batt: good");
	}
}