#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
int main(void)
{
RPS.InitializeMenu();
RPS.Enable();
LServo.SetMin(550);
LServo.SetMax(2300);
RServo.SetMin(550);
RServo.SetMax(2200);
BinServo.SetMax(2200);
BinServo.SetMin(600);
setFLDegree(180);
LCD.Clear();
LCD.WriteLine("Ready");
LCD.WriteLine("RPS Data");
LCD.WriteLine("X-Position: ");
LCD.WriteLine("Y-Position: ");
LCD.WriteLine("Heading: ");
while(1==1)
{
LCD.WriteRC(RPS.X(),1,12);
LCD.WriteRC(RPS.Y(),2,12);
LCD.WriteRC(RPS.Heading(),3,12);
if(buttons.MiddlePressed())
{
while(CdS.Value()>.31){};
driveDistance(31.0,50);
checkXPosition(5.5,1,90);
turnLeft(83);
checkHeading(173);
setFLDegree(110);
setFLDegree(35);
driveDistance(15.0,50);
setFLDegree(150);
driveDistance(7.0,-50);
turnLeftSkid(90);
driveDistance(9.0,50);
checkXPosition(0,-1.5,90.0);
turnLeftSkid(90);
checkHeading(170);
driveDistance(20.0,50);
setFLDegree(50);
turnRightSkid(90);
checkHeading(90);
driveDistance(5.0,50);
turnRightSkid(90);
checkHeading(0);
setFLDegree(30);
driveDistance(5.0,-50);
}
}
}
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(-(5+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.5);
}
void turnRight(int angle)
{
float rotations = (angle/180.0)*19;
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 turnLeft(int angle)
{
float rotations = (angle/180.0)*21;
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 turnRightSkid(int angle)
{
float rotations = (angle/180.0)*70;
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)*70;
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-10))
{
turnLeft(5);
}
while(RPS.Heading()>(heading+10))
{
turnRight(5);
}
if(xorient==-1)
{
while(RPS.X()<(x-1.0))
{
driveDistance(1.0,-50);
}
while(RPS.X()>(x+1.0))
{
driveDistance(1.0,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-10))
{
turnLeft(5);
}
while(RPS.Heading()>(heading+10))
{
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)
{
while(RPS.Heading()<(heading-10))
{
turnLeft(5);
}
while(RPS.Heading()>(heading+10))
{
turnRight(5);
}
}