16-299 Final Project - Puppy TWIP

Introduction

Our project was a following robot. There were two main segments to the project. First we made the Elegoo robot balance in Lab 2, and the we built on top of that to make it follow a target at a set distance.

ELEGOO Tumbller Self-Balancing Robot Car Kit Compatible with Arduino IDE

In order to balance, the ELEGOO Tumbler uses the MPU6050 accelerometer/gyro to determine body angle/angular velocity. A Kalman filter is used to improve the estimate of body angle by combining measurements from the gyro (angular acceleration) and accelerometer (linear acceleration at a distance from the wheel axis). A PID controller is used to keep the robot upright by measuring error between desired body angle and observed body angle, and using these values to drive the motors of the robot. PID is an acronym for Proportional-Integral-Derivative, because it uses tuned constants Kp, Kd, and Ki multiplied by the corresponding P, I, and D error values to determine the control output.

Proportional: The proportional component of a PID controller responds to the error between desired and measured values. Proportional control is analogous to a spring in a mass-spring-damper system, where error in goal pose drives the system directly back towards its goal pose. Up = Kp * (theta_measured - theta_desired). For a purely balancing robot theta_desired is set to zero.

Integral: The integral component of a PID controller responds to the accumulated error between desired and measured values. At each timestep, error*dt is added to the integrator state. Integral control is important to self-balancing robots because over time, integral control drives steady state error to zero, so any biases in the system will diminish over time, and the robot will tend towards upright. Ui = Ki * (SUM[(theta_measured-theta_desired)*dt]).

Derivative: The derivative component of a PID controller responds to the rate of change of error in the system. Derivative control is analogous to a damper in a mass-spring-damper system, where increasing error drives control towards the goal pose and decreasing error drives control away from the goal pose. Proportional and Integral control introduce overshoot into the system dynamics, so derivative control is important for ensuring that the robot settles to steady state quickly. Ud = Kd * (omega_measured-omega_desired). For a balancing robot, omega_desired is zero.

PID control on body angle works well to keep the robot upright, however with no control law related to position, the robot has a tendency to drive off in one direction. This effect isn't super apparent on carpet, since soft floors introduce natural damping/friction into the system, however when shoved the robot drifts away without stopping.

In order to help stabilize the robot, we added a derivative controller to the encoder values of the robot. Rather than acting on the motor control output of the robot, the position controller instead changed the body angle setpoint of the robot. As the robot would drive in one direction, the positional D controller would move the body angle setpoint in the opposite direction resulting in the robot leaning back and braking itself. Additionally, we added a manual bias to the zero setpoint of the robot, as we observed that there was a slight discrepancy between the MPU zero reading and angular position of the robot to result in zero leaning. These added changes to the control law make the robot attempt to hold its current position, allowing it to balance indefinitely.

Now that balancing is functional and stable, the next step of this project is to use the ultrasonic sensor to measure distance from an object and modify the control law to allow it to maintain a constant distance. The ultrasonic sensor used on this robot is the HC-SR04. By measuring the time between echo pulses (out) and receives (in), the distance between an object and this sensor can be determined using the speed of sound.

For this project, a desired following distance of 30cm was used. A new P and I term were introduced to the robot's control law, acting on the error between a robot's measured distance from a target and desired following distance. Like the derivative term for encoder values, the PI controller for measured distance modified the body angle setpoint of the balancer's PID controller. The reason the D term of position used encoder distance values instead of ultrasonic distance values was because the ultrasonic sensor tended to report distance values with very low granularity, meaning it would report zero velocity for a moving target and then all the sudden report an enormous velocity value. This would cause the robot to set an unreasonable angle setpoint and fall over. While this does make it more difficult for the robot to follow a moving target, a large proportional distance constant is sufficient to accommodate this.

The following code was used to compute the ultrasonic measured distance:

void measureDistance()
{
  if (measure_flag == 0)
  {
    measure_prev_time = micros();
    attachInterrupt(digitalPinToInterrupt(ECHO_PIN), measureDistance, FALLING);
    measure_flag = 1;
  }
  else if (measure_flag == 1)
  {
    distance = (micros() - measure_prev_time) * 0.017; //343 m/s * (10^-6 s / us)/ 2 = 0.017
    measure_flag = 2;
  }
}

