Source code for the self-balancing unicycle described at: http://www.stephanboyer.com/post/17
#define F_CPU 20000000 | |
#include <avr/io.h> | |
#include <avr/interrupt.h> | |
#include <util/delay.h> | |
#include <stdio.h> | |
#include <math.h> | |
/* | |
----------------------- | |
Self-Balancing Unicycle | |
Mon Apr 23 22:03:06 EDT 2012 | |
Stephan Boyer | |
boyers@mit.edu | |
----------------------- | |
This should be burned onto an ATMEGA328P. | |
To set the clock source to be an external | |
20MHz crystal, set the low fuse bit to 0x67 | |
like this: | |
avrdude -c usbtiny -p atmega328p -P com1 -U lfuse:w:0x67:m | |
pins: | |
PC0 - analog angular acceleration | |
PC1 - analog y-acceleration | |
PC2 - analog z-acceleration | |
PD5 - motor controller (forward) | |
PD6 - motor controller (backward) | |
PD7 - kill switch (high: "killed") | |
*/ | |
//////////////////////////////////////////////////////////////////////////////// | |
// utilities | |
//////////////////////////////////////////////////////////////////////////////// | |
// read from an analog input, returns value in range [0.0f, 1.0f] (corresponding to [0.0, 5.0] V) | |
float adc_read(uint8_t channel) { | |
ADMUX = 0b01000000|(channel&0b00001111); | |
ADCSRB = 0b00000000; | |
ADCSRA = 0b11000111; | |
while (ADCSRA&0b01000000); | |
uint8_t low = ADCL; | |
return ((float)((ADCH<<8)|low))/1023.0f; | |
} | |
// wrap an angle to the range [-pi/2, pi/2) | |
float wrap_angle(float angle) { | |
while (angle >= M_PI) | |
angle -= 2.0f*M_PI; | |
while (angle < -M_PI) | |
angle += 2.0f*M_PI; | |
return angle; | |
} | |
//////////////////////////////////////////////////////////////////////////////// | |
// motor controller interface | |
//////////////////////////////////////////////////////////////////////////////// | |
// set up the motor | |
void motor_init(void) { | |
// make sure the motor starts out off | |
PORTD &= ~(1<<5); | |
PORTD &= ~(1<<6); | |
// set the pin modes for the motor outputs | |
DDRD |= (1<<5); | |
DDRD |= (1<<6); | |
// set up TIMER0 to PWM at 312500 Hz exactly (PWM frequency: 1220.703125 Hz) | |
TCCR0A = 0b00000011; | |
TCCR0B = 0b00000010; | |
TIMSK0 = 0b00000000; | |
} | |
// set the speed of the motor from -1.0f to 1.0f | |
void set_motor_speed(float speed) { | |
// forward | |
if (speed > 0.0f) { | |
// disable PWM output on both motor pins | |
TCCR0A = 0b00000011; | |
// turn off the reverse output | |
PORTD &= ~(1<<6); | |
// set the PWM frequency | |
OCR0A = (uint8_t)(speed*255.5f); | |
// enable PWM output on the forward pin | |
TCCR0A = 0b10000011; | |
} | |
// reverse | |
if (speed < 0.0f) { | |
// disable PWM output on both motor pins | |
TCCR0A = 0b00000011; | |
// turn off the forward output | |
PORTD &= ~(1<<5); | |
// set the PWM frequency | |
OCR0B = (uint8_t)(-speed*255.5f); | |
// enable PWM output on the reverse pin | |
TCCR0A = 0b00100011; | |
} | |
// coast | |
if (speed == 0.0f) { | |
// disable PWM output on both motor pins | |
TCCR0A = 0b00000011; | |
// turn off both outputs | |
PORTD &= ~(1<<5); | |
PORTD &= ~(1<<6); | |
} | |
} | |
//////////////////////////////////////////////////////////////////////////////// | |
// IMU interface | |
//////////////////////////////////////////////////////////////////////////////// | |
// the offset to subtract from the accelerometer angle (depends on the desired orientation and how the sensor is mounted) | |
#define OFFSET_ANGLE 0.35f | |
// the gyro neutral point (adjusted over time) | |
volatile float gyro_neutral; | |
// initialize the IMU | |
void imu_init(void) { | |
// set the gyro pin to input mode | |
DDRC &= ~(1<<0); | |
// set the accelerometer pins to input mode | |
DDRC &= ~(1<<1); | |
DDRC &= ~(1<<2); | |
} | |
// compute the angle from the accelerometer (radians) | |
float get_accelerometer_angle(void) { | |
// read the voltages on the accelerometer pins | |
float y_acceleration = (adc_read(1)-0.386f)*163.333333f; // m/sec^2 | |
float z_acceleration = (adc_read(2)-0.386f)*163.333333f; // m/sec^2 | |
// compute the angle from the accelerometer readings | |
return wrap_angle(atan2(z_acceleration, y_acceleration)-OFFSET_ANGLE); | |
} | |
// get the angular velocity from the gyro (rad/sec) | |
float get_gyro_angular_velocity(int update_neutral_point) { | |
// read the voltage on the gyro pin | |
float adc_val = adc_read(0); | |
// do the units conversion | |
float angular_vel = (adc_val-gyro_neutral)*9.58972116f; // rad/sec | |
// low pass filter it (this will need to be adjusted if you change how often this function is called) | |
if (update_neutral_point) | |
gyro_neutral = gyro_neutral*0.9997f+adc_val*0.0003f; | |
return angular_vel; | |
} | |
//////////////////////////////////////////////////////////////////////////////// | |
// main logic | |
//////////////////////////////////////////////////////////////////////////////// | |
// proportional gain | |
#define MOTOR_GAIN_P 10.0f | |
// derivative gain | |
#define MOTOR_GAIN_D 0.4f | |
// larger value here means smaller "dead zone" | |
#define MOTOR_DEAD_ZONE 1200.0f | |
// the maximum angle before the motor shuts off | |
#define MAX_ANGLE (M_PI*0.4f) | |
// our current estimate of the angle | |
volatile float angle; | |
// the current power value (starts at 0 and ramps up to 1) | |
volatile float power_level; | |
// whether the motor is ready | |
volatile uint8_t motor_ready; | |
// TIMER1 interrupt handler | |
ISR(TIMER1_COMPA_vect) { | |
// get the angular velocity | |
float angular_vel = get_gyro_angular_velocity(1); | |
// integrate the value from the gyro | |
angle += angular_vel*0.0016f; | |
// compute the angle delta due to the accelerometer angle | |
float angle_delta = wrap_angle(get_accelerometer_angle()-angle); | |
// complementary filter | |
angle = wrap_angle(angle+0.001f*angle_delta); | |
// set the motor speed to compensate for tilt | |
float motor_speed = 0.0f; | |
// proportional term | |
motor_speed += angle*MOTOR_GAIN_P*(1.0f-exp(-(angle*angle)*MOTOR_DEAD_ZONE)); | |
// derivative term | |
motor_speed += angular_vel*MOTOR_GAIN_D; | |
// get the absolute value of the angle | |
float abs_angle = angle; | |
if (abs_angle < 0.0f) | |
abs_angle *= -1.0f; | |
// ramp the power level up if the angle is reasonable, down otherwise | |
if (abs_angle < MAX_ANGLE && !(PIND&(1<<7)) && motor_ready) | |
power_level += 0.0005f; | |
else | |
power_level -= 0.003f; | |
// clamp the power level | |
if (power_level > 1.0f) | |
power_level = 1.0f; | |
if (power_level < 0.0f) | |
power_level = 0.0f; | |
// cap the motor speed | |
if (motor_speed > 0.95f) | |
motor_speed = 0.95f; | |
if (motor_speed < -0.95f) | |
motor_speed = -0.95f; | |
// apply the power level | |
motor_speed *= power_level; | |
// set the motor speed to compensate for tilt | |
set_motor_speed(motor_speed); | |
} | |
// program entrypoint | |
int main(void) { | |
// set the system clock prescalar to 1 | |
CLKPR = 0x80; | |
CLKPR = 0x00; | |
// give a reasonable initial neutural point for the gyro | |
gyro_neutral = 0.343f; | |
// disable motor output during startup | |
motor_ready = 0; | |
// assume we start upright at the neutral angle | |
angle = 0.0f; | |
// start the power level at zero | |
power_level = 0.0f; | |
// enable interrupts | |
sei(); | |
// initialize the motor | |
motor_init(); | |
// initialize the IMU | |
imu_init(); | |
// set the kill switch pin to input mode | |
DDRD &= ~(1<<7); | |
// give the capacitors on the power rails some time to charge | |
_delay_ms(500); | |
// read from the ADC inputs a few times and discard the results | |
for (uint32_t i = 0; i < 1000; i++) { | |
// read from the accelerometer | |
get_accelerometer_angle(); | |
// read from the gyro | |
get_gyro_angular_velocity(0); | |
} | |
// set up TIMER1 to go off at 625 Hz exactly | |
OCR1A = 0x007C; | |
TCCR1A = 0b00000000; | |
TCCR1B = 0b00001100; | |
TIMSK1 = 0b00000010; | |
// start the motor | |
motor_ready = 1; | |
/////////////////////////////////// | |
// run forever | |
/////////////////////////////////// | |
// loop forever | |
while (1); | |
// never reached | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment