3/7/14 – Code (Version 6.1 + 6.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);

void turnLeftSkid(int angle);

void turnRightSkid(int angle);

void checkXPosition(float x,int xorient,float heading);

void checkYPosition(float y,int yorient,float heading);

void checkHeading(float heading);

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

    LServo.SetMax(2300);

    RServo.SetMin(550);

    RServo.SetMax(2200);

    BinServo.SetMax(2200);

    BinServo.SetMin(600);

    setFLDegree(180);

    LCD.Clear();

    LCD.WriteLine("Ready");

    LCD.WriteLine("RPS Data");

    LCD.WriteLine("X-Position: ");

    LCD.WriteLine("Y-Position: ");

    LCD.WriteLine("Heading: ");

    while(1==1)

        {

        LCD.WriteRC(RPS.X(),1,12);

        LCD.WriteRC(RPS.Y(),2,12);

        LCD.WriteRC(RPS.Heading(),3,12);

            if(buttons.MiddlePressed())

                {

                while(CdS.Value()>.31){};

                        driveDistance(31.0,50);

                        checkXPosition(5.0,1,90);

                        turnLeft(83);

                        checkHeading(160);

                        setFLDegree(110);

                        setFLDegree(32);

                        driveDistance(15.0,50);

                        setFLDegree(178);

                        driveDistance(7.0,-50);

                        turnLeftSkid(90);

                        driveDistance(9.0,50);

                        checkXPosition(2,-1,90.0);

                        turnLeftSkid(90);

                        checkHeading(175);

                        driveDistance(32.0,50);

                        driveDistance(3.0,-50);

                        turnRightSkid(90);

                        checkHeading(90);

                        driveDistance(5.0,50);

                        checkXPosition(-6.0,-1,90);

                        turnRightSkid(60);

                        checkHeading(5);

                        setFLDegree(30);

                        driveDistance(13.0,50);

                        driveDistance(5.0,-50);

                }

        }

}

void driveDistance(float distance,int power)

{

    float rotations=distance*3.15;

    double startTime = TimeNow();

    while(REncod.Counts()<rotations&&LEncod.Counts()<rotations&&(TimeNow()-startTime)<6.0)

    {

        dcmotorL.SetPercent(power);

        dcmotorR.SetPercent(-(2+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.3);

}

void turnRight(int angle)

{

    float rotations = (angle/180.0)*19;

    double startTime = TimeNow();

    while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)

    {

        dcmotorL.SetPercent(50);

        dcmotorR.SetPercent(50);

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

}

void turnLeft(int angle)

{

    float rotations = (angle/180.0)*21;

    double startTime = TimeNow();

    while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)

    {

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

}

void turnRightSkid(int angle)

{

    float rotations = (angle/180.0)*40;

    double startTime = TimeNow();

    while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)

    {

        dcmotorL.SetPercent(50);

        dcmotorR.SetPercent(50);

        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 turnLeftSkid(int angle)

{

    float rotations = (angle/180.0)*40;

    double startTime = TimeNow();

    while(REncod.Counts()<rotations&&(TimeNow()-startTime)<5.0)

    {

        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 checkXPosition(float x, int xorient, float heading)

{

    while(RPS.Heading()<(heading-3))

    {

        turnLeft(3);

    }

    while(RPS.Heading()>(heading+3))

    {

        turnRight(3);

    }

    if(xorient==-1)

    {

        while(RPS.X()<(x-1.0))

        {

            driveDistance(0.5,-50);

        }

        while(RPS.X()>(x+1.0))

        {

            driveDistance(0.5,50);

        }

    }

    if(xorient==1)

    {

        while(RPS.X()<(x-1.0))

        {

            driveDistance(1.0,50);

        }

        while(RPS.X()>(x+1.0))

        {

            driveDistance(1.0,-50);

        }

    }

}

void checkYPosition(float y, int yorient, float heading)

{

    while(RPS.Heading()<(heading-3))

    {

        turnLeft(5);

    }

    while(RPS.Heading()>(heading+3))

    {

        turnRight(5);

    }

    if(yorient==-1)

    {

        while(RPS.Y()<(y-1.0))

        {

            driveDistance(1.0,-50);

        }

        while(RPS.Y()>(y+1.0))

        {

            driveDistance(1.0,50);

        }

    }

    if(yorient==1)

    {

        while(RPS.Y()<(y-1.0))

        {

            driveDistance(1.0,50);

        }

        while(RPS.Y()>(y+1.0))

        {

            driveDistance(1.0,-50);

        }

    }

}

void checkHeading(float heading)

{

    while(RPS.Heading()<(heading-3))

    {

        turnLeft(2);

    }

    while(RPS.Heading()>(heading+3))

    {

        turnRight(5);

    }

}