void getDistance()
{
  if (millis() - get_distance_prev_time > 50)
  {
    get_distance_prev_time = millis();
    measure_flag = 0;
    attachInterrupt(digitalPinToInterrupt(ECHO_PIN), measureDistance, RISING);
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
  }
}

Now that the ultrasonic sensor has been integrated into the control law, the last step is to introduce a way for the robot to orient itself towards its target. Two IR sensors are mounted on the front left and right corners of the robot, and are used to sense if a target is on the left, middle, or right side of the robot. Every 15ms, a 38kHz pulse is sent out from each IR sensor. If the robot receives a signal on both or neither sensors, the target is in the middle, if only the left sensor receives a signal, the target is on the left, and if only the right sensor receives a signal the target is on the right. A simple TURN value is added/subtracted to the motor commands on the left and right motors depending on which side the target is on. We found that a TURN value of 30 is sufficient for hard floors, and 45 for softer carpeting.

The IR LEDs have to be flashed periodically, and the receivers get the reflected IR signals. This was the function used to drive the LEDs. It was called approximately every 15 milliseconds. Note that this code was adapted from the Elegoo Tumbler's source code.

void drive_ir( void )
{
  static unsigned long ir_send_time = 0;
// this time check seems to make sure we don't send flashing IR light
  // more often that every 15 milliseconds.
  if (millis() - ir_send_time > 15)
    {
      left = false;
      right = false;
      for (int i = 0; i < 39; i++)
        {
          digitalWrite(IR_DRIVE_PIN, LOW);
          delayMicroseconds(9);
          digitalWrite(IR_DRIVE_PIN, HIGH);
          delayMicroseconds(9);
        }
      ir_send_time=millis();
    }
}

We used interrupts to read the receivers. The interrupts triggered whenever the reciever pins changed value, and these were the interrupt service routines.

void left_recieve() {
  // Set counter
  ir_counter = COUNTER_INIT;
  // Turn LED on
  digitalWrite( LED_BUILTIN, 1 );
  Serial.println( "L " );
  left = true;
}

void right_recieve() {
  // Set counter
  ir_counter = COUNTER_INIT;
  // Turn LED on
  digitalWrite( LED_BUILTIN, 1 );
  Serial.println( "R " );
  right = true;
}

The last part of this project is to put each component together. Something important to note is that through this whole process, any time a major change is made to the robot and its control laws, small tuning tweaks typically need to be made to PID parameters. When the ultrasonic sensor was implemented, the integral term from the balancing PID controller was removed, as the positional controller would prevent the robot from drifting off to one direction. To accommodate for faster moving targets and stop and go behavior, the derivative term of the positional controller was increased drastically, removing most of the positional overshoot. This did cause the robot to have a lower max following speed, so finding a way to critically damp the robot's positional response while permitting faster driving is a large area of improvement remaining for this project.

Overall we found that these two sets of PID controllers were sufficient to program the Elegoo Tumbler to follow a moving target. To reiterate, PID control on body angle was used to drive the motors, and PID control on separation/position was used to modify the balance setpoint. Here are the final control parameters we used:

Kp_th = 50; %MPU
Ki_th = 0; %MPU
Kd_th = 1.5; %MPU

Kp_pos = 50; %Ultrasonic
Ki_pos = 6; %Ultrsonic
Kd_pos = 50; %Encoders

