Colony

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file motor.c

 * @brief Motors

 *

 * Implementation of functions for controlling the motors.

 *

 * @author Colony Project, CMU Robotics Club

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

 **/

#include "motor.h"

#include <avr/io.h>

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;

}

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;

}

void motors_off( void ) {

  OCR1AL = 0x0;

  OCR1BL = 0x0;

}