/****************************************************************************

Module
MotorService.c

Description
This module contains the motor init function and motor helper functions.

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for the framework and this service
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_ssi.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h" // Define PART_TM4C123GH6PM in project
#include "driverlib/gpio.h"
#include "termio.h"
#include "hw_nvic.h"
#include "hw_pwm.h"
#include "hw_timer.h"
#include "hw_ssi.h"

#include "MyPWMLib.h"
#include "MotorEncoders.h"
#include "MasterSM.h"

/*----------------------------- Module Defines ----------------------------*/

// define motor channels and pins here
#define NUM_MOTORS 5
#define LEFT_DRIVE_CHANNEL 0 //PB6
#define RIGHT_DRIVE_CHANNEL 1 //PB7
#define LEFT_DRIVE_DIR_PIN BIT0HI //PB0
#define RIGHT_DRIVE_DIR_PIN BIT1HI //PB1
#define LAUNCH_SERVO_CHANNEL 3 //PB4
#define RELOAD_SERVO_CHANNEL 2 //PB5
#define FULCRUM_SERVO_CHANNEL 4

// define directions TODO: check this, motor should turn CW/CCW when looking at the
wheel, down the axle
#define CW 1
#define CCW 0

/*---------------------------- Module Functions ---------------------------*/

/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/

static uint16_t SERVO_PERIOD = 25000; //20ms in 0.8us
ticks
static uint16_t LAUNCH_SERVO_POSITIONS[3] = { 800, 2400, 3000 };
//{IDLE,MID,LAUNCH} // upper IDLE 800 MID 2400
static uint16_t RELOAD_SERVO_POSITIONS[2] = { 2400, 1400 };
//{HOLD,RELEASE}
static uint16_t FULCRUM_SERVO_POSITIONS[3] = { 1955, 1955, 1600 };
//{RELOAD,GOAL,GOAL_ROLL} //reload was 2030 MAX:2400 MIN:1500
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitMotors

Parameters
void

Returns
bool, false if error in initialization, true otherwise

Description

****************************************************************************/
bool InitMotors(void)
{
//init PWM lines
My_PWM_Init_Many(NUM_MOTORS);

//init drive motor direction pins

//enable the clock to port B
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
//wait for port B to be ready
//initialize direction pins for digital I/O
HWREG(GPIO_PORTB_BASE + GPIO_O_DEN) |= (LEFT_DRIVE_DIR_PIN |
RIGHT_DRIVE_DIR_PIN);
//initialize direction pins for digital output
HWREG(GPIO_PORTB_BASE + GPIO_O_DIR) |= (LEFT_DRIVE_DIR_PIN |
RIGHT_DRIVE_DIR_PIN);
//initialize directions low
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= (~LEFT_DRIVE_DIR_PIN &
~RIGHT_DRIVE_DIR_PIN);

//set servo period

My_PWM_SetPeriod( SERVO_PERIOD, LAUNCH_SERVO_CHANNEL >> 1);
My_PWM_SetPeriod( SERVO_PERIOD, RELOAD_SERVO_CHANNEL >> 1);
My_PWM_SetPeriod( SERVO_PERIOD, FULCRUM_SERVO_CHANNEL >> 1);

//set initial positions

My_PWM_SetPulseWidth( LAUNCH_SERVO_POSITIONS[0],
LAUNCH_SERVO_CHANNEL);
My_PWM_SetPulseWidth( RELOAD_SERVO_POSITIONS[1],
RELOAD_SERVO_CHANNEL);
My_PWM_SetPulseWidth( FULCRUM_SERVO_POSITIONS[QueryOffenseStrat()],
FULCRUM_SERVO_CHANNEL);

return true;
}

//***************DRIVE FUNCTIONS*******************//
//set duty cycle and direction of left drive motor
void SetLeftDrive(uint8_t Duty, bool Dir)
{
//set direction
if (Dir)
{
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= LEFT_DRIVE_DIR_PIN;
My_PWM_SetDuty(100 - Duty, LEFT_DRIVE_CHANNEL);
}
else
{
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= ~LEFT_DRIVE_DIR_PIN;
My_PWM_SetDuty(Duty, LEFT_DRIVE_CHANNEL);
}
}

//set duty cycle and direction of right drive motor

void SetRightDrive(uint8_t Duty, bool Dir)
{
//set direction
if (Dir)
{
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= RIGHT_DRIVE_DIR_PIN;
My_PWM_SetDuty(100 - Duty, RIGHT_DRIVE_CHANNEL);
}
else
{
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= ~RIGHT_DRIVE_DIR_PIN;
My_PWM_SetDuty(Duty, RIGHT_DRIVE_CHANNEL);
}
}

//stop drive motors

bool StopDrive(void)
{
SetDriving(false);
DisableEncoderControl();
SetLeftDrive(0, CW);
SetRightDrive(0, CW);
DisableLimit();
return true;
}

