This page contains all of the codes used in testing. Detailed descriptions of the test are given in the testing logs subpage. See that for more information.
FirstSteps (2/22/2016)
- This test essentially tested the basic maneuverability of our robot including turning and driving in straight lines.
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> #include <FEHMotor.h> FEHMotor motor1(FEHMotor::Motor0, 7.2); FEHMotor motor2(FEHMotor::Motor1, 7.2); FEHMotor motor3(FEHMotor::Motor2, 7.2); FEHMotor motor4(FEHMotor::Motor3, 7.2); ButtonBoard button(FEHIO::Bank3); void stop() { motor1.SetPercent(0); motor2.SetPercent(0); motor3.SetPercent(0); motor4.SetPercent(0); Sleep(1.0); } int main() { LCD.WriteLine("Press the left button"); LCD.WriteLine("to go forward..."); while(!button.LeftPressed()); while(button.LeftReleased()); motor1.SetPercent(80.0); motor2.SetPercent(-80.0); motor3.SetPercent(-80.0); motor4.SetPercent(80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go backward..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(-80.0); motor2.SetPercent(80.0); motor3.SetPercent(80.0); motor4.SetPercent(-80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go left..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(-80.0); motor2.SetPercent(-80.0); motor3.SetPercent(80.0); motor4.SetPercent(80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go right..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(80.0); motor2.SetPercent(80.0); motor3.SetPercent(-80.0); motor4.SetPercent(-80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go turn CW..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(80.0); motor2.SetPercent(80.0); motor3.SetPercent(80.0); motor4.SetPercent(80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go turn CCW..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(-80.0); motor2.SetPercent(-80.0); motor3.SetPercent(-80.0); motor4.SetPercent(-80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go NE..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(80.0); motor2.SetPercent(0); motor3.SetPercent(-80.0); motor4.SetPercent(0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go turn SW..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(-80.0); motor2.SetPercent(0); motor3.SetPercent(80.0); motor4.SetPercent(0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go turn NW..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(0); motor2.SetPercent(-80.0); motor3.SetPercent(0); motor4.SetPercent(80.0); Sleep(1.0); stop(); LCD.WriteLine("Press the left button"); LCD.WriteLine("to go turn SE..."); // while(!button.LeftPressed()); // while(button.LeftReleased()); motor1.SetPercent(0); motor2.SetPercent(80.0); motor3.SetPercent(0); motor4.SetPercent(-80.0); Sleep(1.0); stop(); }
-
RPSTest (3/11/2016)
- This program tested the ability to turn the robot into a correct range of headings via RPS.
-
#include <FEHLCD.h> #include <FEHRPS.h> #include "constants.h" #include "drive.h" #include "worldstate.h" int main(void) { RPS.InitializeTouchMenu(); initializeLog(); LCD.WriteRC("Press left button...", 5, 3); while(!button.LeftPressed()); while(!button.LeftReleased()); LCD.WriteRC("Left button pressed", 6, 3); turnUntilRPS(0, 15, 9000); turnUntilRPS(45, 15, 9000); turnUntilRPS(90, 15, 9000); turnUntilRPS(180, 15, 9000); turnUntilRPS(270, 15, 9000); worldState(true, 0, 0, 0, 0, 0, 0); closeLog(); return 0; } -
#ifndef CONSTANTS_H #define CONSTANTS_H #include <FEHMotor.h> #include <FEHIO.h> #define PI 3.14159 #define VERSION 2 #define TITLE "RPSTest" #define TIMEOUT 10000 extern FEHMotor motor1; extern FEHMotor motor2; extern FEHMotor motor3; extern FEHMotor motor4; 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 "constants.h" //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); //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); //other buttons and sensors 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(int desiredX, float power, int timeout); void driveUntilRPSy(int desiredY, float power, 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
-
#include <FEHLCD.h> #include <FEHBuzzer.h> #include <FEHUtility.h> #include <FEHMotor.h> #include <FEHIO.h> #include <FEHRPS.h> #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. */ 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. */ 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. */ 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 drives the robot with a given heading at a given power until the cdsvalue is reached. */ 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. */ 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. */ void driveUntilRPSy(int desiredY, float power, int timeout) { float startTime = TimeNow(); while((RPS.Y() < desiredY - 0.2 || RPS.Y() > desiredY + 0.2) && TimeNow() - 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); Sleep(200); motor1.SetPercent(0); motor2.SetPercent(0); motor3.SetPercent(0); motor4.SetPercent(0); Sleep(250); } 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); Sleep(300); motor1.SetPercent(0); motor2.SetPercent(0); motor3.SetPercent(0); motor4.SetPercent(0); Sleep(250); } } } 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 */ 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. */ void turnUntilRPS(int desiredHeading, int power, int timeout) { int startTime = TimeNowMSec(); while( ( fabs(desiredHeading-RPS.Heading())>1 && fabs(desiredHeading-RPS.Heading())<359 ) && (TimeNowMSec() - startTime < timeout)) { worldState(true, desiredHeading, power, 1,1,0,0); LCD.WriteRC("Desired H:", 5, 3); LCD.WriteRC(desiredHeading, 5, 13); if ( fabs(desiredHeading-RPS.Heading())<5 || fabs(desiredHeading-RPS.Heading())>355) { motor1.SetPercent(0); motor2.SetPercent(0); motor3.SetPercent(0); motor4.SetPercent(0); worldState(true, desiredHeading, power, 0,0,0,0); Sleep(500); } if (fabs(desiredHeading-RPS.Heading())<0.5 || fabs(desiredHeading-RPS.Heading())>359.5) { motor1.SetPercent(0); motor2.SetPercent(0); motor3.SetPercent(0); motor4.SetPercent(0); worldState(true, desiredHeading, power, 0,0,0,0); Sleep(3000); return; } if (desiredHeading>RPS.Heading() && (180 >= fabs(desiredHeading-RPS.Heading()))) { motor1.SetPercent(power); motor2.SetPercent(power); motor3.SetPercent(power); motor4.SetPercent(power); LCD.WriteRC(1, 6, 3); } else if (desiredHeading<RPS.Heading() && (180 <= fabs(desiredHeading-RPS.Heading()))) { motor1.SetPercent(power); motor2.SetPercent(power); motor3.SetPercent(power); motor4.SetPercent(power); } else if (desiredHeading<RPS.Heading() && fabs(desiredHeading-RPS.Heading())<180) { motor1.SetPercent(-power); motor2.SetPercent(-power); motor3.SetPercent(-power); motor4.SetPercent(-power); } else if (desiredHeading>RPS.Heading() && fabs(desiredHeading-RPS.Heading())<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 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
-
#include <FEHRPS.h> #include <FEHIO.h> #include <FEHLCD.h> #include <FEHUtility.h> #include <FEHSD.h> #include "constants.h" 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"); } void closeLog() { SD.CloseLog(); } 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()); }
-
DriveWhileRotate (4/8/2016)
- This program tested the omnidirectional wheels of the robot by making the robot rotate while driving straight.
-
// Required FEH libraries #include <FEHLCD.h> #include <FEHBuzzer.h> #include <FEHUtility.h> #include <FEHMotor.h> #include <FEHIO.h> #include <FEHRPS.h> #include <FEHSD.h> // Required library to calculate motor ratios based on heading #include <math.h> // Required custom libraries #include "constants.h" #include "worldstate.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 void driveWhileRotate(int startHeading, float turnDegree, float power, float turnPower, int time) { float x = turnDegree / (time / 50.0); for( int increment = 0; increment <= turnDegree; increment += x) { ratios(startHeading + increment); motor1.SetPercent(power * motor1ratio + turnPower); motor2.SetPercent(power * motor2ratio + turnPower); motor3.SetPercent(power * motor3ratio + turnPower); motor4.SetPercent(power * motor4ratio + turnPower); Sleep(50); if (increment > turnDegree / 4.0) break; } motor1.SetPercent(0); motor2.SetPercent(0); motor3.SetPercent(0); motor4.SetPercent(0); } int main(void) { while(button.LeftReleased()); driveWhileRotate(225, 180, 100, -22, 3000); } -
#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.0330 #define TITLE "FEHRobotCode" #define TIMEOUT 10000 // Define the min and max for the arm servo #define SERVO_MIN 606 #define SERVO_MAX 2016 // Define CdS cell values #define CDSRedLow 0.0 #define CDSRedHigh 0.8 #define CDSBlueLow 1.0 #define CDSBlueHigh 2.6 // Define all RPS values #define RPSOffStart 24.0 #define RPSSuppliesY 10.8 // calibrate per course #define RPSTempRampBottomY 24.0 #define RPSTempRampTopX 31.3 #define RPSFuelLightY 61.8 // calibrate per course #define RPSFuelLightX 31.8 // calibrate per course #define RPSLongRunY 50.0 #define RPSRedTopY 43.5 #define RPSRedTopX 1.5 #define RPSWhiteTopX 6.4 #define RPSBlueTopX 11.5 #define RPSTopRampX 27.7 // calibrate per course #define RPSTopRampY 42.5 #define RPSBottomRampY 22.0 #define RPSFinalButtonX 18.4 #define RPSFinalButtonY 21.3 // 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
-
// Required FEH libraries #include <FEHIO.h> #include <FEHMotor.h> #include <FEHServo.h> // Required custom library #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);
-