Created
December 28, 2011 10:38
-
-
Save johnhowe/1527512 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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 | |
Stephan Boyer, 2011 | |
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 (-M_PI-0.1f) | |
// 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(void) { | |
// read the voltage on the gyro pin | |
float adc_val = adc_read(0); | |
// low pass filter it (this will need to be adjusted if you change how often this function is called) | |
gyro_neutral = gyro_neutral*0.9999f+adc_val*0.0001f; | |
// do the units conversion and return the result | |
return (adc_val-gyro_neutral)*9.58972116f; // rad/sec | |
} | |
//////////////////////////////////////////////////////////////////////////////// | |
// main logic | |
//////////////////////////////////////////////////////////////////////////////// | |
// how actively the motor corrects for tilt | |
#define MOTOR_GAIN 260.0f | |
// the maximum angle before the motor shuts off | |
#define MAX_ANGLE (M_PI*0.35f) | |
// 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(); | |
// 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 | |
if (angle > 0.0f) | |
motor_speed += angle*angle*MOTOR_GAIN; | |
else | |
motor_speed -= angle*angle*MOTOR_GAIN; | |
// derivative term | |
motor_speed += angular_vel*0.3f; | |
// 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 > 1.0f) | |
motor_speed = 1.0f; | |
if (motor_speed < -1.0f) | |
motor_speed = -1.0f; | |
// 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 neatural point for the gyro | |
gyro_neutral = 0.27f; | |
// 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); | |
// set up TIMER1 to go off at 625 Hz exactly | |
OCR1A = 0x007C; | |
TCCR1A = 0b00000000; | |
TCCR1B = 0b00001100; | |
TIMSK1 = 0b00000010; | |
// give the capacitors on the power rails some time to charge | |
_delay_ms(1000); | |
// 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