//go forward at a certain duty cycle

bool DriveForward(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(Duty, CCW);
SetRightDrive(Duty, CW);
return true;
}

//go backward at a certain duty cycle

bool DriveBackward(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(Duty, CW);
SetRightDrive(Duty, CCW);
return true;
}

//turn left at a certain duty cycle

bool TurnLeft(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(Duty, CW);
SetRightDrive(Duty, CW);
return true;
}

//turn right at a certain duty cycle

bool TurnRight(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(Duty, CCW);
SetRightDrive(Duty, CCW);
return true;
}
//***************SERVO FUNCTIONS*******************//
//move launch servo to preset positions (Pos: 0 = idle, 1 = mid, 2 = launch)
void MoveLaunchServo(uint8_t Pos)
{
My_PWM_SetPulseWidth(LAUNCH_SERVO_POSITIONS[Pos], LAUNCH_SERVO_CHANNEL);
}

//move reload servo to preset positions (Pos: 0 = hold, 1 = release)

void MoveReloadServo(uint8_t Pos)
{
My_PWM_SetPulseWidth(RELOAD_SERVO_POSITIONS[Pos], RELOAD_SERVO_CHANNEL);
}

//move fulcrum servo to preset positions (Pos: 0 = layup, 1 = reload, 2 = goal)

void MoveFulcrumServo(uint8_t Pos)
{
My_PWM_SetPulseWidth(FULCRUM_SERVO_POSITIONS[Pos], FULCRUM_SERVO_CHANNEL);
}

//left forward
bool DriveLeftForward(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(Duty, CCW);
SetRightDrive(0, CW);
return true;
}

//right forward
bool DriveRightForward(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(0, CCW);
SetRightDrive(Duty, CW);
return true;
}

//left backward
bool DriveLeftBackward(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(Duty, CW);
SetRightDrive(0, CCW);
return true;
}

//right backward
bool DriveRightBackward(uint8_t Duty)
{
DisableEncoderControl();
SetLeftDrive(0, CW);
SetRightDrive(Duty, CCW);
return true;
}

//left backward
bool SquareMoveRightForward(uint8_t DutyForward, uint8_t DutyBackward)
{
DisableEncoderControl();
SetLeftDrive(DutyBackward, CW);
SetRightDrive(DutyForward, CW);
return true;
}
bool SquareMoveLeftForward(uint8_t DutyForward, uint8_t DutyBackward)
{
DisableEncoderControl();
SetLeftDrive(DutyForward, CCW);
SetRightDrive(DutyBackward, CCW);
return true;
}

//drive straight using encoders (distance in inches)

bool DriveForwardStraight(uint8_t RPM, bool limit, uint8_t distance)
{
//reset the encoder counts and set a limit
if (limit)
{
SetDistanceLimit(distance);
}
else
{
DisableLimit();
}

//set the RPM in the encoder module

SetLeftRPM(RPM, CCW);
SetRightRPM(RPM, CW);

//last thing to do!

EnableEncoderControl();
return true;
}

//drive back straight using encoders (distance in inches)

bool DriveBackwardStraight(uint8_t RPM, bool limit, uint8_t distance)
{
//reset the encoder counts and set a limit
if (limit)
{
SetDistanceLimit(distance);
}
else
{
DisableLimit();
}

// encoder
SetLeftRPM(RPM, CW);
SetRightRPM(RPM, CCW);

//last thing to do!

EnableEncoderControl();
return true;
}

//turn CW using encoders (distance in inches)

bool DriveCWStraight(uint8_t RPM, bool limit, uint16_t angle)
{
//reset the encoder counts and set a limit
if (limit)
{
SetAngleLimit(angle);
}
else
{
DisableLimit();
 }

//set the RPM in the encoder module

SetLeftRPM(RPM, CCW);
SetRightRPM(RPM, CCW);

//last thing to do!

EnableEncoderControl();
return true;
}

//turn CCW using encoders (distance in inches)

bool DriveCCWStraight(uint8_t RPM, bool limit, uint16_t angle)
{
//reset the encoder counts and set a limit
if (limit)
{
SetAngleLimit(angle);
}
else
{
DisableLimit();
}

//set the RPM in the encoder module

SetLeftRPM(RPM, CW);
SetRightRPM(RPM, CW);

//last thing to do!

EnableEncoderControl();
return true;
}

//drive left back peel

bool LeftBackPeel(uint8_t RPM, bool limit, uint16_t distance)
{
if (limit)
{
SetDistanceLimit(distance);
}
else
{
DisableLimit();
}

SetLeftRPM(RPM, CW);
SetRightRPM(0, CW);

//last thing to do!

EnableEncoderControl();
return true;
}

/***************************************************************************
private functions
***************************************************************************/

/*------------------------------- Footnotes -------------------------------*/

/*------------------------------ End of file ------------------------------*/