#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/*,float heading*/);
void checkYPosition(float y,int yorient/*,float heading*/);
void checkHeading(float heading);
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;//In Servo7
float shopLightValue;
int buttonPresses;
int main(void)
{
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(180);
setBinDegree(10);
LCD.Clear();
LCD.WriteLine("Ready");
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(),2,12);
LCD.WriteRC(RPS.Y(),3,12);
LCD.WriteRC(RPS.Heading(),4,12);
LCD.WriteRC(CdS.Value(),5,12);
LCD.WriteRC(buttonPresses,6,17);
if(buttons.MiddlePressed())
{
//while(CdS.Value()>.31){};
// driveDistance(20.0,75);
// checkXPosition(1.0,1);
// turnLeft(90);
// checkHeading(0);
driveDistance(25.0,-30);
checkYPosition(17.7,1);
shopLightValue=CdS.Value();
if(shopLightValue<0.45)
{
LCD.WriteRC("Red",7,12);
turnRight(70);
driveDistance(5.0,-75);
checkHeading(135);
checkXPosition(-6.5,1);
turnLeft(45);
//checkHeading(0);
driveDistance(5.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.5,1);
turnLeft(60);
checkHeading(0);
}
else
{
LCD.WriteRC("Blue",7,12);
turnLeft(70);
driveDistance(5.0,-75);
checkHeading(45);
checkXPosition(7.0,-1);
turnRight(45);
//checkHeading(0);
driveDistance(5.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,-1);
turnRight(45);
}
//checkHeading(0);
driveDistance(28.0,90);
checkYPosition(43.8,1);
turnLeft(95);
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(-(2+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.3);
}
void turnRight(int angle)
{
float rotations = (angle/180.0)*26;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(50);
LCD.WriteLine(LEncod.Counts());
LCD.Clear();
}
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.3);
}
void turnLeft(int angle)
{
float rotations = (angle/180.0)*26;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(-50);
LCD.WriteLine(REncod.Counts());
LCD.Clear();
}
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.3);
}
void turnRightSkid(int angle)
{
float rotations = (angle/180.0)*40;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(50);
LCD.WriteLine(LEncod.Counts());
LCD.Clear();
}
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.5);
}
void turnLeftSkid(int angle)
{
float rotations = (angle/180.0)*40;
double startTime = TimeNow();
while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(-50);
LCD.WriteLine(REncod.Counts());
LCD.Clear();
}
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.5);
}
void setFLDegree(int angle)
{
LServo.SetDegree(angle);
RServo.SetDegree(180-angle);
Sleep(1.0);
}
void setBinDegree(int angle)
{
BinServo.SetDegree(angle);
Sleep(1.0);
}
void checkXPosition(float x, int xorient/*, float heading*/)
{
/*while(RPS.Heading()<(heading-3))
{
turnLeft(3);
}
while(RPS.Heading()>(heading+3))
{
turnRight(3);
}*/
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 /*,float heading*/)
{
/*while(RPS.Heading()<(heading-3))
{
turnLeft(5);
}
while(RPS.Heading()>(heading+3))
{
turnRight(5);
}*/
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)
{
float temp=RPS.Heading();
if(heading==0)
{
while(RPS.Heading()>=temp)
{
temp=RPS.Heading();
turnLeft(1);
}
temp=RPS.Heading();
while(RPS.Heading()<=temp)
{
temp=RPS.Heading();
turnRight(1);
}
}
else
{
while(RPS.Heading()<(heading-3))
{
turnLeft(1);
}
while(RPS.Heading()>(heading+3))
{
turnRight(1);
}
}
}