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 "dragonfly_defs.h"

#include "motor.h"

/**

 * @defgroup motors Motors

 * @brief Functions for controlling the motors.

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

 * @{

 **/

unsigned char motors_initd=0;

/**

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

 * calls to motor1_set and motor2_set.

 *

 * @return 0 if init succesfull, an error code otherwise

 *

 * @see motors_off, motor1_set, motor2_set

 **/

int motors_init( void ) {

  if(motors_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: motors already init'd\r\n");

    return ERROR_INIT_ALREADY_INITD;

  }

  // 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;

  motors_initd=1;

  return 0;

}

/**

 * Sets the speed and direction of the left motor.

 * 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.

 *

 * @return 0 if init succesfull, an error code otherwise

 *

 * @see motor_r_set, motors_init

 **/

int motor_l_set(int direction, int speed) {

  if(!motors_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: motors not init'd\r\n");

    return ERROR_LIBRARY_NOT_INITD;

  }

  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;

  return 0;

}

/**

 * Sets the speed and direction of the right motor.

 * 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.

 *

 * @return 0 if init succesfull, an error code otherwise

 *

 * @see motor_l_set, motors_init

 **/

int motor_r_set(int direction, int speed) {

  if(!motors_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    return ERROR_LIBRARY_NOT_INITD;

  }

  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;

  return 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.

 *

 * @return 0 if init succesfull, an error code otherwise

 *

 * @see motor2_set, motors_init

 **/

int motor1_set(int direction, int speed) {

  return motor_l_set(direction, 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.

 *

 * @return 0 if init succesfull, an error code otherwise

 *

 * @see motor2_set, motors_init

 **/

int motor2_set(int direction, int speed) {

  return motor_r_set(direction, speed);

}

/**

 * Turns off both motors.

 *

 * @return 0 if init succesfull, an error code otherwise

 *

 * @see motors_init

 **/

int motors_off( void ) {

  if(!motors_initd) {

    DRAGONFLY_DEBUG_PRINT("ERROR: encoders not init'd\r\n");

    return ERROR_LIBRARY_NOT_INITD;

  }

  OCR1AL = 0x0;

  OCR1BL = 0x0;

  return 0;

}

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