2/27/14 – Code (Verision 5.2 + 5.0)

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

//}