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"

/**

 * @defgroup motors Motors

 * @brief Functions for controlling the motors.

 * Functions for controlling the motors. Found in motor.h.

 * @{

 **/

/**

 * Initializes both motors so that they can be used with future

 * calls to motor1_set and motor2_set.

 *

 * @see motors_off, motor1_set, motor2_set

 **/

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;

}

/**

 * Sets the speed and direction of motor1.

 * motors_init must be called before this function can be used.

 *

 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.

 * @param speed The speed the motor will run at, in the range 0-255.

 * 

 * @see motor2_set, motors_init

 **/

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;

}

/**

 * Sets the speed and direction of motor2.

 * motors_init must be called before this function can be used.

 *

 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.

 * @param speed The speed the motor will run at, in the range 0-255.

 *

 * @see motor1_set, motors_init

 **/

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;

}

/**

 * Turns off both motors.

 *

 * @see motors_init

 **/

void motors_off( void ) {

        OCR1AL = 0x0;

        OCR1BL = 0x0;

}

/**@}**///end defgroup