lab5

#include <RedBot.h>

#define SPEED 90

// initialize RedBot motors and motor encoders
RedBotMotors motors;
RedBotEncoder encoder = RedBotEncoder(A2, 10);

// using counts per rev of 192/rev: 967 revs for 1m
int targetcount = 967;

// initialize lCount, rCount, lDiff, rDiff, prevlCount, and prevrCount
int lCount, rCount, lDiff, rDiff;
int prevlCount = 0;
int prevrCount = 0;

// set the speeds of left and right motors and speed offset
int lSpeed = SPEED;
int rSpeed = SPEED;
int offset = 3;


void setup() {
  // begin serial monitoring
  Serial.begin(9600);
}

void loop() {
  // starts the loop by getting the number of ticks the encoders have on each motor
  lCount = encoder.getTicks(LEFT);
  rCount = encoder.getTicks(RIGHT);

  // stops to turn when the right motor hits the targeted amount of ticks
  if(rCount < targetcount) {
    // get the difference in ticks between 50ms intervals and set previous tick counts
    lDiff = (lCount - prevlCount);
    rDiff = (rCount - prevrCount);
    prevlCount = lCount;
    prevrCount = rCount;
    // if left wheel is ahead, slow down left, speed up right
    if(lDiff > rDiff) {
      lSpeed = lSpeed - offset;
      rSpeed = rSpeed + offset;
    // if right wheel is ahead, slow down right, speed up left
    }else if(lDiff < rDiff) {
      lSpeed = lSpeed + offset;
      rSpeed = rSpeed - offset;
    }else{
      lSpeed = SPEED;
      rSpeed = SPEED;
    }
    // run the motors at the set speeds
    motors.rightMotor(rSpeed);
    motors.leftMotor(-lSpeed);
    delay(50);
  // turn using encoder ticks
  }else{
    // stop and clear encoders and delay for 500ms before pivoting
    motors.brake();
    encoder.clearEnc(BOTH);
    lCount = encoder.getTicks(LEFT);
    rCount = encoder.getTicks(RIGHT);
    delay(500);
    // pivot at 100 speed until the left motor hits 85 ticks
    while(lCount < 88) {
      rightPivot(100);
      lCount = encoder.getTicks(LEFT);
      delay(50);
    }
    // stop and reset speed variables and encoders
    motors.stop();
    lSpeed = SPEED;
    rSpeed = SPEED;
    encoder.clearEnc(BOTH);
    delay(1000);
  }
}

void rightPivot(int n) {
  motors.rightMotor(-n);
  motors.leftMotor(-n);
}