Source code for the self-balancing unicycle described at: http://www.stephanboyer.com/post/17

bullet.c

#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
    [email protected]
    -----------------------

    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;
}