Fuel Button

This code presses the correct fuel button depending on the fuel light.

#ifndef FUELBUTTON_H
#define FUELBUTTON_H

void fuelbutton();

#endif // FUELBUTTON_H

// Required FEH libraries
#include <FEHIO.h>
#include <FEHLCD.h>
#include <FEHUtility.h>

// Required custom libraries
#include "constants.h"
#include "drive.h"
#include "start.h"
#include "fuelbutton.h"

/* This function presses the correct fuel button based on the color of the fuel light.
 * This starts after the robot aligns with the fuel light right after the robot leaves the temporary access ramp
 * and ends after turning/heading check for the drive to the supplies drop zone.
 * Last modified: 4/7/2016 JKL
 */
void fuelbutton() {

    driveUntilCds(0, 60, 1, 1300);

    int redbutton = 0;

    // if a red light has been detected
    if (cdscell.Value() < 1)
        redbutton = 1;

    // if a red light has not been detected
    if (redbutton != 1) {
        driveUntilBump(90, 60, 2);
        driveUntilRPSy( RPSFuelLightY - RPSFuelLightYOffset, 25, 0, 1000);
        driveUntilTime(270, 40, 350, true);
        driveUntilRPSx( RPSFuelLightX - RPSFuelLightXOffset, 25, 90, 1000);
        turnUntilRPS(0, 20, 1000);
    }

    // if red light has been detected, press the red fuel button
    if (redbutton == 1) {
        LCD.WriteRC("Red!!!", 8, 5);
        driveUntilBump(90, 50, 2);
        driveUntilTime(10,50, 800, true);
        LCD.WriteRC("Sleep 5.1 sec", 5, 3);
        Sleep(5100);

        driveUntilTime(180, 60, 50, false);
    }
    else if (CDSBlueLow < cdscell.Value() && cdscell.Value() < CDSBlueHigh) {
       LCD.WriteRC("Blue!!!", 8, 5);
       driveUntilTime(0,50, 800,true);

       LCD.WriteRC("Sleep 5.1 sec", 5, 3);
       Sleep(5100);

       driveUntilTime(180, 60, 50, false);
    }
    else if (CDSRedLow < cdscell.Value() && cdscell.Value() < CDSRedHigh) {

        LCD.WriteRC("Red!!!", 8, 5);
        driveUntilBump(90, 50, 2);
        driveUntilTime(10,50, 800, true);

        LCD.WriteRC("Sleep 5.1 sec", 5, 3);
        Sleep(5100);

        driveUntilTime(180, 60, 50, false);
    }
    else {
        // There is a 50% probability that the light is blue.
        // Plus, the robot has failed to detect a red light twice and a blue light once, if it gets this far.
        LCD.WriteRC("NOPE! HOPE IT'S BLUE", 5, 3);
        driveUntilBump(90, 60, 2);
        driveUntilRPSy( RPSFuelLightY - RPSFuelLightYOffset, 25, 0, 1000);
        driveUntilTime(270, 40, 350, true);
        driveUntilRPSx( RPSFuelLightX - RPSFuelLightXOffset, 25, 90, 1000);
        turnUntilRPS(0, 20, 1000);

        driveUntilTime(0,50, 800, true);
        Sleep(5100);

        driveUntilTime(180, 60, 50, false);
    }

    //driveUntilTime(210, 100, 1000, false);

    // Cool code to do cool stuff
    // Values determined empirically a day before the final competition
    driveWhileRotate(205, 180, 80, -57, 1275);

    driveUntilRPSy( RPSLongRunY, 26, 225, 1000);

    //turnUntilTime(60, 1350);
    turnUntilRPS(180, 20, 4000);
} // end fuelbutton function