Colony

/*

        motor.c - Contains functions necessary for activating and driving the

        H-bridge

        author: Robotics Club, Colony project

        much of this is taken from FWR's library, author: Tom Lauwers

*/

#include "motor.h"

/*

motor initialization

initializes both motors

*/

void motors_init( void ) {

        // Configure counter such that we use phase correct

        // PWM with 8-bit resolution

        PORTA &= 0x0F;

        DDRA |= 0xF0;

        DDRB |= 0x60;

        //timer 1A and 1B

        TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);

        TCCR1B = _BV(WGM12) | _BV(CS10);

//        TCCR1A = 0xA1;

//        TCCR1B = 0x04;

        OCR1AH=0;

        OCR1AL=0;

        OCR1BH=0;

        OCR1BL=0;

}

// The following functions set the motor direction and speed

void motor1_set(int direction, int speed) {

        if(direction == 0) {

            // turn off PWM first if switching directions

                if((PORTA & 0x30) != 0x10)

                {

                  OCR1A = 0;

                }

                PORTA = (PORTA & 0xCF) | 0x10;

//                PORTD |= 0x10;

//                PORTD &= 0xBF;

        }

        else {

            // turn off PWM first if switching directions

                if((PORTA & 0x30) != 0x20)

                {

                  OCR1A = 0;

                }

                PORTA = (PORTA & 0xCF) | 0x20;

//                PORTD |= 0x40;

//                PORTD &= 0xEF;

        }

        // Set the timer to count up to speed, an 8-bit value

        OCR1AL = speed;

}

void motor2_set(int direction, int speed) {

        if(direction == 0) {

//                PORTD |= 0x20;

//                PORTD &= 0x7F;

            // turn off PWM first if switching directions

                if((PORTA & 0xC0) != 0x80)

                {

                  OCR1B = 0;

                }

                PORTA = (PORTA & 0x3F) | 0x80;

        }

        else {

//                PORTD |= 0x80;

//                PORTD &= 0xDF;

            // turn off PWM first if switching directions

                if((PORTA & 0xC0) != 0x40)

                {

                  OCR1B = 0;

                }

                PORTA = (PORTA & 0x3F) | 0x40;

        }

        OCR1BL = speed;

}

// Just turns off both motors

void motors_off( void ) {

        OCR1AL = 0x0;

        OCR1BL = 0x0;

}