Toggles

This code was used to control the robot to hit the toggles from the bottom or the top.

#ifndef TOGGLES_H
#define TOGGLES_H

void togglesBottom();
void togglesTop();

#endif // TOGGLES_H

// Required FEH library
#include <FEHRPS.h>

// Required custom libraries
#include "constants.h"
#include "drive.h"
#include "toggles.h"

/* This function controls the robot to press the launch sequence toggles from the lower level.
 * This function starts after the start light has been detected on the start platform
 * and ends after backing up from the toggles.
 * Last modified: 4/2/2016 JKL
 */
void togglesBottom() {

    //driveUntilTime(225, 100, 800, true);
    driveUntilTime(180, 140, 500, true);
    driveUntilRPSyRange( RPSOffStart, 30, 90, 1, 3000);
    turnUntilTime(-70, 500);
    driveUntilBump(180, 60, 3);
    driveUntilBumpTimeout(265, 100, 4, 2000);

    driveUntilTime(90, 100, 10, false);
    driveUntilTime(0, 80, 450, false);

    turnUntilRPS(270, 20, 1000);

    // Hit the blue toggle only if needed
    if (RPS.BlueSwitchDirection() == 2) {
        driveUntilBumpTimeout(270, 80, 4, 2000);
        driveUntilTime(90, 80, 40, true);
    }
} // end togglesBottom function

/* This function controls the robot to press the launch sequence toggles from the upper level.
 * This function starts after the robot drives away from the drop zone
 * and ends after backing away from the blue toggle.
 * Last modified: 4/2/2016 JKL
 */
void togglesTop() {

    // If the robot is in the dead zone, continue to drive out of it
    if (RPS.X() < 0 )
        driveUntilTime(0, 60, 100, false);

    driveUntilRPSy( RPSRedTopY, 27, 0, 2000 );

    // check the RPS X position if the red toggle needs to be pressed from the top
    if (RPS.RedSwitchDirection() == 1)
        driveUntilRPSx( RPSRedTopX, 27, 0, 1000);

    turnUntilRPS(180, 20, 1000);

    if (RPS.RedSwitchDirection() == 1) {
        driveUntilBumpTimeout(0, 60, 1, 1000);
        driveUntilTime(180, 60, 400, true);
    }

    driveUntilTime(270, 60, 400, false);

    if (RPS.WhiteSwitchDirection() == 1) {
        // check the RPS X position if the red toggle was skipped from the top
        if (RPS.RedSwitchDirection() == 2)
            driveUntilRPSx( RPSWhiteTopX, 27, 90, 1000);
        driveUntilBumpTimeout(0, 60, 1, 1000);
        driveUntilTime(180, 60, 400, true);
    }

    driveUntilTime(270, 60, 400, false);

    if (RPS.BlueSwitchDirection() == 1) {
        driveUntilRPSx( RPSBlueTopX, 27, 90, 1000);
        driveUntilBumpTimeout(0, 80, 1, 1000);
        driveUntilTime(180, 60, 400, true);
    }
} // end togglesTop function

A fast version was written but but not used at final competition.

#ifndef TOGGLES_FAST_H
#define TOGGLES_FAST_H

void togglesBottom_fast();
void togglesTop_fast();

#endif // TOGGLES_FAST_H

// Required FEH library
#include <FEHRPS.h>

// Required custom libraries
#include "constants.h"
#include "drive.h"
#include "toggles_fast.h"

/* This function controls the robot to press the launch sequence toggles from the lower level.
 * This function starts after the start light has been detected on the start platform
 * and ends after backing up from the toggles.
 * Last modified: 4/2/2016 JKL
 */
void togglesBottom_fast() {

    driveUntilTime(225, 100, 800, true);
    driveUntilRPSyRange( RPSOffStart, 30, 90, 1, 3000);
    turnUntilTime(-70, 250);
    driveUntilBump(180, 100, 3);
    driveUntilBumpTimeout(265, 100, 4, 2000);

    driveUntilTime(90, 100, 10, false);
    driveUntilTime(0, 80, 450, false);

    turnUntilRPS(270, 20, 1000);

    if (RPS.BlueSwitchDirection() == 2) {
        driveUntilBumpTimeout(270, 80, 4, 2000);
        driveUntilTime(90, 80, 40, true);
    }
} // end togglesBottom function

/* This function controls the robot to press the launch sequence toggles from the upper level.
 * This function starts after the robot drives away from the drop zone
 * and ends after backing away from the blue toggle.
 * Last modified: 4/2/2016 JKL
 */
void togglesTop_fast() {

    if (RPS.X() < 0 )
        driveUntilTime(0, 60, 100, false);

    driveUntilRPSyRange( RPSRedTopY, 27, 0, 0.3, 2000 );

    // check the RPS X position if the red toggle needs to be pressed from the top
    if (RPS.RedSwitchDirection() == 1)
        driveUntilRPSxRange( RPSRedTopX, 27, 0, 0.4, 1000);

    turnUntilRPS(180, 20, 1000);

    if (RPS.RedSwitchDirection() == 1) {
        driveUntilBumpTimeout(0, 60, 1, 1000);
        driveUntilTime(180, 60, 400, true);
    }

    driveUntilTime(270, 60, 400, false);

    if (RPS.WhiteSwitchDirection() == 1) {
        // check the RPS X position if the red toggle was skipped from the top
        if (RPS.RedSwitchDirection() == 2)
            driveUntilRPSxRange( RPSWhiteTopX, 27, 90, 0.4, 1000);
        driveUntilBumpTimeout(0, 60, 1, 1000);
        driveUntilTime(180, 60, 400, true);
    }

    driveUntilTime(270, 60, 400, false);

    if (RPS.BlueSwitchDirection() == 1) {
        driveUntilRPSxRange( RPSBlueTopX, 27, 90, 0.4, 1000);
        driveUntilBumpTimeout(0, 80, 1, 1000);
        driveUntilTime(180, 60, 400, true);
    }
} // end togglesTop_fast function