Create a gist now

Instantly share code, notes, and snippets.

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