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 ) {

#ifndef FFPP

        // Configure counter such that we use phase correct

        // PWM with 8-bit resolution

        DDRB |= 0x60;

        //timer 1A and 1B

        TCCR1A = 0xA1;

        TCCR1B = 0x04;

        OCR1AH=0;

        OCR1AL=0;

        OCR1BH=0;

        OCR1BL=0;

#else

        // Configure counter such that we use phase correct

        // PWM with 8-bit resolution at a frequency of

        // PWM is mode 1, Freq = Clock I/O/1024. 

        // OC1A and OC1B as pwm.

        OCR1A = 0x00;

        OCR1B = 0x00;

        PORTD &= 0x0F;

        DDRD |= 0xF0;

        DDRB |= 0x60;

        TCCR1A = 0xA1;

        TCCR1B = 0x05;

#endif

}

// The following functions set the motor direction and speed

void motor1_set(int direction, int speed) {

#ifndef FFPP

        if(direction == 0) {

                PORTD |= 0x10;

                PORTD &= 0xBF;

        }

        else {

                PORTD |= 0x40;

                PORTD &= 0xEF;

        }

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

        OCR1AL = speed;

#else

        if(direction == FORWARD) {

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

        }

        else {

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

        }

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

        OCR1A = speed;

#endif

}

void motor2_set(int direction, int speed) {

#ifndef FFPP

        if(direction == 0) {

                PORTD |= 0x20;

                PORTD &= 0x7F;

        }

        else {

                PORTD |= 0x80;

                PORTD &= 0xDF;

        }

        OCR1BL = speed;

#else

        if(direction != FORWARD) {

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

        }

        else {

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

        }

        OCR1B = speed;

#endif

}

// Just turns off both motors

void motors_off( void ) {

        OCR1AL = 0x0;

        OCR1BL = 0x0;

}