Untitled

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