Introduction: 16-299 Final Project: Bottle Balancing Robot
My final project made use of the Elegoo Tumbller robot. I programmed this two wheeled robot to balance itself. Then I took it even further by placing it under a variety of conditions and developing a controller that worked regardless.
Supplies
https://www.elegoo.com/collections/robot-kits/products/elegoo-tumbller-self-balancing-robot-car
Various household objects to use as weights
Tape to secure the objects
A half filled bottle of fluid
Step 1: Balancing Without Payload
This step was done as a part of the lab for homework 6. In order to balance the robot we made use of two different sensors, the MPU6050 which has both an accelerometer and a gyroscope, and the wheel encoders in the motors. By combining these sensors we can build a program that can approximate the robot position, speed, tilt, and tilt speed by sampling these sensors.
Once we reach that point we now actually have feedback we can use to control the robot. By aiming to bring the robot speed, tilt, and tilt speed to zero, we can design a controller that can balance the robot from a wide variety of initial conditions while also being very simple.
The easiest way to do this is PID. PID stands for proportion, integral, and differential. This is the simplest type of controller to implement and I didn't even need to use all three to make the robot balance. I made use of a PD controller which only uses two parts. The proportion linearly scales the motor output with distance from the goal. The differential focuses on the speed and preventing overshoot. The differential represents the speed term of your system so as you approach the ideal point of balancing you slow down as to avoid completely overshooting it. In contrast, a P controller is unable to slow down until after it overshoots and an undamped P controller would oscillate forever.
Step 2: Adding Payloads
After getting the initial balance setup working and improving it so that it could start from all angles I decided to take the first step towards improving the controls by making able to handle payloads. I started with several smaller objects like a coaster and deck of cards before moving up to using the pot lid. In order to make these work I needed to tweak the gains of my controller. Primarily I needed to increase the kP of my PID as the system needed to apply more force from the starting position in order to lift the additional weight.
After that it was merely a matter of tweaking the kD until the robot was once again able to balance all on its own.
Step 3: Trying the Bottle
After testing on a variety of household objects I decided to move onto the most challenging one. The half full bottle of oil. This is more challenging than any of the other objects because even though it weighs less than the pot lid, it has a shifting center of mass. As the robot moves the liquid inside the bottle is perturbed from one side to the other resulting in a rapidly shifting center of mass. As the mass of the system shifts, so do the system dynamics. The PD controller that I had developed and was using up until this point was insufficient to balance the bottle as you can see in the video above. Therefore I decided to take the path we discussed in homework 6. By evaluating the system response I could develop a stronger controller using LQR.
Step 4: Improving the Bottle
Now before I could start collecting data I needed to improve my algorithm to the point that it could at least balance on its own a bit so that I could have some data collected over a period. By improving the gains and starting the robot at a slightly less severe angle, as you can see from the video I was able to make the robot balance.
While this controller wasn't perfect as is clearly visible from all of the oscillation present. It was enough to generate the data I needed to develop my LQR controller. To start I needed to identify the data I wanted to extract. To do this I needed to identify the key aspects of the system like the states and actions of the robot. The robot state is the collection of variables that define the robot's existence in a given moment and have all of the information to control it. For example not only are robot tilt and position states, but so to are the speed of the tilt and position. The actions are inputs the robot can add to the system that changes it from acting according to the natural dynamics of the world. In this case it is motor power. As such I am designing a controller that evaluates the state based on its internal algorithm and provides a motor power to reach the target state. By starting this controller at several different angles and recording the data, I was then able to perform a regression using the Matlab code provided for us in homework 6 and get the values to build an LQR controller.
Step 5: Supreme Bottle Balancing
After performing the regression with the matlab code I was able to generate the A, B, and C matrices of the system. The A matrix represents the systems dynamics and behavior without any interference. The B matrix shows the effects of the action on the state of the system. Finally, the C matrix represents how the state of the system can be translated to the outputs that the user needs.
A = [ 1 .004 0 0 | 0 .99999 .00031 -.00084 | 0 0 1 .004 | 0 .00304 .88643 .01056 ]
B = [ 0 | -.000002 | 0 | .000017 ]
C = [1 0 0 0 | 0 0 1 0 | 0 0 0 1]
After this I used these matrices with the dlqr command and the Q and R filter and lqr matrices in Homework 6 to get my final gains.
These final gains were K = [ 10 | 20 | 550 | 75 | 0.0345 | 0 ]
As you can clearly see above, this controller massively outperforms the previous ones reaching the steady state balance much more quickly and staying there the entire time the robot is moving. In the video above, the robot is performing the maximally smooth/minimum jerk trajectory that we discussed in class. The fact that the robot is able to remain fully balanced despite the presence of this feedforward trajectory with such a volatile payload says great things about the potential of this controller. While there are certainly more things to test and explore with this robot, I am very satisfied with what I was able to develop for this project.
/**********************************************************************
Run a balancing servo off a clock.
Switch to SERVO_INTERVAL = 0.004
Filter the command, not the wheel velocity
Add a dead zone on the command.
Move to first order filtering on wheel angle to produce wheel velocity
Add a launch behavior
Commented out az, only getting ay now
NOPE * Need to have "I2Cdev" folder in Arduino libraries
* Need to have "MPU6050" folder in Arduino libraries
/**********************************************************************/
// #include "I2Cdev.h"
#include "MPU6050.h"
#include "Pins.h"
#include "My_encoders.h"
/**********************************************************************/
// milliseconds
#define RUN_DURATION 30000
#define DO_SINUSOID 0
#define DO_MIN_JERK 1
#define MAX_ULONG 0xFFFFFFFF
// I had to increase this to 4ms to accomodate more computation/tick
// microseconds
#define SERVO_INTERVAL 4000
// These are in seconds
#define SERVO_INTERVAL_F (SERVO_INTERVAL*(1e-6))
#define ONE_OVER_SERVO_INTERVAL (1.0/SERVO_INTERVAL_F)
// start data collection N milliseconds after starting servo
#define START_COLLECT 5000
// start balancer N milliseconds after START_COLLECT
#define START_BALANCE 100
// start safety checks N milliseconds after START_COLLECT
#define START_SAFETY 1000
// start movements N milliseconds after START_BALANCE
#define START_MOVEMENT 5000
// milliseconds,
#define DEBUG_PRINT_INTERVAL 500
// The maximum command allowed
#define MAX_COMMAND 255
// #define PI 3.141592653589793
#define ENCODER_TO_RADIANS 0.004027682889218
#define GSCALE_TO_ACC 0.001575
// convert Y accelerometer to radians
#define ASCALE 5.575380291904716e-05
// convert gyro to radians/second
#define GSCALE 1.352011295799561e-04
// this is 0.5*Ts*GSCALE
// #define HALF_TS_GSCALE 2.0280e-07 // Ts = 0.003
#define HALF_TS_GSCALE 2.704022591599122e-07 // Ts = 0.004
// entries in check vector
#define ENCODER_DIFFS_SUM2 0
#define ACCELEROMETER_SUM 1
#define GYRO_SUM 2
// Safety limit (radians)
#define BODY_ANGLE_LIMIT 0.5
// Generic size of arrays
#define NN 10
#define DO_NOT_COLLECT_DATA 0
#define PLEASE_COLLECT_DATA 1
#define MIN_JERK_MOVEMENT_SIZE (4.0*PI)
/**********************************************************************/
// Enable for running a servo
bool go = false;
// Encoder readings: these can be positive or negative.
long left_angle = 0;
long right_angle = 0;
// Wheel desired angle
float drive_frequency = 0.0; // sinusoid frequency in Hz
float freq_2PI_over_1000 = 0; // time scaling factor
float desired_angle_amplitude = 0;
float desired_velocity_amplitude = 0;
float wheel_desired_angle = 0;
float wheel_desired_velocity = 0;
// Wheel velocity variables
float wheel_angle = 0;
float last_wheel_angle = 0;
// Command filter variables
float past_raw_commands[NN];
float past_filtered_commands[NN];
MPU6050 accelgyro;
// old approach to setting zeros
int ax0 = 0;
// int ay0 = -590; // -975; // -661; // 22; // 260; // 589; // 219;
// adjustment of az has to be done separately
int gx0 = -110;
int gy0 = 91.23;
int gz0 = -75.55;
// now hand adjusting ay0
int ay0 = -266;
// body angle Kalman filter gain
float Kf_body = 0.005; // 0.0001 and 0.001 too low, 0.1 too high
// Used in body angle Kalman filter
float last_body_angle = 0;
long last_gxx = 0; // last zero adjusted X gyro reading
// These two are used to handle the wierd gyro saturation behavior
long last_gx = 0; // last raw X gyro reading
long last_last_gx = 0; // previous to last raw 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?
// The servo gains
/*
// Original gains
float wheel_angle_gain = 3.0697; // command/radian
float wheel_angular_velocity_gain = 38.0003; // command/(radians/second)
float body_angle_gain = 1055.6959; // command/radian
float body_angular_velocity_gain = 113.1936; // command/(radians/second)
// Q 1 100 1 1; R 0.1 - unstable?
float wheel_angle_gain = 2.940576540698; // command/radian
float wheel_angular_velocity_gain = 106.482698970935; // command/(radians/second)
float body_angle_gain = 2538.632312883816; // command/radian
float body_angular_velocity_gain = 299.026185181349; // command/(radians/second)
// Q 1 100 1 1; R 1
float wheel_angle_gain = 0.962583708633; // command/radian
float wheel_angular_velocity_gain = 51.701738210705; // command/(radians/second)
float body_angle_gain = 1344.088947648885; // command/radian
float body_angular_velocity_gain = 149.848009296931; // command/(radians/second)
// Q 1 100 1 1; R 10 good, especially with -500 ay0
float wheel_angle_gain = 0.306904187685; // command/radian
float wheel_angular_velocity_gain = 35.787021312886; // command/(radians/second)
float body_angle_gain = 1010.702758072962; // command/radian
float body_angular_velocity_gain = 108.024691090534; // command/(radians/second)
// Q 1 100 1 1; R 1000000, collected data with these
float wheel_angular_velocity_gain = 31.5997905221488; // command/(radians/second)
float body_angle_gain = 926.2722415000157; // command/radian
float body_angular_velocity_gain = 97.3938972633543; // command/(radians/second)
// Q 100 1 1; R 1000000, new model
float wheel_angular_velocity_gain = 23.6682434820394; // command/(radians/second)
float body_angle_gain = 639.9856507938772; // command/radian
float body_angular_velocity_gain = 94.5450730117037; // command/(radians/second)
*/
// Q 100 1 1; R 1000000, new new model
float wheel_angular_velocity_gain = 20; // command/(radians/second)
float body_angle_gain = 550; // command/radian
float body_angular_velocity_gain = 75; // command/(radians/second
float past_command_gain = 0.038; // command/command_units
// override LQR designs above.
float wheel_angle_gain = 10;
int target_distance = .3; // in m
// Low pass filters
/*
// From Matlab butter( 1, 0.001 )
float a_filter[2] = {1.000000000000000, -0.996863331833438};
float b_filter[2] = {0.001568334083281, 0.001568334083281};
// From Matlab butter( 1, 0.002 )
float a_filter[2] = {1.000000000000000, -0.993736471541615};
float b_filter[2] = {0.003131764229193, 0.003131764229193};
// From Matlab butter( 1, 0.005 )
float a_filter[2] = {1.000000000000000, -0.984414127416097};
float b_filter[2] = {0.007792936291952, 0.007792936291952};
// From Matlab butter( 1, 0.01 )
float a_filter[2] = {1.000000000000000, -0.969067417193793};
float b_filter[2] = {0.015466291403103, 0.015466291403103};
// From Matlab butter( 1, 0.02 )
float a_filter[2] = {1.000000000000000, -0.939062505817492};
float b_filter[2] = {0.030468747091254, 0.030468747091254};
// From Matlab butter( 1, 0.05 )
float a_filter[2] = {1.000000000000000, -0.854080685463467};
float b_filter[2] = {0.072959657268267, 0.072959657268267};
*/
// From Matlab butter( 1, 0.1 )
float a_filter[2] = {1.000000000000000, -0.726542528005361};
float b_filter[2] = {0.136728735997319, 0.136728735997319};
/*
// From Matlab butter( 1, 0.15 )
float a_filter[2] = {1.000000000000000, -0.612800788139932};
float b_filter[2] = {0.193599605930034, 0.193599605930034};
// From Matlab butter( 1, 0.2 )
float a_filter[2] = {1.000000000000000, -0.509525449494429};
float b_filter[2] = {0.245237275252786, 0.245237275252786};
// From Matlab butter( 1, 0.3 )
float a_filter[2] = {1.000000000000000, -0.324919696232906};
float b_filter[2] = {0.337540151883547, 0.337540151883547};
// From Matlab butter( 1, 0.4 )
float a_filter[2] = {1.000000000000000, -0.158384440324536};
float b_filter[2] = {0.420807779837732, 0.420807779837732};
// From Matlab butter( 1, 0.5 )
float a_filter[2] = {1.0, 0.0};
float b_filter[2] = {0.5, 0.5};
// From Matlab butter( 1, 0.7 )
float a_filter[2] = {1.000000000000000, 0.324919696232906};
float b_filter[2] = {0.662459848116453, 0.662459848116453};
// From Matlab butter( 1, 0.9 )
float a_filter[2] = {1.000000000000000, 0.726542528005361};
float b_filter[2] = {0.863271264002680, 0.863271264002680};
*/
int launch_duration = 0;
// Some checks to look at at the end of a run
long checks[NN];
int check_counter = 0;
int dead_zone = 0; // dead zone to deal with stiction
float movement_duration = 1;
float one_over_movement_duration = 1.0/movement_duration;
float scaled_time = 0;
int past_command = 0;
/**********************************************************************/
/**********************************************************************/
// Controlling the motor
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 );
}
}
unsigned long get_distance_prev_time = 0;
float distance = 0;
void measureDistance()
{
if (digitalRead(A3)){
get_distance_prev_time = micros();
}
else{
distance = (micros() - get_distance_prev_time) *.017;
}
}
void getDistance()
{
digitalWrite(11, LOW);
delayMicroseconds(2);
digitalWrite(11, HIGH);
delayMicroseconds(10);
digitalWrite(11, LOW);
}
/**********************************************************************/
// Initializations and instructions to user
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
while( !Serial ); // Wait for serial port to become ready
Serial.println( "balance10 version 1" ); // print out what code is running
delay(1000); // Delay to make sure above message prints out.
pinMode(A3, INPUT);
pinMode(11, OUTPUT);
attachInterrupt(digitalPinToInterrupt(A3), measureDistance, CHANGE);
motor_init(); // Initialize motors
Serial.println( "motor_init done." );
delay(1000); // Delay to make sure above message prints out.
Encoder_init( &left_angle, &right_angle ); // Initialize (zero) encoders
Serial.println( "Initialized encoders" );
delay(1000); // Delay to make sure above message prints out.
accelgyro.initialize(); // Initialize IMU
Serial.println( "IMU initialized" );
delay(1000); // Delay to make sure above message prints out.
Serial.println( "Robot should be standing up with training wheels." );
Serial.println( "Type g <return> to run test, s <return> to stop." );
Serial.println( "Typing window is at the top of the Arduino serial monitor window." );
Serial.println( "Type into the main window of a Putty serial monitor window." );
// code starts disabled, so every time Arduino powers up nothing happens
go = false;
}
/******************************************************************/
// Get input from user
void ProcessCommand()
{
if ( Serial.available() <= 0 )
return;
int c = Serial.read();
switch (c)
{
case 'S': case 's':
Serial.println( "Stop!" );
go = false;
break;
case 'G': case 'g':
Serial.println( "Go!" );
go = true;
break;
default:
break;
}
}
/**********************************************************************
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 "g" command.
START_BALLANCE starts balancer (START_COLLECT+START_BALANCE) milliseconds
after "g" command.
DEBUG_PRINT_INTERVAL milliseconds between debug printouts when not doing
data collection
/**********************************************************************/
void run_balancer( int launch_duration,
float wheel_angle_gain, float wheel_angular_velocity_gain,
float body_angle_gain, float body_angular_velocity_gain,
float past_command_gain,
float some_kind_of_integral_gain, // placeholder
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;
// other initializations
last_body_angle = 0;
last_gx = 0;
last_last_gx = 0;
last_gxx = 0;
int in_dead_zone = 0;
last_wheel_angle = 0;
for ( int i = 0; i < NN; i++ )
{
past_raw_commands[i] = 0;
past_filtered_commands[i] = 0;
}
// how many ticks has the servo executed (used to compute averages)
check_counter = 0;
float movement_start_time
= START_COLLECT + START_BALANCE + START_MOVEMENT;
wheel_desired_angle = 0;
wheel_desired_velocity = 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);
if ( collect_data )
{
Serial.print( "Current voltage " );
Serial.print( voltage );
Serial.print( " " );
Serial.println( voltage_raw );
}
// print out the balancer gains
if ( collect_data )
{
Serial.print( "Gains " );
Serial.print( wheel_angle_gain, 8 );
Serial.print( " " );
Serial.print( wheel_angular_velocity_gain, 8 );
Serial.print( " " );
Serial.print( body_angle_gain, 8 );
Serial.print( " " );
Serial.print( body_angular_velocity_gain, 8 );
Serial.print( " " );
Serial.print( past_command_gain, 8 );
Serial.print( " " );
Serial.println( some_kind_of_integral_gain, 8 );
}
// print out sinusoidal drive parameters
if ( collect_data )
{
Serial.print( "drive_frequency " );
Serial.println( drive_frequency );
Serial.print( "freq_2PI_over_1000 " );
Serial.println( freq_2PI_over_1000 );
Serial.print( "desired_angle_amplitude " );
Serial.println( desired_angle_amplitude );
Serial.print( "desired_velocity_amplitude " );
Serial.println( desired_velocity_amplitude );
}
// print out other parameters
if ( collect_data )
{
Serial.print( "SERVO_INTERVAL " );
Serial.println( SERVO_INTERVAL );
Serial.print( "launch_duration " );
Serial.println( launch_duration );
Serial.print( "Kf_body " );
Serial.println( Kf_body, 8 );
Serial.print( "dead_zone " );
Serial.println( dead_zone );
Serial.println( "command filter_cutoff 0.1" );
}
// print out IMU biases
if ( collect_data )
{
Serial.print( "IMU biases " );
Serial.print( ax0 );
Serial.print( " " );
Serial.print( ay0 );
Serial.print( " " );
Serial.print( gx0 );
Serial.print( " " );
Serial.print( gy0 );
Serial.print( " " );
Serial.println( gz0 );
}
// 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.
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 ) || ( !go ) )
{
motor_stop();
Serial.println( "Motor stopped." );
Serial.println( "Time ran out." );
Serial.println( "Type g <return> to run test, s <return> to stop." );
return;
}
/*****************************************
State estimation part */
// Read the sensors
Read_encoders( &left_angle, &right_angle );
int enc_diff = left_angle - right_angle;
checks[ENCODER_DIFFS_SUM2] += enc_diff*enc_diff;
// convert to radians
wheel_angle = -0.5*(ENCODER_TO_RADIANS)*((float) (left_angle + right_angle));
float wheel_velocity = ONE_OVER_SERVO_INTERVAL*(wheel_angle - last_wheel_angle);
last_wheel_angle = wheel_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 );
// This is type long so when add bias, don't flip sign
long ay = (long) (accelgyro.getAccelerationY());
// This is type "long" to handle wierd negative saturation
long gx = (long) (accelgyro.getRotationX());
// handle wierd negative saturation turns in to positive number on gyro
if ( (last_gx <= last_last_gx) && ( last_gx < 0 ) && ( gx > 32000 ) )
gx = -32761;
last_last_gx = last_gx;
last_gx = gx;
// subtract zeros: avoid sign flip by using longs
// long axx = (long) ax - ax0;
long ayy = (long) ay - ay0;
// adjustment of az is not useful.
// long azz = az;
long gxx = (long) gx - gx0;
// int gyy = gy - gy0;
// int gzz = gz - gz0;
checks[ ACCELEROMETER_SUM ] += ayy;
checks[ GYRO_SUM ] += gxx;
check_counter++;
// Body angle Kalman Filter
// prediction step
float body_angle = last_body_angle + HALF_TS_GSCALE*((float) (gxx + last_gxx));
// measurement update
body_angle -= Kf_body*(body_angle - (ASCALE)*((float) ayy));
last_body_angle = body_angle;
last_gxx = gxx;
// Safety code
if ( run_time > START_COLLECT + START_BALANCE + START_SAFETY )
{
if ( body_angle < -BODY_ANGLE_LIMIT )
{
motor_stop();
Serial.println( "Motor stopped." );
Serial.print( "Robot fell backward:" );
Serial.print( " " );
Serial.println( body_angle );
Serial.println( "Type g <return> to run test, s <return> to stop." );
return;
}
if ( body_angle > BODY_ANGLE_LIMIT )
{
motor_stop();
Serial.println( "Motor stopped." );
Serial.print( "Robot fell forward:" );
Serial.print( " " );
Serial.println( body_angle );
Serial.println( "Type g <return> to run test, s <return> to stop." );
return;
}
}
float body_angular_velocity = (GSCALE)*gxx;
/*****************************************
Control part:
*/
float command_float = 0;
int command = 0;
if ( run_time < START_COLLECT + START_BALANCE )
{ // Do nothing;
command = 0;
past_raw_commands[1] = command;
past_raw_commands[0] = command;
past_filtered_commands[0] = command;
}
else if ( run_time < START_COLLECT + START_BALANCE + launch_duration )
{ // Launch behavior
command = -MAX_COMMAND;
past_raw_commands[1] = command;
past_raw_commands[0] = command;
past_filtered_commands[0] = command;
}
else // Let's balance!
{
wheel_desired_angle = 0;
wheel_desired_velocity = 0;
float movement_time = (run_time - movement_start_time);
// Drive the robot with sinusoids
if ( movement_time > 0 && DO_SINUSOID )
{
scaled_time = freq_2PI_over_1000*movement_time;
// set up to go backwards first
wheel_desired_angle
= -desired_angle_amplitude*(1 - cos( scaled_time ));
wheel_desired_velocity
= -desired_velocity_amplitude*sin( scaled_time );
}
// Drive the robot with minimum jerk movements
static int min_jerk_direction = -1;
static int min_jerk_old_integer_part = 0;
if ( movement_time > 0 && DO_MIN_JERK )
{
scaled_time = 0.001*movement_time*one_over_movement_duration;
int integer_part = scaled_time; // integer conversion
scaled_time -= integer_part; // get fractional part of float
// now scaled time is between 0 and 1.
// check if we need to flip direction
if ( integer_part != min_jerk_old_integer_part )
{ // we need to flip direction (start a new movement)
min_jerk_direction = -min_jerk_direction;
min_jerk_old_integer_part = integer_part;
}
float scaled_time2 = scaled_time*scaled_time;
float scaled_time3 = scaled_time*scaled_time2;
float scaled_time4 = scaled_time*scaled_time3;
float scaled_time5 = scaled_time*scaled_time4;
if ( min_jerk_direction > 0 )
{
// positive direction sends us forward from -movement_size
wheel_desired_angle = -MIN_JERK_MOVEMENT_SIZE
+ MIN_JERK_MOVEMENT_SIZE
*(10.0*scaled_time3 - 15.0*scaled_time4 + 6.0*scaled_time5);
wheel_desired_velocity
= one_over_movement_duration*MIN_JERK_MOVEMENT_SIZE
*(30.0*scaled_time2 - 60.0*scaled_time3 + 30.0*scaled_time4);
}
else
{
// negative direction sends us backward from 0
wheel_desired_angle = -MIN_JERK_MOVEMENT_SIZE
*(10.0*scaled_time3 - 15.0*scaled_time4 + 6.0*scaled_time5);
wheel_desired_velocity =
-one_over_movement_duration*MIN_JERK_MOVEMENT_SIZE
*(30.0*scaled_time2 - 60.0*scaled_time3 + 30.0*scaled_time4);
}
}
// no integrator yet
/**
command_float =
- wheel_angle_gain * (wheel_angle - wheel_desired_angle)
- wheel_angular_velocity_gain
* (wheel_velocity - wheel_desired_velocity)
- body_angle_gain*body_angle
- body_angular_velocity_gain*body_angular_velocity
- past_command_gain*past_command;
**/
if(abs(body_angular_velocity) <= .05){
getDistance();
Serial.println(distance);
}
command_float = - body_angle_gain*body_angle
- body_angular_velocity_gain*body_angular_velocity
- wheel_angle_gain * (wheel_angle - wheel_desired_angle)
- wheel_angular_velocity_gain
* (wheel_velocity - wheel_desired_velocity)
- past_command_gain*past_command;
// apply filter
// a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
// - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
past_raw_commands[1] = past_raw_commands[0];
past_raw_commands[0] = command_float;
float filtered_command = b_filter[0]*past_raw_commands[0]
+ b_filter[1]*past_raw_commands[1]
- a_filter[1]*past_filtered_commands[0];
past_filtered_commands[0] = filtered_command;
if ( filtered_command > MAX_COMMAND )
filtered_command = MAX_COMMAND;
if ( filtered_command < -MAX_COMMAND )
filtered_command = -MAX_COMMAND;
command = (int) filtered_command;
// implement dead zone
if ( dead_zone > 0 )
{
if ( !in_dead_zone )
{ // not already in dead zone, command positive
if ( command > 0 )
{
if ( command <= (dead_zone >> 1) )
{
in_dead_zone = 1;
command = 0;
}
}
else // not already in dead zone, command negative
{
if ( command >= (-dead_zone >> 1) )
{
in_dead_zone = 1;
command = 0;
}
}
}
else
{ // already in dead zone
if ( command > 0 )
{
if ( command <= dead_zone )
command = 0;
else
in_dead_zone = 0;
}
else // in dead zone, command negative
{
if ( command >= -dead_zone )
command = 0;
else
in_dead_zone = 0;
}
}
}
}
past_command = command;
motor_left_command( command );
motor_right_command( command );
/*****************************************
Data printing part */
// print out debugging info before data collection
if ( ( debug_print_time > DEBUG_PRINT_INTERVAL )
&& ( ( run_time < START_COLLECT )
|| ( run_time > run_time_limit ) ) )
{
Serial.print( servo_late );
Serial.print( " " );
Serial.print( max_servo_late );
Serial.print( " " );
Serial.print( ayy );
Serial.print( " " );
Serial.println( body_angle, 4 );
/*
Serial.print( " " );
Serial.print( left_error );
Serial.print( " " );
Serial.print( right_error );
Serial.print( " " );
Serial.println( ramp );
*/
ProcessCommand();
debug_print_time -= DEBUG_PRINT_INTERVAL;
}
// print out data
if ( ( run_time >= START_COLLECT ) && ( run_time <= run_time_limit )
&& collect_data )
{
if ( !started )
{
servo_late = 0;
max_servo_late = 0;
started = 1;
Serial.println( "Data" );
}
Serial.print( current_micros );
Serial.print( " " );
Serial.print( left_angle );
Serial.print( " " );
Serial.print( right_angle );
Serial.print( " " );
Serial.print( command );
Serial.print( " " );
Serial.print( command );
Serial.print( " " );
// Serial.print( axx );
// Serial.print( " " );
Serial.print( ayy ); // positive is tipping forward
Serial.print( " " );
// Serial.print( azz ); // positive is down
// Serial.print( " " );
Serial.print( gxx ); // positive is body rolling forward
Serial.print( " " );
float v = 1000*body_angle; // print out more digits for body_angle
Serial.print( v );
Serial.print( " " );
Serial.println( wheel_velocity );
// Serial.print( scaled_time );
// Serial.print( " " );
// Serial.print( wheel_desired_angle );
// Serial.print( " " );
// Serial.println( wheel_desired_velocity );
// Serial.print( " " );
// Serial.println( gy );
// Serial.print( " " );
// Serial.println( gz );
// Serial.print( " " );
// Serial.println( left_velocity);
// Serial.print( " " );
// Serial.println( right_velocity);
// Serial.print( " " );
// Serial.print( left_angle_radians);
// Serial.print( " " );
// Serial.println( left_error );
}
}
}
/**********************************************************************/
void loop()
{
if ( !go )
{
motor_stop();
delay(500);
ProcessCommand();
return;
}
for ( int i = 0; i < NN; i++ )
checks[i] = 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);
Serial.print( "Volts: " );
Serial.println( voltage );
/*
Serial.print( "Dead_zone (" );
Serial.print( dead_zone );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
int dead_zone_typed = Serial.parseInt();
if ( dead_zone_typed == 0 )
dead_zone_typed = dead_zone;
Serial.print( "You typed: " );
Serial.println( dead_zone_typed );
if ( dead_zone_typed < 0 || dead_zone_typed > 300 )
{
Serial.println( "Bad value: Start over" );
return;
}
dead_zone = dead_zone_typed;
Serial.print( "Launch_duration (" );
Serial.print( launch_duration );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
int launch_duration_typed = Serial.parseInt();
if ( launch_duration_typed == 0 )
launch_duration_typed = launch_duration;
Serial.print( "You typed: " );
Serial.println( launch_duration_typed );
if ( launch_duration_typed < 0 || launch_duration_typed > 300 )
{
Serial.println( "Bad value: Start over" );
return;
}
launch_duration = launch_duration_typed;
Serial.print( "Ay0 (" );
Serial.print( ay0 );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
long ay0_typed = Serial.parseInt();
if ( ay0_typed == 0 )
ay0_typed = ay0;
Serial.print( "You typed: " );
Serial.println( ay0_typed );
if ( ay0_typed < -1000 || ay0_typed > 1000 )
{
Serial.println( "Bad value: Start over" );
return;
}
ay0 = ay0_typed;
Serial.print( "Wheel_angle_gain (" );
Serial.print( wheel_angle_gain );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float wheel_angle_gain_typed = Serial.parseFloat();
if ( wheel_angle_gain_typed == 0 )
wheel_angle_gain_typed = wheel_angle_gain;
Serial.print( "You typed: " );
Serial.println( wheel_angle_gain_typed );
if ( wheel_angle_gain_typed < -100 || wheel_angle_gain_typed > 100 )
{
Serial.println( "Bad value: Start over" );
return;
}
wheel_angle_gain = wheel_angle_gain_typed;
Serial.print( "wheel_angular_velocity_gain (" );
Serial.print( wheel_angular_velocity_gain );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float wheel_angular_velocity_gain_typed = Serial.parseFloat();
if ( wheel_angular_velocity_gain_typed == 0 )
wheel_angular_velocity_gain_typed = wheel_angular_velocity_gain;
Serial.print( "You typed: " );
Serial.println( wheel_angular_velocity_gain_typed );
if ( wheel_angular_velocity_gain_typed < -1000 || wheel_angular_velocity_gain_typed > 1000 )
{
Serial.println( "Bad value: Start over" );
return;
}
wheel_angular_velocity_gain = wheel_angular_velocity_gain_typed;
Serial.print( "Body_angle_gain (" );
Serial.print( body_angle_gain );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float body_angle_gain_typed = Serial.parseFloat();
if ( body_angle_gain_typed == 0 )
body_angle_gain_typed = body_angle_gain;
Serial.print( "You typed: " );
Serial.println( body_angle_gain_typed );
if ( body_angle_gain_typed < -10000 || body_angle_gain_typed > 10000 )
{
Serial.println( "Bad value: Start over" );
return;
}
body_angle_gain = body_angle_gain_typed;
Serial.print( "body_angular_velocity_gain (" );
Serial.print( body_angular_velocity_gain );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float body_angular_velocity_gain_typed = Serial.parseFloat();
if ( body_angular_velocity_gain_typed == 0 )
body_angular_velocity_gain_typed = body_angular_velocity_gain;
Serial.print( "You typed: " );
Serial.println( body_angular_velocity_gain_typed );
if ( body_angular_velocity_gain_typed < -1000 || body_angular_velocity_gain_typed > 1000 )
{
Serial.println( "Bad value: Start over" );
return;
}
body_angular_velocity_gain = body_angular_velocity_gain_typed;
Serial.print( "past_command_gain (" );
Serial.print( past_command_gain );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float past_command_gain_typed = Serial.parseFloat();
if ( past_command_gain_typed == 0 )
past_command_gain_typed = past_command_gain;
Serial.print( "You typed: " );
Serial.println( past_command_gain_typed );
if ( past_command_gain_typed < -100 || past_command_gain_typed > 100 )
{
Serial.println( "Bad value: Start over" );
return;
}
past_command_gain = past_command_gain_typed;
*/
if ( DO_SINUSOID )
{
Serial.print( "Drive_frequency (" );
Serial.print( drive_frequency );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float drive_frequency_typed = Serial.parseFloat();
if ( drive_frequency_typed == 0 )
drive_frequency_typed = drive_frequency;
Serial.print( "You typed: " );
Serial.println( drive_frequency_typed );
if ( drive_frequency_typed < -100 || drive_frequency_typed > 100 )
{
Serial.println( "Bad value: Start over" );
return;
}
drive_frequency = drive_frequency_typed;
freq_2PI_over_1000 = drive_frequency*2*PI/1000.0;
desired_angle_amplitude = 2*PI;
desired_velocity_amplitude = 4*PI*PI*drive_frequency;
}
if ( DO_MIN_JERK )
{
Serial.print( "Movement_duration (" );
Serial.print( movement_duration );
Serial.println( ")?" );
Serial.setTimeout( 100000 ); // 10 seconds
float movement_duration_typed = Serial.parseFloat();
if ( movement_duration_typed == 0 )
movement_duration_typed = movement_duration;
Serial.print( "You typed: " );
Serial.println( movement_duration_typed );
if ( movement_duration_typed < 0.01 || movement_duration_typed > 10 )
{
Serial.println( "Bad value: Start over" );
return;
}
movement_duration = movement_duration_typed;
one_over_movement_duration = 1.0/movement_duration;
}
Encoder_init( &left_angle, &right_angle ); // set wheel encoders to zero
run_balancer( launch_duration,
wheel_angle_gain, wheel_angular_velocity_gain,
body_angle_gain, body_angular_velocity_gain,
past_command_gain, 0.0,
RUN_DURATION, PLEASE_COLLECT_DATA );
Serial.println( "Stopped!" );
go = false;
Serial.print( "servo_late: " );
Serial.println( servo_late );
Serial.print( "max_servo_late: " );
Serial.println( max_servo_late );
Serial.print( "Average encoder diff^2: " );
Serial.println( checks[ENCODER_DIFFS_SUM2]/check_counter );
Serial.print( "Average Y accelerometer reading: " );
Serial.println( checks[ACCELEROMETER_SUM]/check_counter );
Serial.print( "Average X gyro reading: " );
Serial.println( checks[GYRO_SUM]/check_counter );
}
/**********************************************************************/
Attachments
Step 6: Ultrasonic Sensor Stuff
While I did develop a distance control algorithm for the robot to maintain a certain distance from the wall in front of it, I was unable to combine it with the bottle balancing. This is because as I was doing testing for the bottle I damaged the HC-SR24 ultrasonic sensor and it know is only capable of returning the max value. Because of this I decided to focus on the bottle balancing as it was the more challenging of the two. You can see the code I used to do this below.
void measureDistance()
{
if (digitalRead(A3)){
get_distance_prev_time = micros();
}
else{
distance = (micros() - get_distance_prev_time) *.017;
}
}
void getDistance()
{
digitalWrite(11, LOW);
delayMicroseconds(2);
digitalWrite(11, HIGH);
delayMicroseconds(10);
digitalWrite(11, LOW);
}
void setup()
{
...
attachInterrupt(digitalPinToInterrupt(A3), measureDistance, CHANGE);
...
}
void run_balancer(...)
{
...
if( abs( body_angular_velocity ) <= .05 )
{
getDistance();
if( distance != 0 )
{
wheel_desired_angle = distance / 15.71 * 1560;
}
}
...
}
Step 7: Possible Improvements and Next Steps
Well I am not currently sure how well this controller generalizes. It does a decent job without the bottle however because the fluid in the bottle almost acts like an extra damping factor, it does oscillate a little more without it. Also because the filter and lqr matrices in the homework example worked so well I did not try to modify them at all so it is possible by doing so I could develop an even better controller. I would like to see what happens if we try a bottle with a different fluid. I used this small oil bottle because it was convenient for me but what if we tried a water bottle. Would a less viscous fluid work as well? These are the kinds of questions I intend to continue testing as I move forward with this robot.
![Tim's Mechanical Spider Leg [LU9685-20CU]](https://content.instructables.com/FFB/5R4I/LVKZ6G6R/FFB5R4ILVKZ6G6R.png?auto=webp&crop=1.2%3A1&frame=1&width=306)