/**********************************************************************
Run a balancing servo off a clock.
Get input from IMU:
 * Need to have "I2Cdev" folder in Arduino libraries
 * Need to have "MPU6050" folder in Arduino libraries
Estimate body angle (in accelerometer Y units)
/**********************************************************************/
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Pins.h"
#include "My_encoders.h"
#include "rgb.h"
//#include "PinChangeInt.h"
/**********************************************************************/
#define MAX_ULONG 0xFFFFFFFF
// I had to increase this to 3ms to accomodate reading the IMU
#define SERVO_INTERVAL 3000 // microseconds
// start data collection after N milliseconds after starting servo
#define START_COLLECT  5000 // milliseconds
// start balancer N milliseconds after START_COLLECT
#define START_BALANCE   100 // milliseconds
#define DEBUG_PRINT_INTERVAL 500 // milliseconds,
#define MAX_COMMAND 255
// #define PI 3.141592653589793
#define ENCODER_TO_RADIANS 0.004027682889218
#define BUTTON_PIN 10
#define MAX_SETPOINT 1500 //ABOUT 15 DEGREES
/*
#define ECHO_PIN A3
#define TRIG_PIN 11
#define RECV_PIN 9
#define IR_SEND_PIN 9
#define LEFT_RECEIVE_PIN A0
#define RIGHT_RECEIVE_PIN A1
*/
/**********************************************************************/
bool go = false;  /* motor enable *
/ Encoder readings: these can be positive or negative.
long left_angle = 0;
long right_angle = 0;
// velocity filter stuff
float left_diff[10];
float right_diff[10];
float past_left_angle_radians;
float past_right_angle_radians;
float past_left_velocity[10];
float past_right_velocity[10];
double distance = 0;
int measure_flag = 0;
float measure_prev_time = 0;
float get_distance_prev_time = 0;
volatile bool left = false;
volatile bool right = false;
MPU6050 accelgyro;
// Current accelerometer and gyro zero values.
int16_t ax, ay, az;
int16_t gx, gy, gz;
int ax0, ay0, az0;
int gx0, gy0, gz0;
int last_body_angle = 0;
int last_gxx = 0; // last X gyro reading
// Keep track of late control cycles. 
unsigned long servo_late = 0; // How many times am I late?
unsigned long max_servo_late = 0; // What was the worst one?
/**********************************************************************/
/**********************************************************************/
void motor_init()
{
  pinMode(AIN1, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(PWMA_LEFT, OUTPUT);
  pinMode(PWMB_RIGHT, OUTPUT);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, LOW);
  analogWrite(PWMA_LEFT, 0);
  analogWrite(PWMB_RIGHT, 0);
  pinMode(STBY_PIN, OUTPUT);
  digitalWrite(STBY_PIN, HIGH);
}
void motor_stop()
{
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, LOW);
  analogWrite(PWMA_LEFT, 0);
  analogWrite(PWMB_RIGHT, 0);
}
void motor_left_command( int speed )
{
    if ( speed >= 0 )
    {
      digitalWrite( AIN1, 1 );
      analogWrite( PWMA_LEFT, speed );
    }
  else
    {
      digitalWrite( AIN1, 0 );
      analogWrite( PWMA_LEFT, -speed );
    }
}
// reverses the sign of "speed"
void motor_right_command( int speed )
{
  if ( speed >= 0 )
    {
      digitalWrite( BIN1, 1 );
      analogWrite( PWMB_RIGHT, speed );
    }
  else
    {
      digitalWrite( BIN1, 0 );
      analogWrite( PWMB_RIGHT, -speed );
    }
}
void calibrate_MPU(int zeros[6]) {
  int n = 0;
  int sums[6];
  accelgyro.initialize();
  Serial.println("MPU Initialized");
  for(int i = 1; i < 100; i++) {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    zeros[0] += (ax - zeros[0]) / i;
    zeros[1] += (ay - zeros[1]) / i;
    zeros[2] += (az - zeros[2]) / i;
    zeros[3] += (gx - zeros[3]) / i;
    zeros[4] += (gy - zeros[4]) / i;
    zeros[5] += (gz - zeros[5]) / i;
    delay(50);
  }
  Serial.println("MPU Calibrated");
}
/** Start ELEGOO CODE **/
//The following code is adapted directly from the ELEGOO source code
void ultrasonicInit()
{
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(IR_SEND_PIN, OUTPUT);
  pinMode(LEFT_RECEIVE_PIN, INPUT_PULLUP);
  pinMode(RIGHT_RECEIVE_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LEFT_RECEIVE_PIN), left_recieve, FALLING);
  attachInterrupt(digitalPinToInterrupt(RIGHT_RECEIVE_PIN), right_recieve, FALLING);
}
void measureDistance()
{
  if (measure_flag == 0)
  {
    measure_prev_time = micros();
    attachInterrupt(digitalPinToInterrupt(ECHO_PIN), measureDistance, FALLING);
    measure_flag = 1;
  }
  else if (measure_flag == 1)
  {
    distance = (micros() - measure_prev_time) * 0.017; //343 m/s * (10^-6 s / us)/ 2 = 0.017
    measure_flag = 2;
  }
}
void getDistance()
{
  if (millis() - get_distance_prev_time > 50)
  {
    get_distance_prev_time = millis();
    measure_flag = 0;
    attachInterrupt(digitalPinToInterrupt(ECHO_PIN), measureDistance, RISING);
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
  }
}
void left_recieve() {
  left = true;
}
void right_recieve() {
  right = true;
}
/**** END ELEGOO CODE ****
**********************************************************************/
void setup()
{
Wire.begin();  // I2C (TWI) uses Wire library
  Serial.begin( 1000000 );  // run Serial fast to print out data
  Wire.setClock( 1000000UL ); // run I2C as fast as possible
  analogReference(INTERNAL1V1); // was INTERNAL on 328P, voltage
rgb.initialize();
  rgb.theaterChase(255,255,0,0); //Yellow for calibration/initialization
motor_init(); // Initialize motors
  Encoder_init( &left_angle, &right_angle ); // Initialize (zero) encoders
  //accelgyro.initialize(); // Initialize IMU
  int zeros[6];
  calibrate_MPU(zeros);
  ax0 = zeros[0];
  ay0 = zeros[1];
  az0 = zeros[2];
  gx0 = zeros[3];
  gy0 = zeros[4];
  gz0 = zeros[5];
  ultrasonicInit();
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), button_isr, FALLING);
  rgb.theaterChase(255, 0, 0, 0); //Red for initialized but deactivated
}
/******************************************************************
/Scan for button press
volatile bool button_pressed = false;
void button_isr(void) {
  button_pressed = true;
}
/**********************************************************************
Run a balancer with the given gains.
run time starts at the beginning and is a millisecond clock.
START_COLLECT   starts data collection START_COLLECT milliseconds after beginning
START_BALLANCE   starts balancer START_COLLECT + START_BALANCE milliseconds
                 after beginning.
DEBUG_PRINT_INTERVAL   milliseconds between debug printouts
/**********************************************************************/
void run_balancer(float body_angle_gain, float body_angular_velocity_gain,
     		          float body_angle_integral_gain, //WIP
                   unsigned long run_time_limit, int collect_data )
{
  // Clocks and time management
  // used to keep track of time intervals in servo
  unsigned long last_micros = micros();
  unsigned long last_millis = millis();
  int servo_time = 0; // Servo clock: needs to be signed, can be int
  // how long have we been running? Safety feature
  unsigned long run_time = 0; // milliseconds
  // Clocks for printing and other stuff
  unsigned long debug_print_time = 0; // milliseconds
run_time_limit += START_COLLECT; // account for startup time
// state
  int started = 0;
// integrators
  long integrator_state = 0;
  long position_integrator_state = 0;
// keep track of voltage
  int voltage_raw = analogRead(VOL_MEASURE_PIN); //Read voltage value
  double voltage = (voltage_raw*1.1/1024)*((10+1.5)/1.5);
float body_angle_setpoint = 0;
  float body_angle_error = 0;
  float position_error = 0;
  float position_error_last = 0;
  float position_error_deriv = 0;
  float distance_setpoint = 30; //cm
// servo loop
  for( ; ; ) {
/*****************************************
Timekeeping part */
      unsigned long current_micros = micros();
      if ( current_micros > last_micros )
        servo_time += current_micros - last_micros;
      else // rollover
        servo_time += (MAX_ULONG - last_micros) + current_micros;
      last_micros = current_micros;
// It isn't time yet ...
      if ( servo_time < SERVO_INTERVAL )
        continue;
// It is time! Reset the servo clock.
      //rgb.theaterChase(0,255,0,0);
      servo_time -= SERVO_INTERVAL;
// Keep track of maximum lateness
      if ( max_servo_late < servo_time )
        max_servo_late = servo_time;
// Let's have some slop in counting late cycles.
      if ( servo_time > 50 )
        servo_late += 1;
    
      // handle the millisecond clocks
      unsigned long current_millis = millis();
      int millis_increment;
      if ( current_millis > last_millis )
        millis_increment = current_millis - last_millis;
      else // rollover
        millis_increment = (MAX_ULONG - last_millis) + current_millis;
      last_millis = current_millis;
  
      run_time += millis_increment;
      debug_print_time += millis_increment;
/*****************************************
Administrative part */
// Turn off after a while to keep from running forever
      if ( ( run_time > run_time_limit ) || ( button_pressed ) )
        {
          motor_stop();
          integrator_state = 0;
          button_pressed = false;
          rgb.theaterChase(255,0,0,0);
          return;
        }
/*****************************************
State estimation part */
// Read the sensors
      Read_encoders( &left_angle, &right_angle );
      // Current accelerometer and gyro zero values.
      int16_t ax, ay, az;
      int16_t gx, gy, gz;
// accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      accelgyro.getAcceleration( &ax, &ay, &az );
      gx = accelgyro.getRotationX();
      // subtract zeros
      int axx = ax - ax0;
      int ayy = ay - ay0;
      // adjustment of az is not useful.
      int azz = az;
      int gxx = gx - gx0;
      // int gyy = gy - gy0;
      // int gzz = gz - gz0;
float gscale = 0.001;
      float Kf_body = 0.01;
getDistance();
// prediction step
      int body_angle = last_body_angle + gscale*(gxx + last_gxx);
      // measurement update
      body_angle = body_angle - Kf_body*(body_angle - ayy);
if (abs(body_angle) > 10000) {
        motor_stop();
        integrator_state = 0;
        button_pressed = false;
        rgb.theaterChase(255,0,0,0);
        return;
      }
last_body_angle = body_angle;
      last_gxx = gxx;
int body_angular_velocity = gxx;
position_error_last = position_error;
      position_error = distance - distance_setpoint;
      position_error_deriv = (position_error-position_error_last) / 0.003; //3ms
      position_integrator_state += 0.003*position_error; //3ms
float Kp_pos = 50; //6
      float Kd_pos = 50;//0.06
      float Ki_pos = 0.1; // 6
      
      body_angle_setpoint = -1*(-Kp_pos*position_error - Ki_pos*position_integrator_state - Kd_pos*position_error_deriv);
      if (body_angle_setpoint < -MAX_SETPOINT) {
        body_angle_setpoint = -MAX_SETPOINT;
      }
      if (body_angle_setpoint > MAX_SETPOINT) {
        body_angle_setpoint = MAX_SETPOINT;
      }
      body_angle_error = body_angle - body_angle_setpoint;
Serial.print(position_error); Serial.print("\t"); Serial.print(body_angle_setpoint); Serial.print("\t"); Serial.println(body_angle);
      integrator_state += body_angle_error*0.003; //3ms cycle time, can technically be omitted and Ki arbitrary
long command_long = 0;
    
      if ( run_time < START_COLLECT + START_BALANCE )
        { // Do nothing;
      	}
      else // Let's balance!
        {
          command_long =  body_angle_gain*body_angle_error +
	                        body_angular_velocity_gain*body_angular_velocity +
    	                    body_angle_integral_gain*integrator_state;
          command_long = command_long >> 8;			 
        }
int command = (int) command_long;
if ( command > MAX_COMMAND )
        command = MAX_COMMAND;
      if ( command < -MAX_COMMAND )
        command = -MAX_COMMAND;
//command = 0;
      motor_left_command( command );
      motor_right_command( command );
    }
}
/**********************************************************************/
float body_angle_gain = 50;
float body_angle_integral_gain = 0;
float body_angular_velocity_gain = 1.5;
void loop() {
  if (button_pressed) {
    rgb.theaterChase(0,255,0,0);
    button_pressed = false;
    Encoder_init( &left_angle, &right_angle ); // set wheel encoders to zero
    //               Kp                           Kd            Ki    Tf    Data collect
    //For now let the max time be 2 mins, can be removed later b/c button will deactivate robot
    run_balancer( body_angle_gain, body_angular_velocity_gain, body_angle_integral_gain, 120000, 0);
  } else {
    motor_stop();
  }
  delay(50);
}
  
/**********************************************************************/

Possible Improvements and Next Steps

One possible improvement would be to make the robot follow faster changes in the target's position. The robot also currently turns by a constant amount every time the target is not straight ahead and trying different ways to determine how much to turn could also lead to improvements.