2/24/14 – Code (Version 5.0)

#include <FEHLCD.h>

#include <FEHIO.h>

#include <FEHUtility.h>

#include <FEHMotor.h>

#include <FEHServo.h>

void driveDistance(float distance,int power);

void turnLeft(int power, int angle);

void turnRight(int power, int angle);

void setFLDegree(int angle);

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

int main(void)

{

    LServo.SetMin(525);

    LServo.SetMax(2400);

    RServo.SetMin(525);

    RServo.SetMax(2400);

    while(1==1)

    {

        if(buttons.MiddlePressed())

        {

            setFLDegree(180);

            while(1==1)

            {

                if(CdS.Value()<0.35)

                {

                    driveDistance(6.0,75);

                    turnLeft(75,90);

                    setFLDegree(20);

                }

            }

        }

    }

}

void driveDistance(float distance,int power)

{

    float rotations=distance*3.1831;

    while(REncod.Counts()<rotations&&LEncod.Counts()<rotations)

    {

        dcmotorL.SetPercent(power);

        dcmotorR.SetPercent(power);

    }

    dcmotorL.SetPercent(0);

    dcmotorR.SetPercent(0);

    LEncod.ResetCounts();

    REncod.ResetCounts();

    Sleep(0.5);

}

void turnRight(int power,int angle)

{

    float rotations = (angle/180.0)*60;

    while(LEncod.Counts()<rotations)

    {

        dcmotorL.SetPercent(power);

        dcmotorR.SetPercent(-(power));

    }

    dcmotorL.SetPercent(0);

    dcmotorR.SetPercent(0);

    LEncod.ResetCounts();

    REncod.ResetCounts();

    Sleep(0.5);

}

void turnLeft(int power,int angle)

{

    float rotations = (angle/180.0)*60;

    while(REncod.Counts()<rotations)

    {

        dcmotorL.SetPercent(-(power));

        dcmotorR.SetPercent(power);

    }

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

}