#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHServo.h>
#include <FEHWONKA.h>
void driveDistance(float distance,int power);
void turnLeft(int angle);
void turnRight(int angle);
void setFLDegree(int angle);
void setBinDegree(int angle);
void turnLeftSkid(int angle);
void turnRightSkid(int angle);
void checkXPosition(float x,int xorient);
void checkYPosition(float y,int yorient);
void checkHeading(float heading);
void depositScoop();
void getSkid();
void getPin();
void flipSwitch();
void upRamp();
void pressButton();
FEHMotor dcmotorL(FEHMotor::Motor0);
FEHMotor dcmotorR(FEHMotor::Motor3);
ButtonBoard buttons(FEHIO::Bank3);
AnalogInputPin CdS(FEHIO::P0_0);
FEHEncoder LEncod(FEHIO::P2_0);
FEHEncoder REncod(FEHIO::P1_0);
FEHServo LServo(FEHServo::Servo5);
FEHServo RServo(FEHServo::Servo0);
FEHServo BinServo(FEHServo::Servo3);
FEHWONKA RPS;
float shopLightValue;
int buttonPresses;
int main(void)
{
REncod.SetThresholds(1.9,2.2);
RPS.InitializeMenu();
RPS.Enable();
buttonPresses=RPS.Oven();
LServo.SetMin(550);
LServo.SetMax(2300);
RServo.SetMin(550);
RServo.SetMax(2200);
BinServo.SetMax(2200);
BinServo.SetMin(600);
setFLDegree(160);
setBinDegree(10);
LCD.Clear();
LCD.WriteLine("RPS DATA:");
LCD.WriteLine("X-Position: ");
LCD.WriteLine("Y-Position: ");
LCD.WriteLine("Heading: ");
LCD.WriteLine("CdS Value: ");
LCD.WriteLine("Button Presses: ");
LCD.WriteLine("Store Light: ");
while(1==1)
{
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
LCD.WriteRC(CdS.Value(),4,12);
LCD.WriteRC(buttonPresses,5,17);
if(buttons.MiddlePressed())
{
while(CdS.Value()>.75){};
driveDistance(33.0,75);
getPin();
getSkid();
setFLDegree(100);
turnRight(90);
checkHeading(90);
driveDistance(7.0,75);
checkXPosition(0.8,1);
turnLeft(90);
checkHeading(0);
depositScoop();
upRamp();
flipSwitch();
pressButton();
setFLDegree(160);
turnLeft(90);
checkHeading(0);
checkYPosition(43.8,1);
turnLeft(90);
checkHeading(90);
driveDistance(10,75);
checkHeading(100);
driveDistance(18,90);
}
}
}
void driveDistance(float distance,int power)
{
float rotations=distance*3.15;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&LEncod.Counts()<rotations&&(TimeNow()-startTime)<6.0)
{
dcmotorL.SetPercent(power);
dcmotorR.SetPercent(10+power);
}
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
LEncod.ResetCounts();
REncod.ResetCounts();
Sleep(0.2);
}
void turnRight(int angle)
{
float rotations = (angle/180.0)*40;
double startTime = TimeNow();
while(LEncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(-50);
}
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
LEncod.ResetCounts();
REncod.ResetCounts();
Sleep(0.2);
}
void turnLeft(int angle)
{
float rotations = (angle/180.0)*35;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(50);
}
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
LEncod.ResetCounts();
REncod.ResetCounts();
Sleep(0.2);
}
void turnRightSkid(int angle)
{
float rotations = (angle/180.0)*65;
double startTime = TimeNow();
while(LEncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(-50);
}
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
LEncod.ResetCounts();
REncod.ResetCounts();
Sleep(0.2);
}
void turnLeftSkid(int angle)
{
float rotations = (angle/180.0)*60;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(50);
}
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
LEncod.ResetCounts();
REncod.ResetCounts();
Sleep(0.2);
}
void setFLDegree(int angle)
{
LServo.SetDegree(angle);
RServo.SetDegree(180-angle);
Sleep(0.5);
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
}
void setBinDegree(int angle)
{
BinServo.SetDegree(angle);
Sleep(0.5);
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
}
void checkXPosition(float x, int xorient)
{
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
if(xorient==-1)
{
while(RPS.X()<(x-1.0))
{
driveDistance(0.5,-50);
}
while(RPS.X()>(x+1.0))
{
driveDistance(0.5,50);
}
}
if(xorient==1)
{
while(RPS.X()<(x-1.0))
{
driveDistance(1.0,50);
}
while(RPS.X()>(x+1.0))
{
driveDistance(1.0,-50);
}
}
}
void checkYPosition(float y, int yorient)
{
if(yorient==-1)
{
while(RPS.Y()<(y-1.0))
{
driveDistance(1.0,-50);
}
while(RPS.Y()>(y+1.0))
{
driveDistance(1.0,50);
}
}
if(yorient==1)
{
while(RPS.Y()<(y-1.0))
{
driveDistance(1.0,50);
}
while(RPS.Y()>(y+1.0))
{
driveDistance(1.0,-50);
}
}
}
void checkHeading(float heading)
{
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
float temp=RPS.Heading();
if(heading==0)
{
while(RPS.Heading()>3&&RPS.Heading()<177)
{
if(RPS.Heading()>90)
{
if(RPS.Heading()<177)
{
turnLeft(1);
}
}
else
{
if(RPS.Heading()>3)
{
turnRight(1);
}
}
}
}
else
{
while(RPS.Heading()<(heading-3))
{
turnLeft(1);
}
while(RPS.Heading()>(heading+3))
{
turnRight(1);
}
}
}
void depositScoop()
{
checkYPosition(17.7,1);
setFLDegree(170);
shopLightValue=CdS.Value();
if(shopLightValue<0.5)
{
LCD.WriteRC("Red",6,12);
turnRight(70);
driveDistance(5.0,-75);
checkHeading(135);
checkXPosition(-6.2,1);
turnLeft(55);
checkHeading(0);
driveDistance(6.0,-75);
setBinDegree(120);
driveDistance(5.0,75);
driveDistance(6.0,-75);
driveDistance(2.0,75);
turnRight(45);
checkHeading(135);
driveDistance(5.0,75);
checkXPosition(0.8,1);
turnLeft(60);
checkHeading(0);
turnLeft(10);
}
else
{
LCD.WriteRC("Blue",6,12);
turnLeft(70);
driveDistance(5.0,-75);
checkHeading(45);
checkXPosition(7.0,-1);
turnRight(45);
checkHeading(0);
driveDistance(6.0,-75);
setBinDegree(120);
driveDistance(5.0,75);
driveDistance(6.0,-75);
driveDistance(2.0,75);
turnLeft(45);
checkHeading(45);
driveDistance(5.0,75);
checkXPosition(-1.3,-1);
turnRight(45);
checkHeading(0);
turnLeft(15);
}
}
void getSkid()
{
checkHeading(90);
checkXPosition(5.5,1);
turnLeft(90);
checkHeading(165);
setFLDegree(110);
setFLDegree(32);
driveDistance(15.0,75);
setFLDegree(130);
driveDistance(10.0,-75);
turnLeftSkid(90);
checkHeading(90);
driveDistance(9.0,75);
checkHeading(90);
checkXPosition(.5,-1);
turnLeftSkid(90);
checkHeading(0);
driveDistance(32.0,50);
driveDistance(3.0,-75);
turnRightSkid(90);
checkHeading(90);
driveDistance(5.0,75);
checkHeading(90);
checkXPosition(-7.0,-1);
turnRightSkid(60);
checkHeading(10);
setFLDegree(30);
driveDistance(13.0,75);
driveDistance(5.0,-75);
}
void getPin()
{
turnLeft(90);
checkHeading(0);
checkYPosition(46.1,1);
turnRight(90);
checkHeading(90);
checkXPosition(5.5,1);
setFLDegree(45);
driveDistance(2.0,50);
checkXPosition(6.6,1);
setFLDegree(130);
driveDistance(2.2,-50);
setFLDegree(45);
driveDistance(2.2,50);
setFLDegree(130);
driveDistance(1.0,-50);
setFLDegree(40);
}
void flipSwitch()
{
setFLDegree(90);
driveDistance(5.0,75);
checkYPosition(49.3,1);
setFLDegree(95);
turnLeft(90);
driveDistance(5.0,-75);
}
void upRamp()
{
checkHeading(0);
setFLDegree(30);
driveDistance(28.0,90);
checkYPosition(43.8,1);
setFLDegree(160);
}
void pressButton()
{
checkHeading(0);
checkYPosition(43.1,1);
turnLeft(90);
checkHeading(90);
setFLDegree(90);
driveDistance(10.0,75);
for(int i=0;i<buttonPresses;i++)
{
checkXPosition(-19,-1);
driveDistance(2,75);
driveDistance(2,-75);
}
driveDistance(10,-75);
setFLDegree(160);
}