root/trunk/electronics/mcc/motorcontroller.c

#include <avr/io.h>
#include <util/delay.h>
#include <stdint.h>
#include <avr/iom168.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>

// macros for setting bits
#define sbi(var, mask)   ((var) |= (uint8_t)(1 << mask))
#define cbi(var, mask)   ((var) &= (uint8_t)~(1 << mask))
#define ABS(x)           ((x>0)?(x):(-x))
#define CAP(speed,max)   ((speed<max)?(speed):(max))


// constant pin locations
const uint8_t L_DIS = 4;
const uint8_t R_DIS = 5;

// setup EEPROM
uint8_t EEMEM EEmax_speed=0xAF;
uint8_t EEMEM EEmax_change=0x01;

// global variables for ramping speed
// default to stop, no speed
volatile uint8_t count;
volatile uint8_t ldest_speed    = 0x00;
volatile uint8_t ldest_dir              = 0x01;
volatile uint8_t rdest_speed    = 0x00;
volatile uint8_t rdest_dir              = 0x01;
volatile uint8_t max_speed;
volatile uint8_t max_change;

void serial_init(long baud)
{
        UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8;
        UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1);

        UCSR0B = (1<<RXEN0) | (1<<TXEN0);
        UCSR0C = (1<<USBS0) | (3 << UCSZ00);
}

uint8_t usart_get( void )
{
    while ( (UCSR0A & _BV(RXC0)) == 0 )
        ;
    return UDR0;
}


void usart_put( uint8_t b )
{
    while ( (UCSR0A & _BV(UDRE0)) == 0 )
        ;
    UDR0 = b;
}

// interrupt every 16ms
void timer_init()
{
        TCCR0A = 0b00000000;
        TCCR0B = 0b00000101;
        OCR0A = 250;
        TIMSK0 = 0b00000010;
}


void pwm_init()
{
        DDRB |= _BV(PB1);
        DDRB |= _BV(PB2);

        /*
         * TCCR1A:, datasheet pg. 131
         * [COM1A1][COM1A0][COM1B1][COM1B0][-][-][WGM11][WGM10]
         * Channel A:  Phase & Freq. correct
         *      OC1A set on match counting up
         *      OC1A cleared on match counting down
         *      OC1B cleared on match counting up
         *      OC1B set on match counting down
         *
         */
//      TCCR1A = 0b10110000;
        TCCR1A = 0b11110000;
        /*
         * TCCR1B:, datasheet pg. 133
         * [ICNC1][ICES1][-][WGM13][WGM12][CS12][CS11][CS10]
         * Set TOP to ICR1
         * CS1 = 0b001, ICR1 = 0x03FF: 7820Hz (10-bit resolution)
         * CS1 = 0b001, ICR1 = 0x0200: 15655Hz (9-bit resolution)
         */
        TCCR1B = 0b00010001;

        /* ICR1 defines TOP */
        ICR1 = 0x0200;

        OCR1A = ICR1>>1;
        OCR1B = ICR1>>1;
}

void io_init()
{
        DDRB = 0b11111111;
        DDRC = 0b11111111;
        DDRD = 0b11111110;
}

void eeprom_init()
{
        max_speed = eeprom_read_byte(&EEmax_speed);
        max_change = eeprom_read_byte(&EEmax_change);
}

void set_motors(uint8_t speed, uint8_t dir, uint8_t motor)
{
        enum {forward=0,stop,backward};
        enum {lmotor=0xF0,rmotor=0x0F};
        switch(motor)   {
        case lmotor:
                ldest_speed = CAP(speed,max_speed);
                ldest_dir = dir;
                if(dir == stop) {
                        ldest_speed = ICR1>>1;
                        OCR1A = ICR1>>1;
                        sbi(PORTB,L_DIS);
                }
                else
                        cbi(PORTB,L_DIS);
                break;

        case rmotor:
                rdest_speed = CAP(speed,max_speed);
                rdest_dir = dir;
                if(dir == stop) {
                        rdest_speed = ICR1>>1;
                        OCR1B = ICR1>>1;
                        sbi(PORTB,R_DIS);
                }
                else
                        cbi(PORTB,R_DIS);
                break;
        default:
                break;

        }


}


int main(void)
{
        enum {motor_state=0,direction_state,speed_state,checksum_state};
        enum {forward=0,stop,backward};
        enum {lmotor=0xF0,rmotor=0x0F};

        uint8_t state = motor_state;
        uint8_t in;
        uint8_t temp;
        uint8_t motor=0;
        uint8_t speed=0;
        uint8_t dir=0;

        eeprom_init();
        serial_init(9600);
        io_init();
        pwm_init();
        timer_init();           // interrupt every 16 ms
        sei();                          // enable interrupts

        while (1) {
                in = usart_get();
                switch (state) {
                case motor_state :
                        motor = in;
                        state = speed_state;
                        break;

                case speed_state :
                        speed = in;
                        state = direction_state;
                        break;

                case direction_state :
                        dir = in;
                        state = checksum_state;
                        break;

                case checksum_state :
                        temp = (motor ^ (3*speed) ^ (5*dir)) ^ in; // auchter's crazy checksum
                        usart_put(temp);
                        if (!temp)
                                set_motors(speed,dir,motor);
                        state = motor_state;
                        break;

                default:
                        state = motor_state;
                        break;
                }
        }
}

// periodic interrupt, currently every 16ms
// ramps speed to desired speed
ISR(TIMER0_COMPA_vect)
{
        uint8_t templ;
        uint8_t tempr;
        templ = (ICR1>>1) + ldest_speed * (ldest_dir - 1);
        tempr = (ICR1>>1) + rdest_speed * (rdest_dir - 1);
        OCR1A += (templ > OCR1A ? max_change : -max_change);
        OCR1B += (tempr > OCR1B ? max_change : -max_change);
        OCR1A = ABS(OCR1A - templ) < max_change ? templ : OCR1A;
        OCR1B = ABS(OCR1B - tempr) < max_change ? tempr : OCR1B;
}