#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);
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(600);
LServo.SetMax(2200);
RServo.SetMin(600);
RServo.SetMax(2200);
BinServo.SetMax(2200);
BinServo.SetMin(600);
setFLDegree(160);
LCD.WriteLine("Ready");
LCD.WriteLine("RPS Data");
LCD.WriteLine("X-Position: ");
LCD.WriteLine("Y-Position: ");
LCD.WriteLine("Heading: ");
while(1==1)
{
if(buttons.MiddlePressed())
{
setFLDegree(110);
driveDistance(36.0,50);
driveDistance(5.0,-50);
setFLDegree(45);
driveDistance(5.0,50);
setFLDegree(130);
//BONUS
// driveDistance(2.0,-50);
// turnLeft(90);
// setFLDegree(10);
// driveDistance(10.0);
// setFLDegree(135);
// driveDistance(10.0,-50);
// turnRight(90); END BONUS
driveDistance(1.2,-50);
driveDistance(1.0,50);
driveDistance(1.0,-50);
setFLDegree(45);
driveDistance(5.2,-50);
turnLeft(90);
driveDistance(20.0,-50);
}
}
}
void driveDistance(float distance,int power)
{
float rotations=distance*3.15;
while(REncod.Counts()<rotations&&LEncod.Counts()<rotations)
{
dcmotorL.SetPercent(-power);
dcmotorR.SetPercent(-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)*118;
while(LEncod.Counts()<rotations)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(0);
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)*110;
while(LEncod.Counts()<rotations)
{
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 checkPinApproach()
//{
// while(RPS.X()<5.0)
// {
// driveDistance(0.5,50);
// }
// while(RPS.Y()<)
//}