VEX Robot

#pragma config(Sensor, dgtl1,   sonarFront,     sensorSONAR_cm)
#pragma config(Sensor, dgtl3,   sonarBack,      sensorSONAR_cm)
#pragma config(Sensor, dgtl5,   limitSwitchL,   sensorTouch)
#pragma config(Sensor, dgtl6,   limitSwitchR,   sensorTouch)
#pragma config(Motor,  port2,   rightMotor,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,   arm,            tmotorNormal, openLoop)
#pragma config(Motor,  port4,   leftMotor,      tmotorNormal, openLoop)
#pragma config(Motor,  port5,   claw,           tmotorNormal, openLoop)

/*==============================================================
||                    VEX COMPETITION STUFF                   ||
==============================================================*/

#pragma platform(VEX)                      // REQUIRED
#pragma competitionControl(Competition)    // Don't touch
#pragma autonomousDuration(20)             // 60 = challange, 20 otherwise
#pragma userControlDuration(120)           // Don't touch
#include "Vex_Competition_Includes.c"      // REQUIRED

/*==============================================================
||                       CODE STARTS HERE                     ||
==============================================================*/

// Variables setup
int automode = -1; // Default is Fail Safe
int wasMoving = 0; // 1 = Was moving last tick, 0 otherwise

// STOP (forces a stop of all motors)
void fstop(){
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
    motor[arm] = 0;
    motor[claw] = 0;
}

// BRAKE (params: milliseconds, power (will be inverted))
void brake(int msec, int motorValue){
    int tick = 0;
    do{
        motor[leftMotor] = -motorValue;
        motor[rightMotor] = -motorValue;
        float percent = tick/msec;
        motorValue = -(int) percent*motorValue;
        tick ++;
    }while(tick < msec);
}

// BEFORE MATCH START
void pre_auton(){
    int limit1 = SensorValue(limitSwitchR);
    int limit2 = SensorValue(limitSwitchL);
    if(limit1 == 1 && limit2 == 0){
        automode = 0;
    }else if(limit1 == 1 && limit2 == 1){
        automode = 1;
    }else if(limit1 == 0 && limit2 == 1){
        automode = 2;
    }else if(limit1 == 0 && limit2 == 0){
        automode = 3;
    }else{
        automode = -1;
    }
}

// Simple Driving Functions
void moveForward(int msec){
    int ticks = 0;
    do{
        motor[leftMotor] = 127;
        motor[rightMotor] = 127;
        ticks ++;
    }while(ticks < msec);
    brake(100, 127/4);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
}

void moveBackward(int msec){
    int ticks = 0;
    do{
        motor[leftMotor] = -127;
        motor[rightMotor] = -127;
        ticks ++;
    }while(ticks < msec);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
}

void turnLeft(int msec){
    int ticks = 0;
    do{
        motor[leftMotor] = 127;
        motor[rightMotor] = -127;
        ticks ++;
    }while(ticks < msec);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
}

void turnRight(int msec){
    int ticks = 0;
    do{
        motor[leftMotor] = -127;
        motor[rightMotor] = 127;
        ticks ++;
    }while(ticks < msec);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
}

// AUTONOMOUS FUNCTIONS
void runRedIso(){ // Isolation, RED
    moveForward(1500);
    turnRight(750);
    moveForward(3000);
    fstop();
}

void runRedGroup(){ // Group, RED
    moveForward(500);
    turnLeft(100);
    moveForward(250);
    fstop();
}

void runBlueIso(){ // Isolation, BLUE
    moveForward(1500);
    turnLeft(750);
    moveForward(3000);
    fstop();
}

void runBlueGroup(){ // Group, BLUE
    moveForward(500);
    turnRight(100);
    moveForward(250);
    fstop();
}

void runFailSafe(){ // Unknown automode, failsafe
    int stop = 0;
    do{
        moveForward(100);
        int distance = SensorValue(sonarFront);
        if(distance > 0 && distance < 5){
            stop = 1;
        }
    }while(stop == 0);
    turnLeft(50);  // 3 lines to create a glance-like action (left, right, center)
    turnRight(100);
    turnLeft(50);
    moveForward(500);
    moveBackwards(500);
    turnRight(300);
    fstop();
}


// AUTONOMOUS
task autonomous(){
    if(automode == 0){
        runRedIso();
    }else if(automode == 1){
        runRedGroup();
    }else if(automode == 2){
        runBlueIso();
    }else if(automode == 3){
        runBlueGroup();
    }else{
        runFailSafe();
    }
}

// USER CONTROL
task usercontrol(){
    fstop(); // Cleanup from auto
    while (true){
        // VARIABLES
        int LMotor = vexRT[Ch3];
        int RMotor = vexRT[Ch2];
        int ARMMotor = 0;
        int CLAWMotor = 0;

        // JOYSTICK CALIBRATION (115 is the absolute, 127 is hardcode)
        if(LMotor != 127 && LMotor > 115){
            LMotor = 127;
        }else if(LMotor != -127 && LMotor < -115){
            LMotor = -127;
        }
        if(RMotor != 127 && RMotor > 115){
            RMotor = 127;
        }else if(RMotor != -127 && RMotor < -115){
            RMotor = -127;
        }

        // LEFT MOTOR CONTROL
        if(LMotor == 127){
            LMotor = 127 - (127/4);
        }else if(LMotor == -127){
            LMotor = 127 - (127/4);
            LMotor = -LMotor; // Flip
        }

        // RIGHT MOTOR CONTROL
        if(RMotor == 127){
            RMotor = 127 - (127/4);
        }else if(RMotor == -127){
            RMotor = 127 - (127/4);
            RMotor = -RMotor; // Flip
        }

        // MOVEMENT DETECTION
        if(RMotor == 0 && LMotor == 0){
            if(wasMoving == 1){
                brake(100, (127/4));
            }
            wasMoving = 0;
        }else{
            wasMoving = 1;
        }

        // ARM CONTROL
        if(vexRT[Btn5U]){
            ARMMotor = 127/2; // Halved for elastics
        }else if(vexRT[Btn5D]){
            ARMMotor = -(127/2); // Halved for elastics
        }

        // CLAW CONTROL
        if(vexRT[Btn6U]){
            CLAWMotor = 127;
        }else if(vexRT[Btn6D]){
            CLAWMotor = -127;
        }

        // Assign motor values
        motor[leftMotor] = LMotor;
        motor[rightMotor] = RMotor;
        motor[claw] = CLAWMotor;
        motor[arm] = ARMMotor;
    }
}