Performance Test Codes

This page contains all of the codes used in the performance tests in addition to analyses of what could be taken or altered from/within the code for the purposes of the competition.

Performance Test 1 – Drive to the upper level

  • The first performance test required for the robot to start with the starting light and completely drive to the upper level via either ramp. The stretch bonus was correctly reading and displaying the fuel delivery light color (3 extra points). Our robot completed a perfect performance test including the stretch bonus with the following code for a score of 23/20.
  • /* This is the main function that controls the robot for Performance Test 1.
 * The robot successfully completed the test with the bonus for 23/20 points.
 * Last modified: 2/26/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include the required FEH libraries
#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHMotor.h>

// Include required custom library
#include "drive.h"

// Define a variable to toggle breakpoints. 1 turns breakpoints on, 0 turns them off.
#define PAUSE 0

// Globally declare the ButtonBoard and cdscell
ButtonBoard button(FEHIO::Bank3);
AnalogInputPin cdscell(FEHIO::P0_0);

int main(void)
{

    // Constantly display CdS cell values to ensure that the CdS cell is working
    LCD.WriteLine("CDS Cell test");
    LCD.WriteLine("Press left button to continue");
    while( !button.LeftPressed()) {
        LCD.WriteRC(cdscell.Value(), 4, 4 );
    }

    // Last button to press to initiate start of the run
    LCD.WriteLine("Waiting for left button...");
    while(!button.LeftPressed());
    while(!button.LeftReleased());
    LCD.WriteLine("Left button pressed");

    // Wait until cdscell value is less than 1, indicating red start light
    LCD.WriteLine("Waiting for start light...");
    while(cdscell.Value() > 1);
    LCD.WriteLine("Beginning run");

    driveUntilTime(180, 60, 1100);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    turnUntilTime(50, 1300);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(95, 60, 2);
    driveUntilTime(90, 40, 100);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(15, 60, -2);
    driveUntilTime(0, 60, 500);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(90, 60, 2);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(15, 60, 1);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(275, 60, -1);
    driveUntilTime(270, 60, 1200);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilTime(0, 60, 1390);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(90, 60, 2);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilTime(270, 40, 450);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    // Constantly display the cdscell value reading in a blue or red font
    while(true) {
        LCD.WriteRC(cdscell.Value(), 5, 5);
        if (1.0 < cdscell.Value() && cdscell.Value() < 1.9)
            LCD.WriteRC("Blue!!!", 6, 5);
        if (0.0 < cdscell.Value() && cdscell.Value() < 0.8)
            LCD.WriteRC("Red!!!", 6, 5);
    }

    return 0;
}


  • #ifndef DRIVE_H
#define DRIVE_H

/* This is part of a custom header that contains the drive functions for the robot.
 * driveUntilBump, driveUntilTime, and turnUntilTime are the only functions that work.
 * Last modified: 2/26/2016 JKL (Comments added on 3/18/2016 JKL)
 */
void driveUntilTime(int heading, float power, int time);
void driveUntilBump(int heading, float power, int bumpSide);
void driveUntilCds(int heading, float power, float cdsvalue);
void driveUntilEncoder();
void driveUntilLine(int heading, float power);

void turnUntilTime(float power, int time);
void turnUntilEncoder();
void turnUntilRPS();


#endif // DRIVE_H

  • /* This is part of a custom header that contains the drive functions for the robot.
 * driveUntilBump, driveUntilTime, and turnUntilTime are the only functions that work.
 * Last modified: 2/26/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include required FEH libraries
#include <FEHLCD.h>
#include <FEHBuzzer.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHIO.h>

// Include required custom libraries
#include <math.h>
#include "drive.h"
#define PI 3.14159

// Globally declare VEX 393 motors
FEHMotor motor1(FEHMotor::Motor0, 7.2);
FEHMotor motor2(FEHMotor::Motor1, 7.2);
FEHMotor motor3(FEHMotor::Motor2, 7.2);
FEHMotor motor4(FEHMotor::Motor3, 7.2);

// Globally declare sides 1 and 2 microswitches
DigitalInputPin microswitch1(FEHIO::P1_0);
DigitalInputPin microswitch2(FEHIO::P1_2);
DigitalInputPin microswitch3(FEHIO::P1_4);
DigitalInputPin microswitch4(FEHIO::P1_6);

// Globally declare the CdS cell
AnalogInputPin Cdscell(FEHIO::P0_0);

// Initialize the motor ratios so that they can be passed between functions
float motor1ratio;
float motor2ratio;
float motor3ratio;
float motor4ratio;

/* This function calculates the motor ratios for a given heading. The calculation is derived from vector quantities.
 * The calculated ratios are assigned to each individual motor ratio. The ratios range from -1 to 1.
 * If the heading is a cardinal direction (N/S/E/W), the ratio is 0.707.
 * Last modified: 2/26/2016 JKL
 */
void ratios(int heading) {

    LCD.WriteLine("Calculating motor ratios");
    float x, y;
    x = cos( (45-heading) * PI / 180.0 );
    y = sin( (45-heading) * PI / 180.0 );
    LCD.WriteLine(x);
    LCD.WriteLine(y);
    motor1ratio = x;
    motor2ratio = -y;
    motor3ratio = -x;
    motor4ratio = y;
}

/* This function drives the robot with a given heading at a given power for a given time in milleseconds.
 * Last modified: 2/26/2016 JKL
 */
