#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 69.46853775
#define DEG_PER_SECONDR 1.05
#define INCH_PER_SECOND 9.212081
#define TOLERANCE .25
//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);
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 pivotTurn(float angle, bool clockwise);
void pivotTurnBack(float angle, bool clockwise);
int main(){
RPS.InitializeTouchMenu();
float XPosition = RPS.X();
float YPosition = RPS.Y();
float Heading = RPS.Heading();
float time;
//while(cdsCell.Value() > .5){}
//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(500);
LCD.Write("DESIRED 17.3 XCOORD: ");
LCD.WriteLine(RPS.X());
LCD.Write("DESIRED 28.2 YCOORD: ");
LCD.WriteLine(RPS.Y());
LCD.Write("DESIRED 270 HEADING: ");
LCD.WriteLine(RPS.Heading());
Sleep(3.0);
LCD.Clear();
}
while(cdsCell.Value() > 1){
LCD.WriteLine("Waiting for start");
}
LCD.Clear();
arm.SetMin(690);
arm.SetMax(2200);
arm.SetDegree(180);
arm.SetDegree(0);
gear.SetMin(621);
gear.SetMax(2367);
//Drive forward
driveStraight(2);
check_y_minus(24.6);
LCD.WriteLine(RPS.Y());
//Turn left
pivotTurn(45, false);
check_heading(347);
LCD.WriteLine(RPS.Heading());
//Turn Right
LCD.WriteLine("Run into button");
Sleep(300);
time = TimeNow();
while(TimeNow()-time < 7){
if(TimeNow()-time < 3){
rightMotor.SetPercent(0);
leftMotor.SetPercent(80);
}else{
rightMotor.SetPercent(0);
leftMotor.SetPercent(40);
}
}
rightMotor.Stop();
leftMotor.Stop();
rightEncoder.ResetCounts();
leftEncoder.ResetCounts();
//Turn out of white
pivotTurnBack(65, false);
check_back_heading(347);
//Reverse
pivotTurnBack(40, true);
check_back_heading(270);
arm.SetDegree(180);
//Turn right
pivotTurn(40, true);
check_heading(225);
//Go Straight
driveStraight(2);
check_y_minus(18.9);
//Turn
pivotTurn(40, true);
check_heading(180);
//Drive straight into wrench
driveStraight(1.5);
check_x_minus(6.8);
//Adjust to align w wrench
check_heading(180);
//Drive into wrench
check_x_minus(6.1);
//Lift Wrench
arm.SetDegree(0);
Sleep(1.0);
//Go back
driveStraight(-2);
check_x_minus(12);
//Turnright
pivotTurnBack(45, true);
check_back_heading(135);
//drive forward
driveStraight(4);
check_x_minus(10.9);
//turn left
pivotTurn(80, false);
check_heading(235);
//turn more left
pivotTurnBack(35, false);
check_heading(270);
//Drive up hill
driveStraight(-25, 90);
check_y_minus(42);
//turn right
pivotTurnBack(45, true);
check_back_heading(225);
driveStraightIndefinitely(-50);
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();
}
/*
A method that turns the robot a certain angle. Implemented
with encoders.
/*
void pivotTurn(float angle, bool clockwise){
float distance = angle / 360 * PI * 2 * (2 * WHEEL_BASE);
float count = distance * ENCODER_LINES / (2.5 * PI);
if(clockwise){
while(leftEncoder.Counts() < count){
leftMotor.SetPercent(60);
}
}else{
while(rightEncoder.Counts() < count-2){
rightMotor.SetPercent(60);
}
}
LCD.WriteLine(leftEncoder.Counts());
LCD.WriteLine(rightEncoder.Counts());
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
rightMotor.Stop();
leftMotor.Stop();
}
/*
Method that drives the robot a distance at a certain speed.
Implemented with encoders.
*/
void driveStraight(float distance, float motorSpeed){
float count = distance * ENCODER_LINES / (2.5 * PI);
if(count > 0){
while(leftEncoder.Counts() < count && rightEncoder.Counts() < count){
leftMotor.SetPercent(motorSpeed+2);
rightMotor.SetPercent(motorSpeed);
}
}else{
count *= -1;
while(leftEncoder.Counts() < count && rightEncoder.Counts() < count){
leftMotor.SetPercent(-motorSpeed-7);
rightMotor.SetPercent(-motorSpeed);
}
}
rightMotor.Stop();
leftMotor.Stop();
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
}
/*
Method that drives robot a distance. Implemented with encoders.
*/
void driveStraight(float distance){
float count = distance * ENCODER_LINES / (2.5 * PI);
if(count > 0){
while(leftEncoder.Counts() < count && rightEncoder.Counts() < count){
leftMotor.SetPercent(57);
rightMotor.SetPercent(50);
}
}else{
count *= -1;
while(leftEncoder.Counts() < count && rightEncoder.Counts() < count){
leftMotor.SetPercent(-57);
rightMotor.SetPercent(-50);
}
}
rightMotor.Stop();
leftMotor.Stop();
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
}