/****************************************************************************
*
*  Motor.c
*
*  Motor Control Code
*
****************************************************************************/

// ---- Include Files -------------------------------------------------------

#include <avr/io.h>

#include "Motor.h"

// ---- Public Variables ----------------------------------------------------
// ---- Private Constants and Types -----------------------------------------

// Port B

enum
{
    MotorA_Dir  = 0,    // Yellow Wire  - D1
    MotorA_PWM  = 1,    // White Wire   - D2

    MotorB_PWM  = 2,    // Blue Wire    - D1
    MotorB_Dir  = 5,    // Orange Wire  - D2
};

// ---- Private Variables ---------------------------------------------------
// ---- Private Function Prototypes -----------------------------------------
// ---- Functions -----------------------------------------------------------

extern void InitMotors( void )
{
    // 8 bit PWM yields divide by 255

#define CHAN_A_NON_INVERTING_PWM    ( 1 << COM1A1 ) | ( 0 << COM1A0 )
#define CHAN_B_NON_INVERTING_PWM    ( 1 << COM1B1 ) | ( 0 << COM1B0 )
#define PWM_8_BIT_MODE              ( 0 << WGM11 )  | ( 1 << WGM10 )

    TCCR1A = CHAN_A_NON_INVERTING_PWM | CHAN_B_NON_INVERTING_PWM | PWM_8_BIT_MODE;

#define T1_PRESCALAR_8  ( 0 << CS22 ) | ( 1 << CS21 ) | ( 0 << CS20 )

    TCCR1B = T1_PRESCALAR_8; // Divide by 8 prescalar

    OCR1A = 0;
    OCR1B = 0;

    // Set the 4 motor control pins as outputs

    {
        uint8_t pins = ( 1 << MotorA_Dir ) | ( 1 << MotorA_PWM )
                     | ( 1 << MotorB_Dir ) | ( 1 << MotorB_PWM );
    
        PORTB |= pins;
        DDRB  |= pins;
    }

    SetMotorSpeed( SPEED_OFF, SPEED_OFF );
}

/****************************************************************************
*
*  Sets the motor speed of both motors.
*/

extern void SetMotorSpeed( speed_t speedL, speed_t speedR )
{
    static  speed_t prevSpeedL = SPEED_OFF + 1;
    static  speed_t prevSpeedR = SPEED_OFF + 1;

    // We need to run the left motor in reverse, so we
    // do the adjustment here.
    
    speedL = -speedL;
    
    if ( speedL != prevSpeedL )
    {
        if ( speedL >= 0 )
        {
           PORTB &= ~( 1 << MotorA_Dir );
           TCCR1A &= ~( 1 << COM1A0 ); // Normal PWM
           OCR1A = speedL;
        }
        else
        {
            PORTB |= ( 1 << MotorA_Dir );
            TCCR1A |= ( 1 << COM1A0 ); // Inverted PWM
            OCR1A = -speedL;
        }
        prevSpeedL = speedL;
    }

    if ( speedR != prevSpeedR )
    {
        if ( speedR >= 0 )
        {
            PORTB &= ~( 1 << MotorB_Dir );
            TCCR1A &= ~( 1 << COM1B0 ); // Normal PWM
            OCR1B = speedR;
        }
        else
        {
            PORTB |= ( 1 << MotorB_Dir );
            TCCR1A |= ( 1 << COM1B0 ); // Inverted PWM
            OCR1B = -speedR;
        }
        prevSpeedR = speedR;
    }

} // SetMotorSpeed