2/20/14 – Code (Version 4.0)

#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();


                }
            }

        }
    }

}

			

Leave a Reply

Your email address will not be published. Required fields are marked *