#include <FEHMotor.h>
#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHServo.h>
#include <LCDColors.h>
#include <FEHRPS.h>
#define PI 3.14159265
#define ENCODER_LINES 16
#define ENCODER_COUNT 4
#define WHEEL_BASE 3.625
#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);
float x_coord;
float y_coord;
void driveStraight(float distance);
void driveStraight(float distance, float motor);
void turnPivot(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.75, 2.75);
leftEncoder.SetThresholds(2.75, 2.75);
//Test the right turnIndefinitely
//Go line by line and dont move the robot off the course
//Wait for the user to orient robot
while(!LCD.Touch(&x_coord, &y_coord)){
Sleep(500);
LCD.Write("XCOORD: ");
LCD.WriteLine(RPS.X());
LCD.Write("YCOORD: ");
LCD.WriteLine(RPS.Y());
LCD.Write("HEADING: ");
LCD.WriteLine(RPS.Heading());
Sleep(3.0);
LCD.Clear();
}
while(cdsCell.Value() > 1){}
LCD.WriteLine("Drive out of starting line");
driveStraight(1.0);
Sleep(300);
while(26 - RPS.Y() < -.5){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
rightMotor.Stop();
leftMotor.Stop();
LCD.WriteLine("Pivot counter clockwise on left wheel");
turnPivot(80, false);
Sleep(300);
while(179 - RPS.Heading() > 1){
rightMotor.SetPercent(40);
leftMotor.SetPercent(0);
Sleep(100);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(180-RPS.Heading());
}
rightMotor.Stop();
leftMotor.Stop();
LCD.WriteLine("DRIVE STRAIGHT");
LCD.WriteLine("Approach control panel");
driveStraight(7);
Sleep(300);
while(26.3 - RPS.X() > .5){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
if(cdsCell.Value() < .2){
LCD.WriteLine(cdsCell.Value());
LCD.WriteLine("RED BUTTON");
LCD.WriteLine("Hit red button");
driveStraight(-10);
while(14.4 - RPS.X() < -.5){
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
LCD.WriteLine("Run into button");
Sleep(300);
turnPivot(95, true);
float time = TimeNow();
while(90 - RPS.Heading() < -1 && TimeNow()-time < 5){
rightMotor.SetPercent(0);
leftMotor.SetPercent(60);
Sleep(300);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(90-RPS.Heading());
}
LCD.WriteLine("Turn out of button");
Sleep(300);
while(180 - RPS.Heading() > 1){ //off by 40 degrees for some reason
rightMotor.SetPercent(0);
leftMotor.SetPercent(-40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(180-RPS.Heading());
}
}else{
LCD.WriteLine(cdsCell.Value());
LCD.WriteLine("BLUE BUTTON NAMRATA");
LCD.WriteLine("Hit blue button");
driveStraight(-4);
while(20.8 - RPS.X() < -.5){
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
Sleep(300);
Heading = RPS.Heading();
LCD.WriteLine("Run into button");
turnPivot(95, true);
time = TimeNow();
while(90 - RPS.Heading() < -1 && TimeNow()-time < 6){
rightMotor.SetPercent(0);
leftMotor.SetPercent(60);
Sleep(300);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(180-Heading);
}
Sleep(300);
LCD.WriteLine("Turn out of button");
while(180 - RPS.Heading() > 1){
rightMotor.SetPercent(0);
leftMotor.SetPercent(-40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(180-RPS.Heading());
}
}
LCD.WriteLine("Hit wrench");
Sleep(300);
driveStraight(-20);
time = TimeNow();
while(6 - RPS.X() < -.5 && TimeNow()-time <5){
rightMotor.SetPercent(-40);
leftMotor.SetPercent(-40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
LCD.WriteLine("Move away from motor");
Sleep(300);
driveStraight(5);
time = TimeNow();
while(10 - RPS.X() > .5 && TimeNow()-time <11){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
LCD.WriteLine("Turn towards red button");
time = TimeNow();
while(250 - RPS.Heading() > 1 && TimeNow()-time < 9){
rightMotor.SetPercent(40);
leftMotor.SetPercent(0);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
LCD.WriteLine(250-Heading);
}
LCD.WriteLine("Run into red button");
rightMotor.SetPercent(80);
leftMotor.SetPercent(80);
Sleep(4.0);
time = TimeNow();
while(TimeNow()-time < 10){
rightMotor.SetPercent(40);
leftMotor.SetPercent(40);
Sleep(150);
rightMotor.Stop();
leftMotor.Stop();
Sleep(200);
}
return 0;
}
/*
Method that turns the robot a certain angle.
*/
void turnPivot(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 straight a certain distance.
*/
void driveStraight(float distance){
float count = distance * ENCODER_LINES / (2.5 * PI);
float kp = 1;
if(count > 0){
while(leftEncoder.Counts() < count && rightEncoder.Counts() < count){
leftMotor.SetPercent(50);
rightMotor.SetPercent(50*kp);
Sleep(500);
}
}else{
count *= -1;
while(leftEncoder.Counts() < count && rightEncoder.Counts() < count){
leftMotor.SetPercent(-50);
rightMotor.SetPercent(-50*kp);
Sleep(500);
}
}
rightMotor.Stop();
leftMotor.Stop();
leftEncoder.ResetCounts();
rightEncoder.ResetCounts();
}
/*
Method that drives the robot straight a certain distance at a certain speed.
*/
void driveStraight(float distance, float motorSpeed){
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();
}