2/25/14 – Code (Version 5.1)

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

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;

int main(void)

{

    //RPS.InitializeMenu();

    //RPS.Enable();

    LServo.SetMin(525);

    LServo.SetMax(2400);

    RServo.SetMin(525);

    RServo.SetMax(2400);

    BinServo.SetMax(2400);

    BinServo.SetMin(525);

    setFLDegree(90);

    while(1==1)

        {

            if(buttons.MiddlePressed())

                {

                    driveDistance(6.0,50);

                    turnLeft(90);

                }

        }

}

void driveDistance(float distance,int power)

{

    float rotations=distance*3.15;

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

{

    float rotations = (angle/180.0)*180;

    while(LEncod.Counts()<rotations)

    {

        dcmotorL.SetPercent(-50);

        dcmotorR.SetPercent(75);

        LCD.WriteLine(LEncod.Counts());

        LCD.Clear();

    }

    dcmotorL.SetPercent(0);

    dcmotorR.SetPercent(0);

    LEncod.ResetCounts();

    REncod.ResetCounts();

    Sleep(0.5);

}

void turnLeft(int angle)

{

    float rotations = (angle/180.0)*118;

    while(REncod.Counts()<rotations)

    {

        dcmotorL.SetPercent(50);

        dcmotorR.SetPercent(-50);

        LCD.WriteLine(REncod.Counts());

        LCD.Clear();

    }

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

}