#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHMotor.h>
#include <FEHServo.h>
int main(void)
{
FEHMotor dcmotorL(FEHMotor::Motor0);
FEHMotor dcmotorR(FEHMotor::Motor1);
ButtonBoard buttons(FEHIO::Bank3);
AnalogInputPin CdS(FEHIO::P0_0);
FEHEncoder LEncod(FEHIO::P2_0);
FEHEncoder REncod(FEHIO::P1_0);
FEHServo LServo(FEHServo::Servo7);
FEHServo RServo(FEHServo::Servo0);
LServo.SetMin(525);
LServo.SetMax(2400);
RServo.SetMin(525);
RServo.SetMax(2400);
while(1==1)
{
if(buttons.MiddlePressed())
{
while(1==1)
{
if(CdS.Value()<0.35)
{
LServo.SetDegree(180);
RServo.SetDegree(0);
Sleep(1.0);
while(REncod.Counts()<7)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
while(REncod.Counts()<8)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
while(REncod.Counts()<4)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(-50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
while(REncod.Counts()<58)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
while(REncod.Counts()<9)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(-50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
while(REncod.Counts()<42)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
while(REncod.Counts()<28)
{
dcmotorL.SetPercent(50);
dcmotorR.SetPercent(50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
LServo.SetDegree(100);
RServo.SetDegree(80);
while(REncod.Counts()<40)
{
dcmotorL.SetPercent(-50);
dcmotorR.SetPercent(50);
}
dcmotorL.SetPercent(0);
dcmotorR.SetPercent(0);
Sleep(0.5);
LCD.Clear();
LCD.WriteLine(LEncod.Counts());
LCD.WriteLine(REncod.Counts());
LEncod.ResetCounts();
REncod.ResetCounts();
}
}
}
}
}