Arduino

// EDIT THE NUMERATOR TO HOW MANY MARKS YOU READ IN ONE
// REVOLUTION (USE THE SENSOR TEST):
float marksPerInch = 7 / 3.9; // 1 rev = 7 marks / 3.9 in diameter
// IF LEFT IS NEGATIVE, SET TO FALSE
bool motorsForward = true; // are the motors currently set to spin forward?
int cruiseSpeed = 50; // how fast the motors should go normally
int uphillSpeed = 75; // speed to get up a hill
int manueverSpeed = 20; // speed when correcting (set to minimum)

void myCode()
{
  //----------------------------------------------------------------------------------------
  // myCode();
  // Note:
  // (1) After running your AEV do not turn the AEV off, connect the AEV to a computer, or
  //     push the reset button on the Arduino. There is a 13 second processing period. In
  //     post processing, data is stored and battery recuperation takes place.
  // (2) Time, current, voltage, total marks, position traveled are recorded approximately
  //     every 60 milliseconds. This may vary depending on the vehicles operational tasks.
  //     It takes approximately 35-40 milliseconds for each recording. Thus when programming,
  //     code complexity may not be beneficial.
  // (3) Always comment your code. Debugging will be quicker and easier to do and will
  //     especially aid the instructional team in helping you.
  //----------------------------------------------------------------------------------------
  // WE HAVE TO MANUALLY PROGRAM GOING UP/DOWN THE HILL DUE TO THE
  // SPEED WE NEED. IF IT ISN'T NECESSARY, THEN USE JUST
  // goToPosition().
  // IF LEFT IS NEGATIVE, NEGATE ALL OF THE POSITIONS
  reverse(2); // one of our motors faces the opposite direction
  // go up first hill
  motorSpeed(4, uphillSpeed - 10); // start slow
  goToRelativePosition(3 * 12 * marksPerInch); // then, halfway up
  motorSpeed(4, uphillSpeed); // go full speed
  goToRelativePosition(3 * 12 * marksPerInch); // brake at the top
  // end go up first hill
  goToPosition(8 * 12, cruiseSpeed); // go to the top of the incline
  // attach other cart
  goFor(4); // wait four seconds
  // go down first hill
  // IF LEFT IS NEGATIVE ADD A ! (i.e., !motorsForward)
  if(motorsForward) { // if the function didn't leave us going right
    reverse(4); // change direction
    motorsForward = !motorsForward; // ditto
  }
  motorSpeed(4, cruiseSpeed); // start going down the hill
  goToRelativePosition(-2 * 12 * marksPerInch); // half way down
  reverse(4); // reverse direction
  motorSpeed(4, cruiseSpeed); // thrust opposite to motion to stay slow
  goToRelativePosition(-4 * 12 * marksPerInch); // at the bottom
  reverse(4); // reverse again
  // end go down first hill
  goToPosition(-2 * 12, cruiseSpeed); // go to the second stop at 20%
  goFor(4); // wait four seconds
  goToPosition(-11 * 12, cruiseSpeed); // go to the third stop
  goFor(4); // wait four seconds
  // go up second hill
  motorSpeed(4, cruiseSpeed); // move to the bottom of the hill
  goToRelativePosition(-1 * 12 * marksPerInch); // ditto
  motorSpeed(4, uphillSpeed - 10); // start slow
  goToRelativePosition(-3 * 12 * marksPerInch); // then, halfway up
  motorSpeed(4, uphillSpeed); // go full blast
  goToRelativePosition(-3 * 12 * marksPerInch); // brake at the top
  // end go up second hill
  goToPosition(-20 * 12, cruiseSpeed); // go to the top of the second incline
  goFor(4); // wait four seconds
  // go down second hill
  // IF LEFT IS NEGATIVE REMOVE THE ! (i.e., motorsForward)
  if(!motorsForward) { // if the function didn't leave us facing the right way
    reverse(4); // change direction
    motorsForward = !motorsForward; // ditto
  }
  motorSpeed(4, cruiseSpeed); // start going down the hill
  goToRelativePosition(2 * 12 * marksPerInch); // half way down
  reverse(4); // reverse direction
  motorSpeed(4, cruiseSpeed); // thrust opposite to motion to stay slow
  goToRelativePosition(4 * 12 * marksPerInch); // at the bottom
  reverse(4); // reverse again
  // end go down second hill
  goToPosition(-12 * 12, cruiseSpeed); // go to the fifth stop
  // detach other cart
  goFor(4); // wait four seconds
  goToPosition(0, 40); // go to the maintenance stop
}

// Accepts an absolute position in inches and a motor speed.
// Automatically handles motor direction, breaking
// and self-correction.
void goToPosition(int targetPosition, int mSpeed) {
  while(true) {
    float curPosIn = getVehiclePostion() / marksPerInch; // convert vehicle position to inches
    if(curPosIn < (targetPosition - 3)) { // If we're behind the target position with a 6 inch tolerance
      if(!motorsForward) { // and the motors are set to backwards
        reverse(4); // reverse the motors
        motorsForward = !motorsForward; // and update our variable
      }
      motorSpeed(4, mSpeed); // then travel at the set speed
    } else if(curPosIn > (targetPosition + 3)) { // If we're in front of the target position with a 6 inch tolerance
      if(motorsForward) { // and the motors are set to forward
        reverse(4); // reverse the motors
        motorsForward = !motorsForward; // and update our variable
      }
      motorSpeed(4, mSpeed); // then travel at the set speed
    } else {
      brake(4); // we're within tolerence, time to brake
      goFor(1.5); // wait for half a second in case we're drifting still
      curPosIn = getVehiclePostion() / marksPerInch; // get our position after that time
      if(curPosIn > (targetPosition - 3) & curPosIn < (targetPosition + 3)) {
        break; // if we're still within tolerance, we're done here
      } else {
        mSpeed = manueverSpeed; // if we've drifted too much, we're going
        // to have to do this all over again, but lets set the
        // speed down to reduce overshooting the target.
      }
    }
  }
}