Start

This code was used to initialize the RPS, calibrate various RPS values on the course, initialize the servo for the arm, and wait for the start light.

#ifndef START_H
#define START_H

void start();

// extern these values so that other .cpp files can use the RPS offset values
extern float RPSSuppliesYOffset;
extern float RPSFuelLightXOffset;
extern float RPSFuelLightYOffset;
extern float RPSTopRampXOffset;

#endif // START_H

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

// Required custom libraries
#include "constants.h"
#include "worldstate.h"
#include "supplyarm.h"
#include "start.h"

// Initialize the RPS offsets as global so that it can be passed out of this .cpp file
float RPSSuppliesYOffset = 0;
float RPSFuelLightXOffset = 0;
float RPSFuelLightYOffset = 0;
float RPSTopRampXOffset = 0;

/* This function is run at the beginning of a course run to go through the launch sequence of the robot.
 * A log is opened, RPS is initialized, RPS offsets are calibrated, CdS cell is tested,
 * servo arm is initialized, and the robot waits for the start light.
 * Last Modified: 4/2/2016 JKL
 */
void start() {

    RPS.InitializeTouchMenu();

    initializeLog();

    // Calibrate the RPS Y value for the supplies
    while( 0 != ( microswitch1.Value() + microswitch2.Value() ) ) {
        worldState(false, 0,0,0,0,0,0);
        LCD.WriteRC("RPSSuppliesY", 5, 3);
        if ( (0 == ( microswitch1.Value() + microswitch2.Value() )) && (RPS.X() > 0) ) {
            worldState(true, 0, 0, 0, 0, 0, 0);
            SD.Printf("RPSSupplies\t");
            RPSSuppliesYOffset = RPSSuppliesY - RPS.Y();
            LCD.WriteRC("Logged", 5, 3);
        }
    }

    LCD.WriteRC(RPSSuppliesYOffset, 5, 3);
    Sleep(1000);

    // Calibrate the RPS X and Y values for the fuel light
    while( 0 != ( microswitch1.Value() + microswitch2.Value() ) ) {
        worldState(false, 0,0,0,0,0,0);
        LCD.WriteRC("RPSFuelLightX and Y", 5, 3);
        if ( (0 == ( microswitch1.Value() + microswitch2.Value() )) && (RPS.X() > 0) ) {
            worldState(true, 0, 0, 0, 0, 0, 0);
            SD.Printf("RPSFuelLight\t");
            RPSFuelLightXOffset = RPSFuelLightX - RPS.X();
            RPSFuelLightYOffset = RPSFuelLightY - RPS.Y();
            LCD.WriteRC("Logged", 5, 3);
        }
    }

    LCD.WriteRC(RPSFuelLightXOffset, 5, 3);
    LCD.WriteRC(RPSFuelLightYOffset, 6, 3);
    Sleep(1000);

    // Calibrate the RPS X value for the main ramp
    while( 0 != ( microswitch1.Value() + microswitch2.Value() ) ) {
        worldState(false, 0,0,0,0,0,0);
        LCD.WriteRC("RPSTopRampX", 5, 3);
        if ( (0 == ( microswitch1.Value() + microswitch2.Value() )) && (RPS.X() > 0) ) {
            worldState(true, 0, 0, 0, 0, 0, 0);
            SD.Printf("RPSTopRamp\t");
            RPSTopRampXOffset = RPSTopRampX - RPS.X();
            LCD.WriteRC("Logged", 5, 3);
        }
    }

    LCD.WriteRC(RPSTopRampXOffset, 5, 3);
    Sleep(1000);


    // Wait for the left button to be pressed
    while( !button.LeftPressed()) {
        worldState(false,0,0,0,0,0,0);
        LCD.WriteRC("CdS Cell test", 5, 3);
        LCD.WriteRC("Press left button...", 6, 3);
        LCD.WriteRC(cdscell.Value(), 7, 10);
    }
    while(!button.LeftReleased());

    initializeArm();

    worldState(true,0,0,0,0,0,0);

    LCD.WriteRC("Press left button...", 5, 3);
    while(!button.LeftPressed());
    while(!button.LeftReleased());

    LCD.WriteRC("Waiting for light...", 6, 3);
    float startTime = TimeNow();
    while(cdscell.Value() > 1 && TimeNow() - startTime < 30) {
        LCD.WriteRC( cdscell.Value(), 7, 10);
    }

    LCD.WriteRC("Beginning run", 8, 3);
} // end start function