/* Include preprocessor directives */
#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHServo.h>
#include <FEHRPS.h>
#include <math.h>
#define TRANSITIONS_PER_ROTATION 318
#define WHEEL_RADIUS 1.25
// Width of the robot from center of back wheel to center of back wheel
#define WIDTH 7.5
#define ARM_MAX 106.0
#define ARM_MIN 16.0
// Declarations for encoders, motors, and CdS cell
DigitalEncoder rightEncoder(FEHIO::P0_1);
DigitalEncoder leftEncoder(FEHIO::P0_0);
FEHMotor rightMotor(FEHMotor::Motor2,9.0);
FEHMotor leftMotor(FEHMotor::Motor3,9.0);
FEHServo servoArm(FEHServo::Servo7);
AnalogInputPin cdsCell(FEHIO::P1_0);
/**
* Moves the robot forward a set distance using percent force of motors.
*
* @parampercent
* how fast the motors move
* @paraminches
* how far the motors move (in in.)
* @requires
* -100 <= percent <= 100
* 0 < inches
* @ensures
* [the robot will move set inches forward +-1 in.]
*/
void moveForward(float percent, float inches) {
// Reset encoder counts
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
// Calculate needed counts
float counts = (inches * TRANSITIONS_PER_ROTATION) / (2 * M_PI * WHEEL_RADIUS);
// Set both motors to desired percent
rightMotor.SetPercent(percent);
leftMotor.SetPercent(percent);
// While the average of the left and right encoder is less than counts,
// keep running motors
while(((leftEncoder.Counts() + rightEncoder.Counts()) / 2.0) < counts) {
if(leftEncoder.Counts() > rightEncoder.Counts()) {
rightMotor.SetPercent(1.05*percent);
leftMotor.SetPercent(percent);
}
else if(leftEncoder.Counts() < rightEncoder.Counts()) {
leftMotor.SetPercent(1.05*percent);
rightMotor.SetPercent(percent);
}
else {
leftMotor.SetPercent(percent);
rightMotor.SetPercent(percent);
}
}
// Turn off motors
rightMotor.Stop();
leftMotor.Stop();
}
/**
* Moves the robot forward a set time using percent force of motors.
*
* @parampercent
* how fast the motors move
* @paramseconds
* how long the motors move (in sec.)
* @param waitTime
* how long the function waits to return after the motors move (in sec.)
* @requires
* -100 <= percent <= 100
* 0 < seconds
* 0 < waitTime
* @ensures
* [the robot will move forward for set seconds and will wait for waitTime seconds]
*/
void moveForward(float percent, float seconds, float waitTime) {
// Reset encoder counts
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
// Initialize seconds start to time now
float secondsStart = TimeNow();
// Set both motors to desired percent
rightMotor.SetPercent(percent);
// While the average of the left and right encoder is less than counts,
// keep running motors
while(TimeNow() - secondsStart < seconds) {
if(leftEncoder.Counts() > rightEncoder.Counts()) {
rightMotor.SetPercent(1.05*percent);
leftMotor.SetPercent(percent);
}
else if(leftEncoder.Counts() < rightEncoder.Counts()) {
leftMotor.SetPercent(1.05*percent);
rightMotor.SetPercent(percent);
}
else {
leftMotor.SetPercent(percent);
rightMotor.SetPercent(percent);
}
}
// Turn off motors
rightMotor.Stop();
leftMotor.Stop();
Sleep(waitTime);
}
/**
* Turns the robot directly right a set number of degrees using percent force of motors.
*
* @parampercent
* how fast the motors move
* @paramdegrees
* how far the motors move (in degrees)
* @requires
* -100 <= percent <= 100
* 0 < degrees
* @ensures
* [the robot will turn set degress right +-5 degrees]
*/
void turnRightDirect(float percent, float degrees) {
// Reset encoder counts
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
// Calculate needed distance (with compensation for friction) and then counts
float inches = ((degrees * M_PI) / 180.0) * ((WIDTH / 2) + .3);
float counts = ((inches * TRANSITIONS_PER_ROTATION) / (2 * M_PI * WHEEL_RADIUS));
// Set both motors to desired percent
rightMotor.SetPercent(percent*-1.0);
leftMotor.SetPercent(percent);
// While the average of the left and right encoder is less than counts,
// keep running motors
while((rightEncoder.Counts() + leftEncoder.Counts()) / 2.0 < counts) {
if(leftEncoder.Counts() > rightEncoder.Counts()) {
rightMotor.SetPercent(-1.05*percent);
leftMotor.SetPercent(percent);
}
else if(leftEncoder.Counts() < rightEncoder.Counts()) {
leftMotor.SetPercent(1.05*percent);
rightMotor.SetPercent(-1.0*percent);
}
else {
leftMotor.SetPercent(percent);
rightMotor.SetPercent(-1.0*percent);
}
}
// Turn off motors
rightMotor.Stop();
leftMotor.Stop();
}
/**
* Turns the robot directly left a set number of degrees using percent force of motors.
*
* @parampercent
* how fast the motors move
* @paramdegrees
* how far the motors move (in degrees)
* @requires
* -100 <= percent <= 100
* 0 < degrees
* @ensures
* [the robot will turn set degress left +-5 degrees]
*/
void turnLeftDirect(float percent, float degrees) {
// Reset encoder counts
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
// Calculate needed distance (with compensation for friction) and then counts
float inches = ((degrees * M_PI) / 180.0) * ((WIDTH / 2) + .3);
float counts = ((inches * TRANSITIONS_PER_ROTATION) / (2 * M_PI * WHEEL_RADIUS));
//Set both motors to desired percent
rightMotor.SetPercent(percent);
leftMotor.SetPercent(percent*-1.0);
// While the average of the left and right encoder is less than counts,
// keep running motors
while((rightEncoder.Counts() + leftEncoder.Counts()) / 2.0 < counts) {
if(leftEncoder.Counts() > rightEncoder.Counts()) {
rightMotor.SetPercent(1.05*percent);
leftMotor.SetPercent(-1.0*percent);
}
else if(leftEncoder.Counts() < rightEncoder.Counts()) {
leftMotor.SetPercent(-1.05*percent);
rightMotor.SetPercent(percent);
}
else {
leftMotor.SetPercent(-1.0*percent);
rightMotor.SetPercent(percent);
}
}
// Turn off motors
rightMotor.Stop();
leftMotor.Stop();
}
/**
* Turns the robot with the left wheel a set number of degrees using percent force of motors.
*
* @parampercent
* how fast the motors move
* @paramdegrees
* how far the motor will move (in degrees)
* @requires
* -100 <= percent <= 100
* 0 < degrees
* @ensures
* [the robot will turn set degress right +-5 degrees]
*/
void turnRightOneWheel(float percent, float degrees) {
// Reset encoder counts
leftEncoder.ResetCounts();
// Calculate needed distance and then counts
float inches = ((degrees * M_PI) / 180.0) * (WIDTH);
float counts = ((inches * TRANSITIONS_PER_ROTATION) / (2 * M_PI * WHEEL_RADIUS));
// Set motor to desired percent
leftMotor.SetPercent(percent);
// Keep running motors until counts is reached
while(leftEncoder.Counts() < counts) {}
// Turn off motors
leftMotor.Stop();
}
/**
* Turns the robot with the right wheel a set number of degrees using percent force of motors.
*
* @parampercent
* how fast the motors move
* @paramdegrees
* how far the motor will move (in degrees)
* @requires
* -100 <= percent <= 100
* 0 < degrees
* @ensures
* [the robot will turn set degress left +-5 degrees]
*/
void turnLeftOneWheel(float percent, float degrees) {
// Reset encoder counts
rightEncoder.ResetCounts();
// Calculate needed distance and then counts
float inches = ((degrees * M_PI) / 180.0) * (WIDTH);
float counts = ((inches * TRANSITIONS_PER_ROTATION) / (2 * M_PI * WHEEL_RADIUS));
// Set motor to desired percent
rightMotor.SetPercent(percent);
// Keep running motors until counts is reached
while(rightEncoder.Counts() < counts) {}
// Turn off motors
rightMotor.Stop();
}
/**
* The robot goes to a given position.
*
* @param xPos
* x-position robot goes to
* @param yPos
* y-position robot goes to
* @param heading
* heading robot will face
* @requires
* [xPos, yPos, and heading are all valid values for the course]
* [The robot has no obstructions on a straight line path to the position]
* @ensures
* [The robot will go to the given position and heading within 1 in. in each direction and 1 degree]
*/
void goTo(float xPos, float yPos, float heading, float motorPercent) {
// Wait and get valid current position
Sleep(0.5);
float currentX = RPS.X();
float currentY = RPS.Y();
float currentHeading = RPS.Heading();
// Check robot is far away from position
if (abs(xPos-currentX) > 1 || abs(yPos-currentY) > 1) {
// Calculate needed heading
float neededHeading;
if (currentX != xPos) {
neededHeading = 360 * atan((yPos-currentY)/(xPos-currentX)) / M_PI;
} else {
neededHeading = 90 * (yPos-currentY)/abs(yPos-currentY);
}
// Turn to needed heading
if (abs(neededHeading-currentHeading) > 180) {
turnLeftDirect(motorPercent*(neededHeading-currentHeading)/abs(neededHeading-currentHeading), 360 - abs(neededHeading - currentHeading));
} else {
turnLeftDirect(motorPercent*(neededHeading-currentHeading)/abs(neededHeading-currentHeading), abs(neededHeading-currentHeading));
}
// Calculate needed distance
float distance = sqrt(pow(xPos - currentX, 2) + pow(yPos - currentY, 2));
// Go the needed distance
moveForward(motorPercent, distance);
// Recursively go to the position again to verify position
goTo(xPos, yPos, heading, motorPercent);
}
else if (abs(heading-currentHeading) > 1) {
// Initialize motor percent
float motorPercentTurning = 20;
// Repeat turning until heading is correct
while (abs(heading-currentHeading) > 5 && abs(heading-currentHeading) < 355) {
if (abs(heading-currentHeading) > 180) {
turnLeftDirect(motorPercentTurning*(heading-currentHeading)/abs(heading-currentHeading), 360 - abs(heading - currentHeading));
} else {
turnLeftDirect(motorPercentTurning*(heading-currentHeading)/abs(heading-currentHeading), abs(heading-currentHeading));
}
Sleep(0.5);
currentHeading = RPS.Heading();
}
}
}
/**
* Completes the boarding pass button task.
*
* @requires
* [Robot is at the upper level of the course]
* @ensures
* [Boarding pass task will be completed]
*/
void boardingPass() {
// Boarding pass nose
moveForward(25, 2.5, 0);
moveForward(-25, 6.5);
turnRightDirect(25, 93);
moveForward(25, 13.5);
Sleep(.5);
if (cdsCell.Value() > .65) { // Blue/red light checker
LCD.Clear();
LCD.SetBackgroundColor(BLUE);
LCD.WriteLine("Blue");
turnRightDirect(25, 90);
moveForward(25, 4.5);
turnLeftDirect(25, 90);
moveForward(40, 1.5, .5);
moveForward(-25, 6.5);
turnRightDirect(25, 90);
moveForward(25, 1.5);
// Update horizontal position for C
if (RPS.CurrentRegionLetter() == 'C') {
moveForward(-25, 0.25);
}
turnLeftDirect(25, 96);
} else {
LCD.Clear();
LCD.SetBackgroundColor(RED);
LCD.WriteLine("Red");
turnRightDirect(25, 90);
moveForward(25, 9.5);
turnLeftDirect(25, 90);
moveForward(40, 1.5, .5);
moveForward(-25, 6.5);
turnLeftDirect(25, 93);
moveForward(25, 2.5);
// Update horizontal position for C
if (RPS.CurrentRegionLetter() == 'C') {
moveForward(25, 0.25);
}
turnRightDirect(25, 90);
}
}
/**
* Completes the fuel lever task.
*
* @requires
* [robot is in position for the task]
* @ensures
* [the fuel lever task is completed]
*/
void fuelLever() {
// 0 - Left, 1 - Middle, 2 - Right
int lever = RPS.GetCorrectLever();
if (lever == 1) {
lever += (0.5/3.5);
}
// Go to correct x-position
moveForward(25, 21.5 + 3.5 * lever);
if(lever == 2) {
moveForward(-25, .2);
}
// Set arm to up position
servoArm.SetDegree(ARM_MAX);
// Go to lever
turnLeftDirect(25, 94);
if(lever == 2) {
turnRightDirect(25, 11);
}
moveForward(-25, 3.0);
// Flip lever and go back
servoArm.SetDegree(21.0);
Sleep(.5);
moveForward(-25, 3);
servoArm.SetDegree(ARM_MIN);
Sleep(.5);
// Flip lever back
moveForward(25, 3);
Sleep(4.5);
servoArm.SetDegree(76.0);
Sleep(.5);
// Reset position
moveForward(-25, 3);
servoArm.SetDegree(ARM_MAX);
if (lever == 2) {
turnLeftDirect(25, 6);
}
}
/**
* Completes the stamp task.
*
* @requires
* [robot is in position near top of slope.]
* @ensures
* [stamp task is completed.]
*/
void stampingPass() {
// Go to boarding pass
servoArm.SetDegree(ARM_MIN-13);
// Move up boarding pass
turnRightDirect(25, 87);
servoArm.SetDegree(ARM_MIN+30);
turnLeftDirect(50, 45);
// Move away from boarding pass
moveForward(-25, 2.0);
servoArm.SetDegree(ARM_MIN-10);
moveForward(-25, 4.0);
servoArm.SetDegree(ARM_MAX);
}
/**
* Completes the luggage drop off task.
*
* @requires
* [robot is in position near top of right ramp]
* @ensures
* [luggage task is completed]
*/
void luggage() {
// Set up position
turnLeftDirect(25, 90*1.15);
moveForward(-30, 3.0, .25);
moveForward(25, 20.0);
turnLeftDirect(25, 90*1.13);
moveForward(25, 1.0, .25);
moveForward(-25, 2.0);
// Set luggage in position and move away
servoArm.SetDegree(30);
Sleep(1.0);
leftMotor.SetPercent(-40);
rightMotor.SetPercent(-40);
Sleep(1.0);
leftMotor.SetPercent(0);
rightMotor.SetPercent(0);
Sleep(.125);
servoArm.SetDegree(ARM_MAX);
moveForward(40, 2.0, .125);
}
int main() {
// For touch screen
float xTouch, yTouch;
// Set servo min and max
servoArm.SetMin(500);
servoArm.SetMax(2267);
servoArm.SetDegree(ARM_MAX+10);
RPS.InitializeTouchMenu();
// Initialize the screen
LCD.Clear(BLACK);
LCD.SetFontColor(WHITE);
LCD.WriteLine("Robot Test");
LCD.WriteLine("Touch the screen");
// Wait for screen to be pressed
while(!LCD.Touch(&xTouch, &yTouch)) {
LCD.WriteLine(cdsCell.Value());
Sleep(.1);
LCD.Clear();
}
// Wait for screen to be unpressed
while(LCD.Touch(&xTouch,&yTouch));
LCD.Clear();
LCD.WriteLine("Waiting for CdS cell signal");
while(cdsCell.Value() > 1.5) {}
servoArm.SetDegree(ARM_MAX);
turnRightDirect(25, 45*1.15);
moveForward(25, 9.0);
turnLeftDirect(25, 90*1.15);
moveForward(-40, 2.0, .125);
moveForward(25, 4.5);
turnRightDirect(25, 90*1.1);
moveForward(25, 28.0);
luggage();
moveForward(-25, 3.0);
turnRightDirect(25, 90);
boardingPass();
stampingPass();
turnRightDirect(25, 135);
moveForward(40, 2.5, .125);
moveForward(-25, 3.0);
turnLeftDirect(25, 90);
moveForward(35, 3.5, .25);
moveForward(-25, 2.5);
turnRightDirect(25, 90);
moveForward(25, 26.0);
turnRightDirect(25, 90);
moveForward(-40, 1.5, .25);
fuelLever();
moveForward(25, 5.5);
turnLeftDirect(25, 90);
moveForward(40, 3.0, 0);
moveForward(-25, 3.0);
turnRightDirect(25, 90);
moveForward(40, 999, 0);
LCD.Clear();
LCD.WriteLine("Test Completed");
return 0;
}