#include <FEHMotor.h>
#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHServo.h>
#include <LCDColors.h>
#include <FEHRPS.h>
#include <cmath>
#define PI 3.14159265
#define ENCODER_LINES 16
#define ENCODER_COUNT 4
#define WHEEL_BASE 3.625
#define MOTOR_CONSTANT 1
#define DEG_PER_SECOND 67.1743621
#define DEG_PER_SECONDR 1.05
#define INCH_PER_SECOND 8.5
#define TOLERANCE .25
#define TIMEOUT 4
//Create variable for servo arm
FEHServo arm(FEHServo::Servo0);
//Create variable for motors
FEHMotor leftMotor(FEHMotor::Motor0, 12);
FEHMotor rightMotor(FEHMotor::Motor1, 12);
//Create variables for encoders
AnalogEncoder leftEncoder(FEHIO::P0_0);
AnalogEncoder rightEncoder(FEHIO::P0_2);
//Create variable for servo gear
FEHServo gear(FEHServo::Servo2);
//Create variable for cdsCell
AnalogInputPin cdsCell(FEHIO::P3_7);
//Create variable for line detectors
AnalogInputPin rightSensor(FEHIO::P2_2);
AnalogInputPin centerSensor(FEHIO::P2_1);
AnalogInputPin leftSensor(FEHIO::P2_0);
float x_coord;
float y_coord;
void driveStraight(float distance);
void driveStraight(float distance, float motor);
void check_y_minus(float y);
void check_y_plus(float y);
void check_x_minus(float x);
void check_x_plus(float x);
void check_heading(float angle);
void check_back_heading(float angle);
void turn(float angle, bool clockwise);
void turnPivot(float angle, bool clockwise);
void turnPivotBack(float angle, bool clockwise);
void turnPivotBackIndefinitely(float angle, bool clockwise);
void turnIndefinitely(float angle, bool clockwise);
void turnPivotIndefinitely(float angle, bool clockwise);
void driveStraightIndefinitely(float distance);
void driveUpHillIndefinitely(float distance);
int main(){
RPS.InitializeTouchMenu();
float time;
//Instantiate proper encoder Value
//Not necessary until encoders come in
rightEncoder.SetThresholds(2.8, 2.8);
leftEncoder.SetThresholds(2.8, 2.8);
//Wait for the user to orient robot
while(!LCD.Touch(&x_coord, &y_coord)){
Sleep(200);
LCD.Write("DESIRED 18.4 XCOORD: ");
LCD.WriteLine(RPS.X());
LCD.Write("DESIRED 28.3 YCOORD: ");
LCD.WriteLine(RPS.Y());
LCD.Write("DESIRED 270 HEADING: ");
LCD.WriteLine(RPS.Heading());
LCD.Write("CDS Value: ");
LCD.WriteLine(cdsCell.Value());
Sleep(3.0);
LCD.Clear();
}
//Wait for starting light
float timeStart = TimeNow();
while((cdsCell.Value() > 1) && ((TimeNow() - timeStart) < 30)){
LCD.WriteLine("Waiting for start");
}
LCD.Clear();
//Set servo arm values
arm.SetMin(690);
arm.SetMax(2200);
arm.SetDegree(180);
arm.SetDegree(0);
gear.SetMin(621);
gear.SetMax(2367);
if (RPS.FuelType() == 1)//clockwise
{
gear.SetDegree(180);
} else
{
gear.SetDegree(0);
}
//Drive forward
driveStraightIndefinitely(2);
check_y_minus(26);
check_y_minus(26);//27 26.5
LCD.WriteLine(RPS.Y());
//Turn left
turnPivotIndefinitely(80, false);
/*Check heading to 1*/
while((RPS.Heading() < 359) && (RPS.Heading() > 1)){
rightMotor.SetPercent(40);
leftMotor.SetPercent(0);
Sleep(100);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(RPS.Heading());
}
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
/*End of check heading*/
//Drive over light
driveStraightIndefinitely(5);
check_x_plus(26.5);
check_x_plus(26.5);
arm.SetDegree(170);
//If blue
LCD.Write("CDS = ");
LCD.WriteLine(cdsCell.Value());
if(cdsCell.Value() > .8){
LCD.WriteLine("BLUE");
Sleep(500);
driveStraightIndefinitely(-.5);
}else{
LCD.WriteLine("RED");
driveStraightIndefinitely(-2.5);
}
turnIndefinitely(100, true);
while(RPS.Heading() < 269 || RPS.Heading() > 271)
check_heading(270);
driveStraightIndefinitely(1);
check_heading(270);
check_y_minus(18.8);
driveStraightIndefinitely(2);
float time1 = TimeNow();
while((RPS.IsDeadzoneActive() != 2) && ((TimeNow()-time1) < 8.0)){
rightMotor.SetPercent(50);
leftMotor.SetPercent(50);
}
rightMotor.Stop();
leftMotor.Stop();
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
//back out of button
driveStraightIndefinitely(-3);
check_y_minus(23.8);
//double pivot to parallel with control panel
turnIndefinitely(90, true);
while(RPS.Heading() < 179 || RPS.Heading() > 181)
check_heading(180);
//drive forward a bit for clearance
driveStraightIndefinitely(8);
Sleep(1.0);
check_back_heading(180);
LCD.WriteLine("Going to 190");
check_back_heading(190);
//Calc angle and distance to wrench
float x_current = RPS.X();
float y_current = RPS.Y();
float angle1;
float dist1 = std::sqrt((16.9-y_current)*(16.9-y_current)+
(9-x_current)*(9-x_current));
for(int i = 0; i < 2; i++){
angle1 = (std::atan((16.9-RPS.Y())/(9-RPS.X())))*180/PI;
angle1 += 188;
check_heading(angle1);
LCD.WriteLine(angle1);
driveStraightIndefinitely((dist1-.5)/2);
}
//Turn to face wrench
check_heading(183.1);
//drive into wrench
driveStraightIndefinitely(1);
check_x_minus(6.1);
//lift wrench
arm.SetDegree(10);
Sleep(500);
//AFTER WRENCH
//Go back
driveStraightIndefinitely(-2);
check_x_minus(8.4);
//back pivot on right wheel
turnPivotBackIndefinitely(90, false);
check_heading(270);
//drive straight
driveStraightIndefinitely(5.5);
check_y_minus(15.7);
//pivot turn on left wheel into jack
turnPivotIndefinitely(80, false);
check_heading(350);
//pivot out of jack
turnPivotBackIndefinitely(60,true);
check_back_heading(270);
//aim at final button
turnPivotBackIndefinitely(45, true);
check_back_heading(225);
//drive toward button
driveStraightIndefinitely(-1);
//turn into button
turnPivotBackIndefinitely(45, false);
check_back_heading(270);
//hit button
driveStraightIndefinitely(-10);
return 0;
}
/*
Method that pivots robot in a specified direction on the back
wheel.
*/
void turnPivotBack(float angle, bool clockwise){
float distance = angle / 360 * PI * 2 * (2 * WHEEL_BASE);
float count = distance * ENCODER_LINES / (2.5 * PI);
if(clockwise){
while(rightEncoder.Counts() < count){
rightMotor.SetPercent(-60);
}
}else{
while(leftEncoder.Counts() < count-2){ leftMotor.SetPercent(-60); } } LCD.WriteLine(leftEncoder.Counts()); LCD.WriteLine(rightEncoder.Counts()); leftEncoder.ResetCounts(); rightEncoder.ResetCounts(); rightMotor.Stop(); leftMotor.Stop(); } /* Method that checks robots heading and adjusts with back pivots. */ void check_back_heading(float angle){ float time = TimeNow(); LCD.WriteLine("Aligning heading"); while((angle - RPS.Heading() > 2 || angle - RPS.Heading() < -2) && TimeNow()-time < TIMEOUT){ Sleep(300); if(angle - RPS.Heading() > 2){
rightMotor.SetPercent(0);
leftMotor.SetPercent(-40);
}else{
rightMotor.SetPercent(-60);
leftMotor.SetPercent(0);
}
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
}
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that checks y-coordinate of robot facing negative-y direction.
*/
void check_y_minus(float y){
LCD.WriteLine("Driving into negative y");
while(y - RPS.Y() < -TOLERANCE || y - RPS.Y() > TOLERANCE){
Sleep(300);
if(y - RPS.Y() < -TOLERANCE){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
}else{
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
}
Sleep(100);
rightMotor.Stop();
leftMotor.Stop();
}
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that checks y-coordinate of robot facing positive-y direction.
*/
void check_y_plus(float y){
LCD.WriteLine("Driving into positive y");
while(y - RPS.Y() > TOLERANCE || y - RPS.Y() < TOLERANCE){
Sleep(100);
if(y - RPS.Y() > TOLERANCE){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
}else{
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
}
Sleep(100);
rightMotor.Stop();
leftMotor.Stop();
}
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that checks x-coordinate of robot facing negative-x direction.
*/
void check_x_minus(float x){
LCD.WriteLine("Driving into negative x");
while(x - RPS.X() < -TOLERANCE || x - RPS.X() > TOLERANCE){
Sleep(200);
if(x - RPS.X() < -TOLERANCE){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
}else{
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
}
Sleep(100);
rightMotor.Stop();
leftMotor.Stop();
}
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that checks x-coordinate of robot facing positive-x direction.
*/
void check_x_plus(float x){
LCD.WriteLine("Driving into positive x");
float time = TimeNow();
while(x - RPS.X() > TOLERANCE || x - RPS.X() < TOLERANCE && TimeNow() - time < 2){
Sleep(300);
if(x - RPS.X() > TOLERANCE){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
}else{
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
}
Sleep(100);
rightMotor.Stop();
leftMotor.Stop();
LCD.WriteLine(x-RPS.X());
}
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that refines the heading of the robot using pivot turns.
*/
void check_heading(float angle){
float time = TimeNow();
LCD.WriteLine("Aligning heading");
while((angle - RPS.Heading() > 2 || angle - RPS.Heading() < -2) && TimeNow()-time < TIMEOUT){ Sleep(300); if(angle - RPS.Heading() > 2){
rightMotor.SetPercent(50); //40
leftMotor.SetPercent(0);
}else{
rightMotor.SetPercent(0);
leftMotor.SetPercent(50); //70
}
Sleep(100); //150
rightMotor.Stop();
leftMotor.Stop();
}
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that drives robot a distance. Implemented with TimeNow().
*/
void driveStraightIndefinitely(float distance){
float seconds = distance/INCH_PER_SECOND;
float time = TimeNow();
if(seconds < 0){
seconds*= -1;
while(TimeNow() - time < seconds){
leftMotor.SetPercent(-60);
rightMotor.SetPercent(-60);
}
}else{
while(TimeNow() - time < seconds){
leftMotor.SetPercent(60);
rightMotor.SetPercent(60);
}
}
rightMotor.Stop();
leftMotor.Stop();
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
}
/*
Method that drives a robot uphill. Implemented with TimeNow().
*/
void driveUpHillIndefinitely(float distance){
float seconds = distance/INCH_PER_SECOND;
float time = TimeNow();
if(seconds < 0){
seconds*= -1;
while(TimeNow() - time < seconds){
leftMotor.SetPercent(-80);
rightMotor.SetPercent(-80);
}
}else{
while(TimeNow() - time < seconds){
leftMotor.SetPercent(80);
rightMotor.SetPercent(80);
}
}
rightMotor.Stop();
leftMotor.Stop();
}
/*
Makes a point turn on a specified angle in a certain direction.
Implemented using the TimeNow() Function.
*/
void turnIndefinitely(float angle, bool clockwise){
float time = TimeNow();
if(clockwise){
float seconds = angle/(DEG_PER_SECOND)/2;
while(TimeNow() - time < seconds){
leftMotor.SetPercent(60);
rightMotor.SetPercent(-60);
}
leftMotor.Stop();
rightMotor.Stop();
}else{
float seconds = angle/DEG_PER_SECOND/2;
while(TimeNow() - time < seconds){
rightMotor.SetPercent(60);
leftMotor.SetPercent(-60);
}
rightMotor.Stop();
leftMotor.Stop();
}
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
rightMotor.Stop();
leftMotor.Stop();
}
/*
Makes a pivot turn on a specified angle in a certain direction.
Implemented using the TimeNow() Function.
*/
void turnPivotIndefinitely(float angle, bool clockwise){
float seconds = angle/DEG_PER_SECOND;
float time = TimeNow();
if(clockwise){
while(TimeNow() - time < seconds){
leftMotor.SetPercent(60);
rightMotor.SetPercent(0);
}
leftMotor.Stop();
}else{
while(TimeNow() - time < seconds){
rightMotor.SetPercent(60);
leftMotor.SetPercent(0);
}
rightMotor.Stop();
}
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
rightMotor.Stop();
leftMotor.Stop();
}