AutoDumpLeftRamp.c

#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,     INF,            sensorHiTechnicIRSeeker1200)
#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)
#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               !!*//

#include "JoystickDriver.c"

void driveToInf(){
        int a = 5;
        int ex = 0;
        nMotorEncoder[yellow] = 0;
        while(SensorValue[INF] != a && nMotorEncoder[yellow] < 6500){
                motor(red) = -80;
                motor(yellow) = 60;
                motor(green) = -80;
                motor(blue) = 60;
                if(nMotorEncoder[yellow] < 1000){
                        a = 5;
                        ex = 350;
                }else if(nMotorEncoder[yellow] < 2000){
                        a = 5;
                        ex = 350;
                }else{
                        a = 5;
                        ex = 50;
                }
        }

        motor(red) = -60;
        motor(yellow) = 60;
        motor(green) = -60;
        motor(blue) = 60;
        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;
}

task blinker(){
        while(true){
                SensorType[RGB] = sensorCOLORBLUE;
                wait10Msec(50);
                SensorType[RGB] = sensorCOLORNONE;
                wait10Msec(50);
        }
}

task main (){
       	bDisplayDiagnostics=false;

        SensorType[RGB] = sensorCOLORRED;

        servo[wrist] = 0;
        servo[autoflipper] = 240;

        eraseDisplay();
        nxtDisplayCenteredTextLine(3, "<-- Test Mode");
        nxtDisplayCenteredTextLine(2, "Tournament -->");

        int waitstart = 0;
        while(waitstart  == 0){
                if(nNxtButtonPressed == 2){
                				nxtDisplayCenteredTextLine(3, "On test mode");
                        waitstart = 1;
                        //test
                        wait1Msec(1000);
                }
                if(nNxtButtonPressed == 1){
                        waitstart = 1;

                        //tournament
												bDisplayDiagnostics = true;
                        SensorType[RGB] = sensorCOLORBLUE;

                        waitForStart();

                        StartTask(blinker);
                }
        }



        motor(red) = -50;
        motor(yellow) = -50;
        motor(green) = 50;
        motor(blue) = 50;
        wait1Msec(500);
        motor(red) = 0;
        motor(yellow) = 0;
        motor(blue) = 0;
        motor(green) = 0;
        servo[autoflipper] = 65;
        wait1Msec(2500);
        servo[autoflipper] = 240;
        PlayTone(300,15);
        motor(red) = -60;
        motor(yellow) = 50;
        motor(green) = -50;
        motor(blue) = 60;
        wait1Msec(1500);
        motor(red) = -50;
        motor(yellow) = -50;
        motor(green) = 60;
        motor(blue) = 60;
        wait1Msec(2500);
        motor(red) = 0;
        motor(yellow) = 0;
        motor(blue) = 0;
        motor(green) = 0;

}