Cruise Control Sketch

This Arduino sketch allows the robot to be maintain a constant speed regardless of the incline that the robot happens to be driving over. In order to complete this challenge, the robot needed to drive on a treadmill that gradually increased its slope to be the maximum angle, and hold a steady speed for ten seconds. In order to accomplish this, the sketch took input from the wheel encoders and periodically updated registry information within the Arduino. The speed was calculated and compared against a preset speed. A PID loop then determined if the speed should be increased, decreased, or remain the same.

Arduino Sketch

#include <encoder.h>
#include <motor.h>
#include <speedcontrol.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <HardwareSerial.h>

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

volatile int toggle_motor_speed = 1;
volatile double speedSetting1 = 70.0;
volatile double speedSetting2 = 50.0;
volatile double speedSetting3 = 70.0;
volatile double speedSetting4 = 50.0;
volatile double samplingRate = 0.02;
volatile int update_speed = 0;
volatile double setspeed = 0;
volatile double setspeed1 = 0;

encoder *rightEncoder = new encoder(20, 66, 48);
encoder *leftEncoder = new encoder(20, 66, 49);

motor*RFMotor = new motor(&AFMS, 1, 0); //right front
motor*LFMotor = new motor(&AFMS, 2, 1); //left front
motor*LRMotor = new motor(&AFMS, 3, 1); //left rear
motor*RRMotor = new motor(&AFMS, 4, 0); //right rear

speedcontrol *rightSpeed = new speedcontrol(&Serial, rightEncoder, RFMotor, RRMotor,samplingRate);
speedcontrol *leftSpeed = new speedcontrol(&Serial, leftEncoder, LFMotor, LRMotor, samplingRate);

void setup() {
Serial.begin(115200);
noInterrupts(); // Disable the ISR's
TCCR3A = 0;
TCCR3B = 0x0A; // Prescale and CIC mode
OCR3A = 0x9C40; // set the compare A Register
TIMSK3 = 0x02; // enable the output compare A Match Interrupt
TCNT3 = 0x00;

// Enable input capture 4 (ICNC4 enable noise canceler, ICES4 trigger on rising edge, CS42/0 no prescaler)
TCCR4A = 0x00; // Initilize register to 0
TCCR4B = 0x00; // Initilize register to 0
TCCR4B = (1 << ICNC4) | (1 << ICES4) | (0 << WGM42) | (1 << CS42) | (0 << CS41) | (1 << CS40);
TCCR4C = 0x00; // Initilize register to 0
TIMSK4 = (1<
// Enable input capture 5 (ICNC5 enable noise canceler, ICES5 trigger on rising edge, CS52/0 no prescaler)
TCCR5A = 0x00; // Initilize register to 0
TCCR5B = 0x00; // Initilize register to 0
TCCR5B = (1 << ICNC5) | (1 << ICES5) | (0 << WGM52) | (1 << CS52) | (0 << CS51) | (1 << CS50);
TCCR5C = 0x00; // Initilize register to 0
TIMSK5 = (1 << ICIE5); // Input capture interrupt enabled
TCNT5 = 0x00; // Sets the Timer to 0

interrupts(); // enable interrupts.
rightSpeed -> enable();
leftSpeed -> enable();
AFMS.begin();
}

void loop() {
  setspeed = rightEncoder -> getSpeed();
  setspeed1 = leftEncoder -> getSpeed();
  if(setspeed < 5 && setspeed1 < 5)
  setspeed = 0;
  if(update_speed == 1){
  rightSpeed -> update(setspeed);
  leftSpeed -> update(setspeed1);
  update_speed = 0;
}
if(toggle_motor_speed == 1){
  rightSpeed -> setDesiredSpeed(speedSetting1);
  leftSpeed -> setDesiredSpeed(speedSetting1);
}
else {
  rightSpeed -> setDesiredSpeed(speedSetting2);
  leftSpeed -> setDesiredSpeed(speedSetting2);
 }
}
ISR(TIMER3_COMPA_vect){
  update_speed = 1;
}
ISR(TIMER5_OVF_vect){
  if(toggle_motor_speed == 1)
    toggle_motor_speed == 0;
  else
    toggle_motor_speed;
}
ISR(TIMER4_OVF_vect){
  if(toggle_motor_speed == 1)
    toggle_motor_speed = 0;
  else
    toggle_motor_speed = 1;
}

ISR(TIMER5_CAPT_vect){
  rightEncoder -> updateTime(ICR5);
  TCNT5 = 0x00;
}
ISR(TIMER4_CAPT_vect){
  leftEncoder -> updateTime(ICR4);
  TCNT4 = 0x00;
}