flagAuton.c

#pragma config(Hubs,  S3, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S4, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     IR,             sensorI2CCustom)
#pragma config(Sensor, S2,     compass,        sensorI2CCustom)
#pragma config(Motor,  mtr_S3_C1_1,     arm,           tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S3_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     x1,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_2,     x2,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_1,     y1,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     y2,            tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S3_C2_1,    spinner,              tServoStandard)
#pragma config(Servo,  srvo_S3_C2_2,    scoop,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c";
#include "MotorToServo.c";
#include "PolarDrive.c";
#include "3rd Party Sensor Drivers/drivers/hitechnic-compass.h";
#include "3rd Party Sensor Drivers/drivers/hitechnic-irseeker-v2.h";
#include "setZeros.h";

void initializeRobot(){
	servo[scoop] = 0;
	servo[spinner] = 0;
}

task main(){

	int offset = 45;
	int waitTime = 0;

	initializeRobot();

	nVolume = 4;
	//allow the user to select a delay before te robot begins the autonomous routine
	while(nNxtButtonPressed != 3){
		if(nNxtButtonPressed == 1){
			while(nNxtButtonPressed == 1){}
			waitTime+=1;
		}
		if(nNxtButtonPressed == 2){
			while(nNxtButtonPressed == 2){}
			waitTime-=1;
		}
		if(waitTime < 0){waitTime = 0;}
		nxtDisplayCenteredBigTextLine(4, "Time: %1.d", waitTime);
		//play a sound until the user selects a time
		if(!bSoundActive){PlaySound(soundBlip);}
	}
	waitTime = waitTime*1000;
	nVolume = 1;

	waitForStart();

	//wait for the pre-designated time, just in case our alliance partner
	//needs time
	wait1Msec(waitTime);
	//move arm to ready position
	while(!moveTo(arm, 10000, 30, 70, 20)){}

	polarDrive(x1,x2,y1,y2,100,offset+45,0);
	wait1Msec(500);

	polarDrive(x1,x2,y1,y2,100,offset+315,0);
	wait1Msec(3000);

	setZero(x2);

	polarDrive(x1,x2,y1,y2,100,offset+90,0);
	//move along the crates while watching for the beacon
	//when beacon encountered, slow down and dump block
	while(nMotorEncoder[x2] < 18000){

		if(HTIRS2readDCDir(IR) == 5){
			//stopPolarMot(x1,x2,y1,y2);
			servo[scoop] = 250;
			polarDrive(x1,x2,y1,y2,45,offset+90,0);
			wait1Msec(500);
			servo[scoop] = 0;
			polarDrive(x1,x2,y1,y2,100,offset+90,0);
		}
	}

	stopPolarMot(x1,x2,y1,y2);
	polarDrive(x1,x2,y1,y2,100,offset+0,0);
	wait1Msec(2500);
	stopPolarMot(x1,x2,y1,y2);
	polarDrive(x1,x2,y1,y2,0,0, 100);
	wait1Msec(1500);
	stopPolarMot(x1,x2,y1,y2);
	polarDrive(x1,x2,y1,y2,100,offset+180,0);
	wait1Msec(3000);
	stopPolarMot(x1,x2,y1,y2);

}