void driveUntilTime(int heading, float power, int time) {

    LCD.Clear();
    LCD.WriteRC("Heading:", 7, 0);
    LCD.WriteRC(heading, 7, 9);
    LCD.WriteRC("Power:", 8, 0);
    LCD.WriteRC(power, 8, 9);
    LCD.WriteRC("Time:", 9, 0);
    LCD.WriteRC(time, 9, 9);
    ratios(heading);

    LCD.WriteRC(power * motor1ratio, 11, 0);
    LCD.WriteRC(power * motor2ratio, 11, 9);
    LCD.WriteRC(power * motor4ratio, 12, 0);
    LCD.WriteRC(power * motor3ratio, 12, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    Sleep(time);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function utilizes the two sides of the robot that have the microswitches and drives
 * until the microswitches are either both true or both false.
 * If bumpSide is negative, then the robot will ride against the wall until that side is no longer touching the wall.
 * Last modified: 2/26/2016 JKL
 */
void driveUntilBump(int heading, float power, int bumpSide) {

    LCD.Clear();
    LCD.WriteRC("Heading:", 7, 0);
    LCD.WriteRC(heading, 7, 9);
    LCD.WriteRC("Power:", 8, 0);
    LCD.WriteRC(power, 8, 9);
    LCD.WriteRC("Bump", 9, 0);
    ratios(heading);

    LCD.WriteRC(power * motor1ratio, 11, 0);
    LCD.WriteRC(power * motor2ratio, 11, 9);
    LCD.WriteRC(power * motor4ratio, 12, 0);
    LCD.WriteRC(power * motor3ratio, 12, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    if (bumpSide == -1)
        while( 2 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == -2)
        while( 2 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == 1)
        while( 0 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == 2)
        while( 0 != (microswitch3.Value() + microswitch4.Value()) );

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function was intended to stop at the fuel delivery light when it reached the cdsvalue.
 * Through experimentation, the robot always stopped when it reached this value no matter the color, rendering this function useless.
 * Last modified: 2/26/2016
 */
void driveUntilCds(int heading, float power, float cdsvalue) {

    LCD.Clear();
    LCD.WriteRC("Heading:", 7, 0);
    LCD.WriteRC(heading, 7, 9);
    LCD.WriteRC("Power:", 8, 0);
    LCD.WriteRC(power, 8, 9);
    LCD.WriteRC("Cds", 9, 0);
    ratios(heading);

    LCD.WriteRC(power * motor1ratio, 11, 0);
    LCD.WriteRC(power * motor2ratio, 11, 9);
    LCD.WriteRC(power * motor4ratio, 12, 0);
    LCD.WriteRC(power * motor3ratio, 12, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    while( Cdscell.Value() > cdsvalue);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* These are placeholder functions that may be implemented in the future.
 */
void driveUntilEncoder(int heading, float power, int clicks);
void driveUntilLine(int heading, float power);

/* This function turns the robot at a given power for a given time.
 * Positive power for clockwise and negative power for counter-clockwise.
 * Last modified: 2/26/2016
 */
void turnUntilTime(float power, int time) {
    LCD.Clear();
    LCD.WriteRC(power, 11, 0);
    LCD.WriteRC(power, 11, 9);
    LCD.WriteRC(power, 12, 0);
    LCD.WriteRC(power, 12, 9);

    motor1.SetPercent(power);
    motor2.SetPercent(power);
    motor3.SetPercent(power);
    motor4.SetPercent(power);

    Sleep(time);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* These are placeholder functions that may be implemented in the future.
 */
void turnUntilEncoder();
void turnUntilRPS();

Performance Test 2 – Switches

  • The second performance test required for the robot to start on the start light, flip one toggle switch forward, move onto the upper level, and flip a different toggle backwards in any order. The stretch bonus was hitting a fuel button (extra 3 points). Our robot successfully completed the performance test with the stretch bonus two days early to achieve a bonus of 2 points. The overall score of this performance test was 25/20 points.
  • /* This is the main function that controls the robot for Performance Test 2.
 * The robot successfully completed the test early with the bonus for 25/20 points.
 * Last modified: 3/2/2016 JKL (Comments added on 3/18/2016 JKL)
 */

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

// Include required custom libraries
#include "drive.h"
#include "constants.h"

// Define a variable to toggle breakpoints. 1 turns breakpoints on, 0 turns them off.
#define PAUSE 0

int main(void)
{

    LCD.WriteLine("CDS Cell test");
    LCD.WriteLine("Press left button to continue");
    while( !button.LeftPressed()) {
        LCD.WriteRC(cdscell.Value(), 4, 4 );
    }

    LCD.WriteLine("Waiting for left button...");
    while(!button.LeftPressed());
    while(!button.LeftReleased());
    LCD.WriteLine("Left button pressed");

    LCD.WriteLine("Waiting for start light...");
    while(cdscell.Value() > 1) {
        LCD.WriteRC( cdscell.Value(), 5,5);
    }
    LCD.WriteLine("Beginning run");

    driveUntilTime(225, 80, 1400);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    turnUntilTime(-60, 420);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(180, 60, 3);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    //should be driveUntilLine
    driveUntilBump(265, 60, 4);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }


    driveUntilBump(180, 40, 3);


    driveUntilTime(45, 60, 850);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }


    driveUntilBump(0, 60, 1);
    driveUntilBump(275, 60, 4);
    driveUntilTime(180, 90, 3250);
    driveUntilBump(90, 60, 2);
    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }
    driveUntilTime(270, 80, 600);
    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }
    driveUntilTime(180, 60, 500);
    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }
    driveUntilBump(90, 60, 2);
    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }
    driveUntilTime(315, 60, 3000);
    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilBump(340, 60, 1);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilTime(180, 40, 530);

    if (PAUSE) {
        LCD.WriteLine("Waiting for left button...");
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteLine("Left button pressed");
    }

    driveUntilTime(270, 40, 1200);

    return 0;
}

  • #ifndef DRIVE_H
#define DRIVE_H

/* This is part of a custom header that contains the drive functions for the robot.
 * driveUntilBump, driveUntilTime, and turnUntilTime are the only functions that work.
 * Last modified: 3/2/2016 JKL (Comments added on 3/18/2016 JKL)
 */
void driveUntilTime(int heading, float power, int time);
void driveUntilBump(int heading, float power, int bumpSide);
void driveUntilCds(int heading, float power, float cdsvalue);
void driveUntilRPSx(int x, float power);
void driveUntilEncoder();
void driveUntilLine(int heading, float power);

void turnUntilTime(float power, int time);
void turnUntilEncoder();
void turnUntilRPS(int desiredHeading, int power);


#endif // DRIVE_H

  • /* This is part of a custom header that contains the drive functions for the robot.
 * driveUntilBump, driveUntilTime, and turnUntilTime are the only functions that work.
 * Last modified: 3/2/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include required FEH libraries
#include <FEHLCD.h>
#include <FEHBuzzer.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHIO.h>
#include <FEHRPS.h>

// Include required custom libraries
#include <math.h>
#include "constants.h"
#include "drive.h"

// Initialize the motor ratios so that they can be passed between functions
float motor1ratio;
float motor2ratio;
float motor3ratio;
float motor4ratio;

/* This function calculates the motor ratios for a given heading. The calculation is derived from vector quantities.
 * The calculated ratios are assigned to each individual motor ratio. The ratios range from -1 to 1.
 * If the heading is a cardinal direction (N/S/E/W), the ratio is 0.707.
 * Last modified: 2/26/2016 JKL
 */
void ratios(int heading) {

    LCD.WriteLine("Calculating motor ratios");
    float x, y;
    x = cos( (45-heading) * PI / 180.0 );
    y = sin( (45-heading) * PI / 180.0 );
    LCD.WriteLine(x);
    LCD.WriteLine(y);
    motor1ratio = x;
    motor2ratio = -y;
    motor3ratio = -x;
    motor4ratio = y;
}

/* This function drives the robot with a given heading at a given power for a given time in milleseconds.
 * Last modified: 2/26/2016 JKL
 */
void driveUntilTime(int heading, float power, int time) {

    LCD.Clear();
    LCD.WriteRC("Heading:", 7, 0);
    LCD.WriteRC(heading, 7, 9);
    LCD.WriteRC("Power:", 8, 0);
    LCD.WriteRC(power, 8, 9);
    LCD.WriteRC("Time:", 9, 0);
    LCD.WriteRC(time, 9, 9);
    ratios(heading);

    LCD.WriteRC(power * motor1ratio, 11, 0);
    LCD.WriteRC(power * motor2ratio, 11, 9);
    LCD.WriteRC(power * motor4ratio, 12, 0);
    LCD.WriteRC(power * motor3ratio, 12, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    Sleep(time);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function utilizes the four sides of the robot that have microswitches and drives
 * until the microswitches are either both true or both false.
 * If bumpSide is negative, then the robot will ride against the wall until that side is no longer touching the wall.
 * Last modified: 3/2/2016 JKL
 */
void driveUntilBump(int heading, float power, int bumpSide) {

    LCD.Clear();
    LCD.WriteRC("Heading:", 7, 0);
    LCD.WriteRC(heading, 7, 9);
    LCD.WriteRC("Power:", 8, 0);
    LCD.WriteRC(power, 8, 9);
    LCD.WriteRC("Bump", 9, 0);
    ratios(heading);

    LCD.WriteRC(power * motor1ratio, 11, 0);
    LCD.WriteRC(power * motor2ratio, 11, 9);
    LCD.WriteRC(power * motor4ratio, 12, 0);
    LCD.WriteRC(power * motor3ratio, 12, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    if (bumpSide == -1)
        while( 2 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == -2)
        while( 2 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == -3)
        while( 2 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == -4)
        while( 2 != (microswitch7.Value() + microswitch8.Value()) );

    if (bumpSide == 1)
        while( 0 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == 2)
        while( 0 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == 3)
        while( 0 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == 4)
        while( 0 != (microswitch7.Value() + microswitch8.Value()) );

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function was intended to stop at the fuel delivery light when it reached the cdsvalue.
 * Through experimentation, the robot always stopped when it reached this value no matter the color, rendering this function useless.
 * Last modified: 2/26/2016
 */
void driveUntilCds(int heading, float power, float cdsvalue) {

    LCD.Clear();
    LCD.WriteRC("Heading:", 7, 0);
    LCD.WriteRC(heading, 7, 9);
    LCD.WriteRC("Power:", 8, 0);
    LCD.WriteRC(power, 8, 9);
    LCD.WriteRC("Cds", 9, 0);
    ratios(heading);

    LCD.WriteRC(power * motor1ratio, 11, 0);
    LCD.WriteRC(power * motor2ratio, 11, 9);
    LCD.WriteRC(power * motor4ratio, 12, 0);
    LCD.WriteRC(power * motor3ratio, 12, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    while( cdscell.Value() > cdsvalue);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* These are placeholder functions that may be implemented in the future.
 */
void driveUntilRPSx(int x, float power) {
    while(RPS.X() < x - 0.2 || RPS.X() > x + 0.2) {


    }

}

void driveUntilEncoder(int heading, float power, int clicks);
void driveUntilLine(int heading, float power);

/* This function turns the robot at a given power for a given time.
 * Positive power for clockwise and negative power for counter-clockwise.
 * Last modified: 2/26/2016
 */
void turnUntilTime(float power, int time) {
    LCD.Clear();
    LCD.WriteRC(power, 11, 0);
    LCD.WriteRC(power, 11, 9);
    LCD.WriteRC(power, 12, 0);
    LCD.WriteRC(power, 12, 9);

    motor1.SetPercent(power);
    motor2.SetPercent(power);
    motor3.SetPercent(power);
    motor4.SetPercent(power);

    Sleep(time);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This is a placeholder function that may be implemented in the future.
 */
void turnUntilEncoder();

/* This function attempts to turn to a given heading using RPS.
 * This algorithm does not work for our robot.
 * Last modified: 3/2/2016 JKL
 */
void turnUntilRPS(int desiredHeading, int power) {
    while(RPS.Heading() <  desiredHeading + 360*(desiredHeading==0) - 1 || RPS.Heading() > desiredHeading - 360*(desiredHeading==0) + 1 ) {
        LCD.WriteRC(RPS.Heading() ,5,5);
        if (RPS.Heading() < desiredHeading + 360*(desiredHeading==0) - 1 ) {
            LCD.Clear();
            LCD.WriteRC(power, 11, 0);
            LCD.WriteRC(power, 11, 9);
            LCD.WriteRC(power, 12, 0);
            LCD.WriteRC(power, 12, 9);

            motor1.SetPercent(power);
            motor2.SetPercent(power);
            motor3.SetPercent(power);
            motor4.SetPercent(power);

            Sleep(15);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

        }
        if (RPS.Heading() > desiredHeading) {
            LCD.Clear();
            LCD.WriteRC(power, 11, 0);
            LCD.WriteRC(power, 11, 9);
            LCD.WriteRC(power, 12, 0);
            LCD.WriteRC(power, 12, 9);

            motor1.SetPercent(-power);
            motor2.SetPercent(-power);
            motor3.SetPercent(-power);
            motor4.SetPercent(-power);

            Sleep(10);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);
        }
    }

}
  • #ifndef CONSTANTS_H
#define CONSTANTS_H
#include <FEHMotor.h>

// Globally define PI so that it can be used
#define PI 3.14159

// Globally declare the definitions for the motors
extern FEHMotor motor1;
extern FEHMotor motor2;
extern FEHMotor motor3;
extern FEHMotor motor4;

// Globally declare the microswitches for the 4 sides
extern DigitalInputPin microswitch1;
extern DigitalInputPin microswitch2;
extern DigitalInputPin microswitch3;
extern DigitalInputPin microswitch4;
extern DigitalInputPin microswitch5;
extern DigitalInputPin microswitch6;
extern DigitalInputPin microswitch7;
extern DigitalInputPin microswitch8;

// Globally declare the cdscell and ButtonBoard
extern AnalogInputPin cdscell;

extern ButtonBoard button;

#endif // CONSTANTS_H


  • #include <FEHIO.h>
#include <FEHMotor.h>
#include "constants.h"

// Declare the motors
FEHMotor motor1(FEHMotor::Motor0, 7.2);
FEHMotor motor2(FEHMotor::Motor1, 7.2);
FEHMotor motor3(FEHMotor::Motor2, 7.2);
FEHMotor motor4(FEHMotor::Motor3, 7.2);

// Declare the microswitches
DigitalInputPin microswitch1(FEHIO::P1_0);
DigitalInputPin microswitch2(FEHIO::P1_2);
DigitalInputPin microswitch3(FEHIO::P1_4);
DigitalInputPin microswitch4(FEHIO::P1_6);
DigitalInputPin microswitch5(FEHIO::P2_0);
DigitalInputPin microswitch6(FEHIO::P2_2);
DigitalInputPin microswitch7(FEHIO::P2_4);
DigitalInputPin microswitch8(FEHIO::P2_6);

// Declare other buttons and sensors
AnalogInputPin cdscell(FEHIO::P0_0);

ButtonBoard button(FEHIO::Bank3);

Performance Test 3 – Supplies

  • The third performance test required for the robot to pickup the supplies on the lower level and transport it to the upper level and drop it off in the drop zone. The stretch bonus for this test was making the robot drive back down to the lower level. Our robot successfully completed the performance test with the stretch bonus two days prior to the due date for a score of 25/20.
  • /* This is the main function that controls the robot for Performance Test 3.
 * The robot successfully completed the test early with the bonus for 25/20 points.
 * Last modified: 3/9/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include required FEH libraries
#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHServo.h>

// Include required custom libraries
#include "start.h"
#include "constants.h"
#include "drive.h"
#include "worldstate.h"

// Define a variable to toggle breakpoints. 1 turns breakpoints on, 0 turns them off.
#define PAUSE 0

/* This function forces the robot to pause and wait for a buttom press to continue.
 * Last modified: 3/9/2016 JKL
 */
void breakpoint() {
    if (PAUSE) {
        LCD.WriteRC("Press left button...", 5, 3);
        while(!button.LeftPressed());
        while(!button.LeftReleased());
        LCD.WriteRC("Left button pressed", 6, 3);
    }
}

int main(void)
{
    start();
    driveUntilTime(180, 80, 1100, true);
    breakpoint();
    turnUntilTime(60, 750);
    breakpoint();
    driveUntilTime(45, 80, 500, false);
    driveUntilBump(90, 60, 2);
    breakpoint();
    driveUntilRPSy(11.2, 20, 1, 8000);
    breakpoint();
    driveUntilBump(90, 30, 2);
    driveUntilTime(90, 20, 100, true);
    breakpoint();

    // Lower arm to pickup supplies
    arm_servo.SetDegree(16);
    Sleep(1000);
    LCD.WriteRC("Degree 16", 6, 3);
    breakpoint();

    // Raise arm incrementally
    for (int degree = 18; degree <= 175; degree+= 2 ) {
        arm_servo.SetDegree(degree);
        Sleep(10);
    }

    driveUntilTime(90, 30, 200, true);
    driveUntilBump(10,80,-2);
    driveUntilTime(0, 70, 250, true);
    breakpoint();
    driveUntilBump(90, 80, 2);
    driveUntilBump(10, 80, 1);
    driveUntilBump(275, 60, -1);
    driveUntilTime(270, 60, 1200, true);
    turnUntilTime(60, 1500);
    breakpoint();
    driveUntilTime(88,80,3000,false);
    driveUntilBump(90, 60, 2);

    // Drive with heading 180 until microswitch 6 is bumped
    while(microswitch6.Value()) {
        motor1.SetPercent(-20);
        motor1.SetPercent(20);
        motor1.SetPercent(20);
        motor1.SetPercent(-20);
    }
    motor1.SetPercent(0);
    motor1.SetPercent(0);
    motor1.SetPercent(0);
    motor1.SetPercent(0);

    breakpoint();
    driveUntilTime(270, 50, 260, true);
    breakpoint();

    // Lower arm
    arm_servo.SetDegree(63);
    Sleep(1000);
    breakpoint();
    driveUntilTime(0,80,600,true);

    // Raise arm
    arm_servo.SetDegree(160);

    driveUntilTime(270, 80, 2800, true);
    driveUntilTime(0,50,5000,true);
    closeLog();
    return 0;
}

  • #ifndef START_H
#define START_H

void start();

#endif // START_H

  • /* This is part of a custom header that contains the start sequence for the robot.
 * Last modified: 3/7/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include required FEH libraries
#include <FEHRPS.h>
#include <FEHLCD.h>

// Include required custom libraries
#include "constants.h"
#include "worldstate.h"
#include <FEHServo.h>

/* This function goes through the start sequence of the robot to start with the start light.
 * A log is opened, RPS is initialized, the CdS cell is tested, and it waits for the start light.
 * Last modified: 3/8/2016 JKL
 */
void start() {
    initializeLog();

    RPS.InitializeTouchMenu();

    // Setup servo
    arm_servo.SetMin(SERVO_MIN);
    arm_servo.SetMax(SERVO_MAX);
    arm_servo.SetDegree(175);

    worldState(true,0,0,0,0,0,0);
    LCD.WriteRC("CdS Cell test", 5, 3);
    LCD.WriteRC("Press left button...", 6, 3);
    while( !button.LeftPressed()) {
        worldState(false,0,0,0,0,0,0);
        LCD.WriteRC(cdscell.Value(), 7, 10);
    }
    while(!button.LeftReleased());

    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);
    while(cdscell.Value() > 1) {
        LCD.WriteRC( cdscell.Value(), 7, 10);
    }

    LCD.WriteRC("Beginning run", 8, 3);
}

  • #ifndef CONSTANTS_H
#define CONSTANTS_H
#include <FEHMotor.h>
#include <FEHIO.h>
#include <FEHServo.h>

// Globally define PI so that it can be used
#define PI 3.14159
#define VERSION 1.0308
#define TITLE "PerformanceTest3"

// Globally define the timeout, though it is never used
#define TIMEOUT 10000

// Globally define the servo minimum and maximum
#define SERVO_MIN 606
#define SERVO_MAX 2016

// Globally declare the definitions for the motors
extern FEHMotor motor1;
extern FEHMotor motor2;
extern FEHMotor motor3;
extern FEHMotor motor4;

// Globally declare the arm servo
extern FEHServo arm_servo;

// Globally declare the microswitches for the 4 sides
extern DigitalInputPin microswitch1;
extern DigitalInputPin microswitch2;
extern DigitalInputPin microswitch3;
extern DigitalInputPin microswitch4;
extern DigitalInputPin microswitch5;
extern DigitalInputPin microswitch6;
extern DigitalInputPin microswitch7;
extern DigitalInputPin microswitch8;

// Globally declare the cdscell and ButtonBoard
extern AnalogInputPin cdscell;

extern ButtonBoard button;

#endif // CONSTANTS_H

  • #include <FEHIO.h>
#include <FEHMotor.h>
#include "constants.h"

// Declare the motors
FEHMotor motor1(FEHMotor::Motor0, 7.2);
FEHMotor motor2(FEHMotor::Motor1, 7.2);
FEHMotor motor3(FEHMotor::Motor2, 7.2);
FEHMotor motor4(FEHMotor::Motor3, 7.2);

// Declare the arm servo
FEHServo arm_servo(FEHServo::Servo0);

// Declare the microswitches
DigitalInputPin microswitch1(FEHIO::P1_0);
DigitalInputPin microswitch2(FEHIO::P1_2);
DigitalInputPin microswitch3(FEHIO::P1_4);
DigitalInputPin microswitch4(FEHIO::P1_6);
DigitalInputPin microswitch5(FEHIO::P2_0);
DigitalInputPin microswitch6(FEHIO::P2_2);
DigitalInputPin microswitch7(FEHIO::P2_4);
DigitalInputPin microswitch8(FEHIO::P2_6);

// Declare other buttons and sensors
AnalogInputPin cdscell(FEHIO::P0_0);

ButtonBoard button(FEHIO::Bank3);

  • #ifndef DRIVE_H
#define DRIVE_H
#include "constants.h"

/* This is part of a custom header that contains the drive functions for the robot.
 * driveUntilBump, driveUntilTime, and turnUntilTime are the only functions that work.
 * driveUntilRPSx and driveUntilRPSy supposedly work though they need to be refined.
 * Last modified: 3/8/2016 JKL (Comments added on 3/18/2016 JKL)
 */
void driveUntilTime(int heading, float power, int time, bool stop);
void driveUntilBump(int heading, float power, int bumpSide);
void driveUntilCds(int heading, float power, float cdsvalue);
void driveUntilRPSx(int desiredX, float power, int timeout);
void driveUntilRPSy(int desiredY, float power, int northFace, int timeout);
void driveUntilEncoder();
void driveUntilLine(int heading, float power);

void turnUntilTime(float power, int time);
void turnUntilEncoder();
void turnUntilRPS(int desiredHeading, int power, int timeout);

#endif // DRIVE_H

  • /* This is part of a custom header that contains the drive functions for the robot.
 * driveUntilBump, driveUntilTime, and turnUntilTime are the only functions that work.
 * driveUntilRPSx and driveUntilRPSy supposedly work though they need to be refined.
 * Last modified: 3/8/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include required FEH libraries
#include <FEHLCD.h>
#include <FEHBuzzer.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHIO.h>
#include <FEHRPS.h>

// Include required custom libraries
#include <math.h>
#include "constants.h"
#include "worldstate.h"
#include "drive.h"

//Declare global motor ratios
float motor1ratio;
float motor2ratio;
float motor3ratio;
float motor4ratio;

/* This function calculates the motor ratios for a given heading. The calculation is derived from vector quantitiies.
 * The calculated ratios are assigned to each individual motor ratio. The ratios range from -1 to 1.
 * Last modified: 2/26/2016
 */
void ratios(int heading) {

    LCD.WriteRC("Calculating motor ratios", 5,3);
    float x, y;
    x = cos( (45-heading) * PI / 180.0 );
    y = sin( (45-heading) * PI / 180.0 );
    LCD.WriteRC(x, 6, 3);
    LCD.WriteRC(y, 7, 3);
    motor1ratio = x;
    motor2ratio = -y;
    motor3ratio = -x;
    motor4ratio = y;
}

/* This function drives the robot with a given heading at a given power for a given time in milleseconds.
 * If bool stop is true, the robot does not stop the motors. There will not be a pause in between the next drive function.
 * Last modified: 3/9/2016
 */
void driveUntilTime(int heading, float power, int time, bool stop) {

    ratios(heading);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    LCD.WriteRC("Time:", 5, 3);
    LCD.WriteRC(time, 5, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    Sleep(time);

    if (stop) {
        motor1.SetPercent(0);
        motor2.SetPercent(0);
        motor3.SetPercent(0);
        motor4.SetPercent(0);

        Sleep(250);
    }
} // driveUntilTime

/* This function drives the robot with a given heading at a given power until a side is bumped (or not).
 * int bumpSide ranges from -4 to 4. The sides are numbered in a clockwise fashion. The front of the robot is side 1.
 * A positive value allows for a robot drive until that side is bumped.
 * A negative value allows for the robot to ride along that side until a microswitch is released.
 * Last modified: 3/9/2016
 */
void driveUntilBump(int heading, float power, int bumpSide) {

    ratios(heading);

    LCD.WriteRC("Bump:", 5, 3);
    LCD.WriteRC(bumpSide, 5, 9);
    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    if (bumpSide == -1)
        while( 2 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == -2)
        while( 2 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == -3)
        while( 2 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == -4)
        while( 2 != (microswitch7.Value() + microswitch8.Value()) );

    if (bumpSide == 1)
        while( 0 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == 2)
        while( 0 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == 3)
        while( 0 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == 4)
        while( 0 != (microswitch7.Value() + microswitch8.Value()) );

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function was intended to stop at the fuel delivery light when it reached the cdsvalue.
 * Through experimentation, the robot always stopped when it reached this value no matter the color, rendering this function useless.
 * Last modified: 3/9/2016 JKL
 */
void driveUntilCds(int heading, float power, float cdsvalue) {

    ratios(heading);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    LCD.WriteRC("CdS:", 5, 3);
    LCD.WriteRC(cdsvalue, 5, 8);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    while( cdscell.Value() > cdsvalue);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function corrects the x-position of the robot using RPS.
 * This is still under development as to whether the robot needs to stop between each check.
 * Last modified: 3/9/2016 JKL
 */
void driveUntilRPSx(int desiredX, float power, int timeout) {
    float startTime = TimeNow();
    while((RPS.X() < desiredX - 0.2 || RPS.X() > desiredX + 0.2) && TimeNow() - startTime < timeout) {
        if (RPS.X() < desiredX) {
            int heading = RPS.Heading() - 90;
            heading+= 360 * (heading < 0);
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            Sleep(200);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        if (RPS.X() > desiredX) {
            int heading = RPS.Heading() + 90;
            heading-= 360 * (heading < 360);
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    }
}

/* This function corrects the y-position of the robot using RPS.
 * This is still under development as to whether the robot needs to stop between each check.
 * Last modified: 3/9/2016 JKL
 */
void driveUntilRPSy(int desiredY, float power, int northFace, int timeout) {
    float startTime = TimeNowMSec();
    while((RPS.Y() < desiredY - 0.15 || RPS.Y() > desiredY + 0.15) && (TimeNowMSec() - startTime < timeout)) {
        if (RPS.Y() < desiredY) {
            driveUntilTime(90*(northFace-1), power, 10, false);
        }
        if (RPS.Y() > desiredY) {
            driveUntilTime(90*(northFace-1), -power, 10, false);
        }
    }
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
}

void driveUntilEncoder(int heading, float power, int clicks);
void driveUntilLine(int heading, float power);

/* This function turns the robot at a given power for a given time.
 * float power (0-100): positive = clockwise rotation, negative = counter-clockwise rotation
 * Last modified: 3/9/2016 JKL
 */
void turnUntilTime(float power, int time) {

    worldState(true, 0, power, 1, 1, 1, 1);
    LCD.WriteRC("Time:", 5, 3);
    LCD.WriteRC(time, 5, 9);

    motor1.SetPercent(power);
    motor2.SetPercent(power);
    motor3.SetPercent(power);
    motor4.SetPercent(power);

    Sleep(time);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
    worldState(true, 0, power, 1, 1, 1, 1);

    Sleep(250);

}

void turnUntilEncoder();

/* This function corrects the heading of the robot using RPS.
 * This algorithm is maybe fixed. Power should be less than 40.
 * Last modified: 3/9/2016 JKL
 */
void turnUntilRPS(int desiredHeading, int power, int timeout) {
    while(RPS.Heading() <  desiredHeading + 360*(desiredHeading==0) - 1 || RPS.Heading() > desiredHeading + 1 ) {
        LCD.WriteRC(RPS.Heading(), 5, 5);
        if (RPS.Heading() < desiredHeading + 360*(desiredHeading==0) - 1 ) {
            worldState(true, 0, power, 1, 1, 1, 1);

            motor1.SetPercent(power);
            motor2.SetPercent(power);
            motor3.SetPercent(power);
            motor4.SetPercent(power);

        }
        if (RPS.Heading() > desiredHeading + 1) {
            worldState(true, 0, -power, 1, 1, 1, 1);

            motor1.SetPercent(-power);
            motor2.SetPercent(-power);
            motor3.SetPercent(-power);
            motor4.SetPercent(-power);

        }
    }
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
    worldState(true,0,0,0,0,0,0);

}

  • #ifndef WORLDSTATE_H
#define WORLDSTATE_H

void initializeLog();
void worldState(bool updateLog, int heading, float power, float motor1ratio, float motor2ratio, float motor3ratio, float motor4ratio);
void closeLog();

#endif // WORLDSTATE_H

  • /* This is part of a custom header that contains the SD log and worldState functions for the robot.
 * initializeLog, closeLog, and worldState functions work.
 * Last modified: 3/7/2016 JKL (Comments added on 3/18/2016 JKL)
 */

// Include required FEH libraries
#include <FEHRPS.h>
#include <FEHIO.h>
#include <FEHLCD.h>
#include <FEHUtility.h>
#include <FEHSD.h>

// Include required custom library
#include "constants.h"

/* This function closes a log file left over, opens a new log, and prints the header to the text file.
 * Last modified: 3/7/2016
 */
void initializeLog(){
    // must close any remaining log files left over
    SD.CloseLog();
    SD.OpenLog();
    SD.Printf("%s - %f - Course %c\n", TITLE, VERSION, RPS.CurrentRegionLetter());
    SD.Printf("Time\tRPSTime\tMS1\tMS2\tMS3\tMS4\tMS5\tMS6\tMS7\tMS8\tCDSCell\tMot1\tMot2\tMot3\tMot4\tH.\tRPS_H\tRPS_X\tRPS_Y");
}
/* This function closes a log. Eliminates need to include another library in the main function.
 * Last modified: 3/7/2016
 */
void closeLog() {
    SD.CloseLog();
}

/* This function prints the world state to the Proteus LCD and, if requested, to a log file
 * Inputs: bool updateLog - true writes the world state to the log file / false does not
 *         The rest of the arguments are what is written to the log file
 * Last Modified: 3/7/2016 JKL
 */
void worldState(bool updateLog, int heading, float power, float motor1ratio, float motor2ratio, float motor3ratio, float motor4ratio) {
    LCD.Clear(BLACK);

    LCD.DrawRectangle(2*12+1, 4*17+1, 22*12-1, 6*16-1);
    //    LCD.WriteRC("*                    *", 5, 2);
    //    LCD.WriteRC("*                    *", 6, 2);
    //    LCD.WriteRC("*                    *", 7, 2);
    //    LCD.WriteRC("*                    *", 8, 2);

    LCD.SetFontColor(WHITE);

    LCD.WriteRC(TimeNow(), 13, 5);
    LCD.WriteRC(RPS.Time(), 13, 15);

    if (updateLog)
        SD.Printf("\n%f\t%f\t", TimeNow(), RPS.Time());

    LCD.WriteRC( (int) microswitch1.Value(), 0, 2);
    LCD.WriteRC( (int) microswitch2.Value(), 0, 23);
    LCD.WriteRC( (int) microswitch3.Value(), 2, 25);
    LCD.WriteRC( (int) microswitch4.Value(), 11, 25);
    LCD.WriteRC( (int) microswitch5.Value(), 13, 23);
    LCD.WriteRC( (int) microswitch6.Value(), 13, 2);
    LCD.WriteRC( (int) microswitch7.Value(), 11, 0);
    LCD.WriteRC( (int) microswitch8.Value(), 2, 0);

    if (updateLog)
        SD.Printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t",
              (int) microswitch1.Value(), (int) microswitch2.Value(), (int) microswitch3.Value(), (int) microswitch4.Value(),
              (int) microswitch5.Value(), (int) microswitch6.Value(), (int) microswitch7.Value(), (int) microswitch8.Value());

    if (0.0 < cdscell.Value() && cdscell.Value() < 0.8)
        LCD.SetFontColor(RED);
    if (1.0 < cdscell.Value() && cdscell.Value() < 1.8)
        LCD.SetFontColor(BLUE);
    LCD.WriteRC( cdscell.Value(), 0, 11);

    if (updateLog)
        SD.Printf("%f\t", cdscell.Value());

    LCD.SetFontColor(WHITE);
    LCD.WriteRC("Mot1", 2, 2);
    LCD.WriteRC(power * motor1ratio, 3, 2);
    LCD.WriteRC("2000", 3, 9);
    LCD.WriteRC("Mot2", 2, 20);
    LCD.WriteRC(power * motor2ratio, 3, 18);
    LCD.WriteRC("2000", 3, 13);
    LCD.WriteRC("Mot3", 11, 20);
    LCD.WriteRC(power * motor3ratio, 10, 18);
    LCD.WriteRC("2000", 10, 13);
    LCD.WriteRC("Mot4", 11, 2);
    LCD.WriteRC(power * motor4ratio, 10, 2);
    LCD.WriteRC("2000", 10, 9);

    if (updateLog)
        SD.Printf("%f\t%f\t%f\t%f\t", power * motor1ratio, power * motor2ratio, power * motor3ratio, power * motor4ratio);

    LCD.WriteRC("H:", 2, 10);
    LCD.WriteRC(heading, 2, 12);

    if (updateLog)
        SD.Printf("%d\t", heading);

    LCD.WriteRC("B:", 11, 9);
    LCD.WriteRC(RPS.Heading(), 11, 11);
    LCD.WriteRC("X:", 12, 4);
    LCD.WriteRC(RPS.X(), 12, 6);
    LCD.WriteRC("Y:", 12, 14);
    LCD.WriteRC(RPS.Y(), 12, 16);

    if (updateLog)
        SD.Printf("%f\t%f\t%f\t", RPS.Heading(), RPS.X(), RPS.Y());

}

Performance Test 4 – Fuel

  • The fourth and final performance test required the robot to touch and hold the correct fuel button for at least 5 seconds and press the final button at the end. The stretch bonus was controlling the supplies. Our robot successfully completed the performance test with the stretch bonus two days prior to the due date for a score of 25/20.
  • // Required FEH libraries
#include <FEHLCD.h>
#include <FEHUtility.h>
#include <FEHIO.h>

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

#include "PerformanceTest4Main.h"

PerformanceTest4Main PerformanceTest4;

/* This function adds a breakpoints to either continue or quit from the program.
 */
int PerformanceTest4Main::breakpoint() {
    if (false) {
        LCD.WriteRC("Press left button to", 5, 3);
        LCD.WriteRC("continue...", 6, 3);
        LCD.WriteRC("or middle to quit.", 7, 3);
        while(!button.LeftPressed() && !button.MiddlePressed());
        if (button.MiddlePressed()) {
            closeLog();
            return 1;
        }
        while(!button.LeftReleased() && !button.MiddleReleased());
        Sleep(250);
        return 0;
    }
}

/* This function is the main program for Performance Test 4.
 * This is run from the menu.
 * This is still being developed.
 * Last Modified 3/14/2016 JKL
 */
void PerformanceTest4Main::Run() {

    start();
    driveUntilTime(180, 80, 1100, true);

    turnUntilTime(60, 600);
    turnUntilRPS(0, 20, 5000);

    driveUntilTime(45, 80, 350, false);
    driveUntilBump(90, 60, 2);

    driveUntilRPSy(10.8 , 27, 8000);

    driveUntilBump(90, 30, 2);

    lowerToPickupArm();
    raiseToPickupArm();

    driveUntilTime(90, 30, 200, true);
    driveUntilBump(10,80,-2);
    driveUntilTime(0, 70, 250, true);
    driveUntilRPSy(23.8, 27, 2000);

    turnUntilRPS(0,20,1000);

    driveUntilBump(90, 80, 2);
    driveUntilBump(10, 80, 1);

    driveUntilBump(275, 60, -1);

    driveUntilTime(270, 50, 500, true);
    driveUntilRPSx(31.3, 25, 2000);

    driveUntilTime(0,60, 1500, true);

    driveUntilBump(90, 60, 2);

    driveUntilRPSy(61.8, 25, 1000);

    driveUntilTime(270, 40, 300, true);


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

    LCD.WriteRC(cdscell.Value(), 5, 5);

    if (1.0 < cdscell.Value() && cdscell.Value() < 2.6) {
       LCD.WriteRC("Blue!!!", 8, 5);
       driveUntilTime(0,50,800,true);

    }
    else if (0.0 < cdscell.Value() && cdscell.Value() < 0.8) {

        LCD.WriteRC("Red!!!", 8, 5);
        driveUntilBump(90, 50, 2);
        driveUntilTime(15,50,1000, true);
    }
    else {
        worldState(false, 0, 0, 0, 0, 0, 0);
        LCD.WriteRC("NO LIGHT", 5, 3);
        Sleep(10000);
    }

    worldState(true, 0, 0, 0, 0, 0,0);
    LCD.WriteRC("Sleep 6 sec", 5, 3);
    Sleep(6000);

    driveUntilTime(180, 60, 500, true);

    driveUntilBump(90, 50, 2);

    driveUntilBump(175, 60, -2);


    driveUntilTime(270, 60, 400, true);


    driveUntilTime(180, 60, 1000, true);


    driveUntilRPSy(42, 30, 2000);
    driveUntilRPSx(29.2, 28, 1000);
    turnUntilRPS(0,20, 1000);


    for (int loop = 0; loop<=12; loop ++)
        LCD.WriteRC("Warning: Going down!", loop,0);

    driveUntilTime(180, 50, 3000, true);


    driveUntilRPSy(22, 27, 2000);
    turnUntilRPS(0,20,1000);
    driveUntilTime(270, 60, 1350, true);
    turnUntilTime(40, 400);
    turnUntilRPS(315, 20, 2000);
    closeLog();
    driveUntilTime(180, 60, 3000, true);
    closeLog();
    return;

}
  • #ifndef START_H
#define START_H

void start();

#endif // START_H

  • /* This is part of a custom header that contains the start sequence for the robot.
 * Last modified: 3/21/2016 JKL
 */

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

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

/* 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, the CdS cell is tested, initializes the arm, and waits for the start light.
 * Last Modified: 3/21/2016 JKL
 */
void start() {

    RPS.InitializeTouchMenu();

    initializeLog();

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

    LCD.WriteRC("CdS Cell test", 5, 3);
    LCD.WriteRC("Press left button...", 6, 3);
    while( !button.LeftPressed()) {
        worldState(false,0,0,0,0,0,0);
        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);
    while(cdscell.Value() > 1) {
        LCD.WriteRC( cdscell.Value(), 7, 10);
    }

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

  • #ifndef CONSTANTS_H
#define CONSTANTS_H
#include <FEHMotor.h>
#include <FEHIO.h>
#include <FEHServo.h>

#define PI 3.14159

// This information is logged in the SD log files
// VERSION is [major build].[month][day]
// TITLE is the program name
#define VERSION 1.0314
#define TITLE "FEHRobotCode"

#define TIMEOUT 10000

// Define the min and max for the arm servo
#define SERVO_MIN 606
#define SERVO_MAX 2016

// Use of extern so that these can be used elsewhere in the code
extern FEHMotor motor1;
extern FEHMotor motor2;
extern FEHMotor motor3;
extern FEHMotor motor4;

extern FEHServo arm_servo;

extern DigitalInputPin microswitch1;
extern DigitalInputPin microswitch2;
extern DigitalInputPin microswitch3;
extern DigitalInputPin microswitch4;
extern DigitalInputPin microswitch5;
extern DigitalInputPin microswitch6;
extern DigitalInputPin microswitch7;
extern DigitalInputPin microswitch8;

extern AnalogInputPin cdscell;

extern ButtonBoard button;

#endif // CONSTANTS_H

  • #include <FEHIO.h>
#include <FEHMotor.h>
#include <FEHServo.h>
#include "constants.h"

// Declare the four drive motors going in a clockwise direction
FEHMotor motor1(FEHMotor::Motor0, 7.2);
FEHMotor motor2(FEHMotor::Motor1, 7.2);
FEHMotor motor3(FEHMotor::Motor2, 7.2);
FEHMotor motor4(FEHMotor::Motor3, 7.2);

// Declare the servo for the arm for the supplies
FEHServo arm_servo(FEHServo::Servo0);

// Declare the microswitches going in a clockwise direction
DigitalInputPin microswitch1(FEHIO::P1_0);
DigitalInputPin microswitch2(FEHIO::P1_2);
DigitalInputPin microswitch3(FEHIO::P1_4);
DigitalInputPin microswitch4(FEHIO::P1_6);
DigitalInputPin microswitch5(FEHIO::P2_0);
DigitalInputPin microswitch6(FEHIO::P2_2);
DigitalInputPin microswitch7(FEHIO::P2_4);
DigitalInputPin microswitch8(FEHIO::P2_6);

// Declare other sensors and the ButtonBoard
AnalogInputPin cdscell(FEHIO::P0_0);

ButtonBoard button(FEHIO::Bank3);

  • #ifndef DRIVE_H
#define DRIVE_H
#include "constants.h"

void driveUntilTime(int heading, float power, int time, bool stop);
void driveUntilBump(int heading, float power, int bumpSide);
void driveUntilCds(int heading, float power, float cdsvalue);
void driveUntilRPSx(float desiredX, float power, int timeout);
void driveUntilRPSy(float desiredY, float power, int timeout);
void driveUntilLine(int heading, float power);

void turnUntilTime(float power, int time);
void turnUntilRPS(int desiredHeading, int power, int timeout);


#endif // DRIVE_H

  • // Required FEH libraries
#include <FEHLCD.h>
#include <FEHBuzzer.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHIO.h>
#include <FEHRPS.h>

// Required library to calculate motor ratios based on heading
#include <math.h>

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

//Declare global motor ratios
float motor1ratio;
float motor2ratio;
float motor3ratio;
float motor4ratio;

/* This function calculates the motor ratios for a given heading. The calculation is derived from vector quantities.
 * The calculated ratios are assigned to each individual motor ratio. The ratios range from -1 to 1.
 * If the heading is a cardinal direction (N/S/E/W), the ratio is 0.707.
 * Last Modified: 3/14/2016 JKL
 */
void ratios(int heading) {

    LCD.WriteRC("Calculating motor ratios", 5,3);
    float x, y;
    x = cos( (45-heading) * PI / 180.0 );
    y = sin( (45-heading) * PI / 180.0 );
    LCD.WriteRC(x, 6, 3);
    LCD.WriteRC(y, 7, 3);
    motor1ratio = x;
    motor2ratio = -y;
    motor3ratio = -x;
    motor4ratio = y;
} // end ratios function

/* This function drives the robot with a given heading at a given power for a given time in milleseconds.
 * If bool stop is false, the robot does not stop the motors. There will not be a pause in between the next drive function.
 * Last Modified: 3/15/2016 JKL
 */
void driveUntilTime(int heading, float power, int time, bool stop) {

    ratios(heading);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    LCD.WriteRC("Time:", 5, 3);
    LCD.WriteRC(time, 5, 9);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    Sleep(time);

    if (stop) {
        motor1.SetPercent(0);
        motor2.SetPercent(0);
        motor3.SetPercent(0);
        motor4.SetPercent(0);

        Sleep(250);
    }
} // end driveUntilTime function

/* This function drives the robot with a given heading at a given power until a side is bumped (or not).
 * int bumpSide ranges from -4 to 4. The sides are numbered in a clockwise fashion. The front of the robot is side 1.
 * A positive value allows for a robot drive until that side is bumped.
 * A negative value allows for the robot to ride along that side until a microswitch is released.
 * Last Modified: 3/14/2016
 */
void driveUntilBump(int heading, float power, int bumpSide) {

    ratios(heading);

    LCD.WriteRC("Bump:", 5, 3);
    LCD.WriteRC(bumpSide, 5, 9);
    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    if (bumpSide == -1)
        while( 2 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == -2)
        while( 2 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == -3)
        while( 2 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == -4)
        while( 2 != (microswitch7.Value() + microswitch8.Value()) );

    if (bumpSide == 1)
        while( 0 != (microswitch1.Value() + microswitch2.Value()) );
    if (bumpSide == 2)
        while( 0 != (microswitch3.Value() + microswitch4.Value()) );
    if (bumpSide == 3)
        while( 0 != (microswitch5.Value() + microswitch6.Value()) );
    if (bumpSide == 4)
        while( 0 != (microswitch7.Value() + microswitch8.Value()) );

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);
} // end driveUntilBump function

/* This function drives the robot with a given heading at a given power until the cdsvalue is reached.
 * Not sure what the benefit this function will have as it cannot correctly stop for both lights.
 * Last Modified: 3/14/2016 JKL
 */
void driveUntilCds(int heading, float power, float cdsvalue) {

    ratios(heading);

    worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
    LCD.WriteRC("CdS:", 5, 3);
    LCD.WriteRC(cdsvalue, 5, 8);

    motor1.SetPercent(power * motor1ratio);
    motor2.SetPercent(power * motor2ratio);
    motor3.SetPercent(power * motor3ratio);
    motor4.SetPercent(power * motor4ratio);

    while( cdscell.Value() > cdsvalue);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);

    Sleep(250);

}

/* This function corrects the x-position of the robot using RPS.
 * This is still under development as to whether the robot needs to stop between each check.
 * Last Modified: 3/21/2016 JKL
 */
void driveUntilRPSx(float desiredX, float power, int timeout) {
    float startTime = TimeNowMSec();
    while((RPS.X() < desiredX - 0.25 || RPS.X() > desiredX + 0.25) && TimeNowMSec() - startTime < timeout) {
        if (RPS.X() < desiredX) {
            int heading = RPS.Heading() + 90;
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            while( fabs( desiredX - RPS.X() ) >= 2);
            if ( fabs( desiredX - RPS.X() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        else if (RPS.X() > desiredX) {
            int heading = RPS.Heading() - 90;
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredX, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            while( fabs( desiredX - RPS.X() ) >= 2);
            if ( fabs( desiredX - RPS.X() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    }
}

/* This function corrects the y-position of the robot using RPS.
 * This is still under development as to whether the robot needs to stop between each check.
 * Last Modified: 3/21/2016 JKL
 */
void driveUntilRPSy(float desiredY, float power, int timeout) {
    float startTime = TimeNowMSec();
    while((RPS.Y() < desiredY - 0.25 || RPS.Y() > desiredY + 0.25) && TimeNowMSec() - startTime < timeout) {
        if (RPS.Y() < desiredY) {
            int heading = RPS.Heading();
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredY, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            while( fabs( desiredY - RPS.Y() ) >= 2);
            if ( fabs( desiredY - RPS.Y() ) < 2)
                Sleep(250);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);

        }
        else if (RPS.Y() > desiredY) {
            int heading = RPS.Heading() + 180;
            heading-= 360 * (heading < 360);
            ratios(heading);

            worldState(true, heading, power, motor1ratio, motor2ratio, motor3ratio, motor4ratio);
            LCD.WriteRC("RPS:", 5, 3);
            LCD.WriteRC(desiredY, 5, 8);

            motor1.SetPercent(power * motor1ratio);
            motor2.SetPercent(power * motor2ratio);
            motor3.SetPercent(power * motor3ratio);
            motor4.SetPercent(power * motor4ratio);

            while( fabs( desiredY - RPS.Y() ) >= 2);
            if ( fabs( desiredY - RPS.Y() ) < 2)
                Sleep(300);

            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);

            Sleep(250);
        }
    }
}

/* This function will drive the robot until the optosensor detects a line.
 * This is not yet developed because we do not have an optosensor on our robot yet.
 * Last Modified: 3/14/2016 JKL
 */
void driveUntilLine(int heading, float power);

/* This function turns the robot at a given power for a given time.
 * float power (0-100): positive = clockwise rotation, negative = counter-clockwise rotation
 * Last Modified: 3/14/2016 JKL
 */
void turnUntilTime(float power, int time) {

    worldState(true, 0, power, 1, 1, 1, 1);
    LCD.WriteRC("Time:", 5, 3);
    LCD.WriteRC(time, 5, 9);

    motor1.SetPercent(power);
    motor2.SetPercent(power);
    motor3.SetPercent(power);
    motor4.SetPercent(power);

    Sleep(time);

    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
    worldState(true, 0, power, 1, 1, 1, 1);

    Sleep(250);

}

/* This function corrects the heading of the robot using RPS.
 * This algorithm is maybe fixed. Power should be less than 30.
 * Last Modified: 3/21/2016 JKL
 */
void turnUntilRPS(int desiredHeading, int power, int timeout) {
    int startTime = TimeNowMSec();
    while( ( fabs(desiredHeading-RPS.Heading())>1.5 && fabs(desiredHeading-RPS.Heading())<358.5 ) && (TimeNowMSec() - startTime < timeout)) {
        worldState(true, desiredHeading, power, 1,1,1,1);
        LCD.WriteRC("Desired H:", 5, 3);
        LCD.WriteRC(desiredHeading, 5, 13);
        if ( fabs(desiredHeading-RPS.Heading())<10 || fabs(desiredHeading-RPS.Heading())>350) {
            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);
            worldState(true, desiredHeading, power, 0,0,0,0);
            Sleep(400);
        }
        if (fabs(desiredHeading-RPS.Heading())<1.5 || fabs(desiredHeading-RPS.Heading())>358.5) {
            motor1.SetPercent(0);
            motor2.SetPercent(0);
            motor3.SetPercent(0);
            motor4.SetPercent(0);
            worldState(true, desiredHeading, power, 0,0,0,0);
            return;
        }


        if (RPS.Heading() - desiredHeading >= 0 ) {
            if (RPS.Heading() - desiredHeading <= 180) {
                motor1.SetPercent(power);
                motor2.SetPercent(power);
                motor3.SetPercent(power);
                motor4.SetPercent(power);
            }
            else if (desiredHeading - RPS.Heading() < 180) {
                motor1.SetPercent(-power);
                motor2.SetPercent(-power);
                motor3.SetPercent(-power);
                motor4.SetPercent(-power);

            }
        }
        if (RPS.Heading() - desiredHeading < 0 ) {
            if (desiredHeading - RPS.Heading() <= 180) {
                motor1.SetPercent(-power);
                motor2.SetPercent(-power);
                motor3.SetPercent(-power);
                motor4.SetPercent(-power);
            }
            else if (RPS.Heading() - desiredHeading < 180) {
                motor1.SetPercent(power);
                motor2.SetPercent(power);
                motor3.SetPercent(power);
                motor4.SetPercent(power);
            }
        }
    }
    motor1.SetPercent(0);
    motor2.SetPercent(0);
    motor3.SetPercent(0);
    motor4.SetPercent(0);
    worldState(true, desiredHeading, power, 0,0,0,0);
}

  • #ifndef SUPPLYARM_H
#define SUPPLYARM_H

void initializeArm();
void lowerToPickupArm();
void raiseToPickupArm();
void lowerToDepositArm();
void raiseToDepositArm();

#endif // SUPPLYARM_H

  • /* This is part of a custom header that contains the functions to operate the servo
 * for the arm to pickup and deposit the supplies.
 * Last modified: 3/21/2016 JKL
 */

// Required FEH libraries
#include <FEHServo.h>
#include <FEHUtility.h>

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

/* This function is run from void start() at the beginning of the run.
 * It sets the minimum and maximum of the servo and raises it to an upright position.
 * Last modified: 3/21/2016 JKL
 */
void initializeArm() {
    arm_servo.SetMin(SERVO_MIN);
    arm_servo.SetMax(SERVO_MAX);
    arm_servo.SetDegree(175);
}

/* This function is run to lower the arm to contact the supplies.
 * Last modified: 3/21/2016 JKL
 */
void lowerToPickupArm() {
    arm_servo.SetDegree(16);
    Sleep(1000);
}

/* This function is run to pickup the supplies without dropping it.
 * A delay must be added to ensure that the supplies are picked up.
 * Last modified: 3/21/2016 JKL
 */
void raiseToPickupArm() {
    for (int degree = 18; degree <= 175; degree+= 2 ) {
        arm_servo.SetDegree(degree);
        Sleep(10);
    }
}

/* This function is run to lower the supplies into the drop zone.
 * Last modified: 3/21/2016 JKL
 */
void lowerToDepositArm() {
    arm_servo.SetDegree(63);
    Sleep(1000);
}

/* This function is run to raise the arm after the supplies are deposited.
 * Last modified: 3/21/2016 JKL
 */
void raiseToDepositArm() {
    arm_servo.SetDegree(160);
}

  • #ifndef WORLDSTATE_H
#define WORLDSTATE_H

void initializeLog();
void worldState(bool updateLog, int heading, float power, float motor1ratio, float motor2ratio, float motor3ratio, float motor4ratio);
void closeLog();

#endif // WORLDSTATE_H

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

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

/* This function initializes the SD card for logging. It closes any open logs, opens a new log, and prints a log header.
 * From past experiences, it is best to try to close all 1000 logs in case any are still open.
 * Last modified: 3/22/2016 JKL
 */
void initializeLog(){
    // must close any remaining log files left over
    for (int x = 0; x<=100; x++) {
        LCD.WriteRC("Closing Log:", 7, 3);
        LCD.WriteRC(x, 7, 16);
        SD.CloseLog();
    }
    SD.OpenLog();
    SD.Printf("%s - %f - Course %c\n", TITLE, VERSION, RPS.CurrentRegionLetter() );
    SD.Printf("Time\tRPSTime\tBatVolt\tMS1\tMS2\tMS3\tMS4\tMS5\tMS6\tMS7\tMS8\tCDSCell\tMot1\tMot2\tMot3\tMot4\tH.\tRPS_H\tRPS_X\tRPS_Y");
} // end initializeLog function

/* This function closes the SD log. This allows for the FEHSD.h library to be isolated in this program.
 * Last Modified: 3/14/2016 JKL
 */
void closeLog() {
    SD.CloseLog();
} // end closeLog function

/* This function prints the world state to the Proteus LCD and, if requested, to a log file
 * Inputs: - bool updateLog - true writes the world state to the log file / false does not
 *         - The rest of the arguments are what is written to the log file
 * Last Modified: 3/14/2016 JKL
 */
void worldState(bool updateLog, int heading, float power, float motor1ratio, float motor2ratio, float motor3ratio, float motor4ratio) {
    LCD.Clear(BLACK);

    LCD.DrawRectangle(2*12+1, 4*17+1, 22*12-1, 6*16-1);
    //    LCD.WriteRC("*                    *", 5, 2);
    //    LCD.WriteRC("*                    *", 6, 2);
    //    LCD.WriteRC("*                    *", 7, 2);
    //    LCD.WriteRC("*                    *", 8, 2);

    LCD.SetFontColor(WHITE);

    LCD.WriteRC(TimeNow(), 13, 5);
    LCD.WriteRC(RPS.Time(), 13, 15);

    if (updateLog)
        SD.Printf("\n%f\t%f\t", TimeNow(), RPS.Time());

    if (updateLog)
        SD.Printf("%f\t", Battery.Voltage());

    LCD.WriteRC( (int) microswitch1.Value(), 0, 2);
    LCD.WriteRC( (int) microswitch2.Value(), 0, 23);
    LCD.WriteRC( (int) microswitch3.Value(), 2, 25);
    LCD.WriteRC( (int) microswitch4.Value(), 11, 25);
    LCD.WriteRC( (int) microswitch5.Value(), 13, 23);
    LCD.WriteRC( (int) microswitch6.Value(), 13, 2);
    LCD.WriteRC( (int) microswitch7.Value(), 11, 0);
    LCD.WriteRC( (int) microswitch8.Value(), 2, 0);

    if (updateLog)
        SD.Printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t",
              (int) microswitch1.Value(), (int) microswitch2.Value(), (int) microswitch3.Value(), (int) microswitch4.Value(),
              (int) microswitch5.Value(), (int) microswitch6.Value(), (int) microswitch7.Value(), (int) microswitch8.Value());

    if (0.0 < cdscell.Value() && cdscell.Value() < 0.8)
        LCD.SetFontColor(RED);
    if (1.0 < cdscell.Value() && cdscell.Value() < 1.8)
        LCD.SetFontColor(BLUE);
    LCD.WriteRC( cdscell.Value(), 0, 11);

    if (updateLog)
        SD.Printf("%f\t", cdscell.Value());

    LCD.SetFontColor(WHITE);
    LCD.WriteRC("Mot1", 2, 2);
    LCD.WriteRC(power * motor1ratio, 3, 2);

    LCD.WriteRC("Mot2", 2, 20);
    LCD.WriteRC(power * motor2ratio, 3, 18);

    LCD.WriteRC("Mot3", 11, 20);
    LCD.WriteRC(power * motor3ratio, 10, 18);
    LCD.WriteRC(motor3ratio, 10, 13);

    LCD.WriteRC("Mot4", 11, 2);
    LCD.WriteRC(power * motor4ratio, 10, 2);
    LCD.WriteRC(motor4ratio, 10, 5);

    if (updateLog)
        SD.Printf("%f\t%f\t%f\t%f\t", power * motor1ratio, power * motor2ratio, power * motor3ratio, power * motor4ratio);

    LCD.WriteRC("H:", 2, 10);
    LCD.WriteRC(heading, 2, 12);

    if (updateLog)
        SD.Printf("%d\t", heading);

    LCD.WriteRC("B:", 11, 9);
    LCD.WriteRC(RPS.Heading(), 11, 11);
    LCD.WriteRC("X:", 12, 4);
    LCD.WriteRC(RPS.X(), 12, 6);
    LCD.WriteRC("Y:", 12, 14);
    LCD.WriteRC(RPS.Y(), 12, 16);

    if (updateLog)
        SD.Printf("%f\t%f\t%f\t", RPS.Heading(), RPS.X(), RPS.Y());
} // end worldState function