Performance Test 1 Source Code

#include <FEHMotor.h>
#include <FEHLCD.h>
#include <FEHIO.h>
#include <FEHUtility.h>
#include <FEHServo.h>
#include <LCDColors.h>
#include <FEHRPS.h>
#include <cmath>

//Create variable for servo arm
FEHServo arm(FEHServo::Servo0);
//Create variable for motors
FEHMotor leftMotor(FEHMotor::Motor0, 12);
FEHMotor rightMotor(FEHMotor::Motor1, 12);
//Create variables for encoders
AnalogEncoder leftEncoder(FEHIO::P0_0);
AnalogEncoder rightEncoder(FEHIO::P0_2);
//Create variable for servo gear
FEHServo gear(FEHServo::Servo2);
//Create variable for cdsCell
AnalogInputPin cdsCell(FEHIO::P3_7);
float x_coord;
float y_coord;


void turnIndefinitely(float angle, bool clockwise);
void driveStraightIndefinitely(float distance);
void driveUpHillIndefinitely(float distance);

int main(){

    driveStraightIndefinitely(3.0);
    Sleep(300);
    turnIndefinitely(90, false);
    Sleep(300);
    driveStraightIndefinitely(11.5);
    Sleep(300);
    turnIndefinitely(90, false);
    Sleep(300);
    driveStraightIndefinitely(-1.75);
    Sleep(300);
    driveUpHillIndefinitely(30);
    Sleep(300);
    turnIndefinitely(90, false);
    Sleep(300);
    driveStraightIndefinitely(25);
    Sleep(300);
    turnIndefinitely(90,true);
    Sleep(300);
    driveStraightIndefinitely(-12);
    Sleep(300);
    turnIndefinitely(125, true);
    Sleep(300);
    driveStraightIndefinitely(20.0);
    Sleep(300);
    turnIndefinitely(55, true);
    Sleep(300);
    driveStraightIndefinitely(3.0);
    Sleep(300);
    turnIndefinitely(90, false);
    Sleep(300);
    driveStraightIndefinitely(3.5);
    Sleep(300);
    turnIndefinitely(90, false);
    Sleep(300);
    driveStraightIndefinitely(22);
    Sleep(300);
    driveStraightIndefinitely(-30.5);
    Sleep(300);
    driveStraightIndefinitely(2);
    Sleep(300);
    turnIndefinitely(90, false);
    Sleep(300);
    driveStraightIndefinitely(8);
    Sleep(300);
    turn(60,false);
    Sleep(300);
    driveStraightIndefinitely(14);
    Sleep(300);
    turnIndefinitely(60, true);
    Sleep(300);
    driveStraightIndefinitely(-5);
    Sleep(300);
    return 0;

}

/*
  Method that drives robot a distance. Implemented with TimeNow().
*/
void driveStraightIndefinitely(float distance){
    float seconds = distance/INCH_PER_SECOND;
    float time = TimeNow();
    if(seconds < 0){
         seconds*= -1;
         while(TimeNow() - time < seconds){
             leftMotor.SetPercent(-60);
             rightMotor.SetPercent(-60);
         }
    }else{
        while(TimeNow() - time < seconds){
            leftMotor.SetPercent(60);
            rightMotor.SetPercent(60);
        }
    }

    rightMotor.Stop();
    leftMotor.Stop();
    rightEncoder.ResetCounts();
    leftEncoder.ResetCounts();
}

/*
  Method that drives a robot uphill. Implemented with TimeNow().
*/
void driveUpHillIndefinitely(float distance){
    float seconds = distance/INCH_PER_SECOND;
    float time = TimeNow();
    if(seconds < 0){
         seconds*= -1;
         while(TimeNow() - time < seconds){
             leftMotor.SetPercent(-80);
             rightMotor.SetPercent(-80);
         }
    }else{
        while(TimeNow() - time < seconds){
            leftMotor.SetPercent(80);
            rightMotor.SetPercent(80);
        }
    }

    rightMotor.Stop();
    leftMotor.Stop();
}

/*
  Makes a point turn on a specified angle in a certain direction.
  Implemented using the TimeNow() Function.
*/
void turnIndefinitely(float angle, bool clockwise){
    float time = TimeNow();
    if(clockwise){
      float seconds = angle/(DEG_PER_SECOND)/2;
      while(TimeNow() - time < seconds){
          leftMotor.SetPercent(60);
          rightMotor.SetPercent(-60);
      }
      leftMotor.Stop();
      rightMotor.Stop();
    }else{
        float seconds = angle/DEG_PER_SECOND/2;
        while(TimeNow() - time < seconds){
          rightMotor.SetPercent(60);
          leftMotor.SetPercent(-60);
      }
      rightMotor.Stop();
      leftMotor.Stop();
    }
    leftEncoder.ResetCounts();
    rightEncoder.ResetCounts();
    rightMotor.Stop();
    leftMotor.Stop();
}