//#define DCMOTOR_TEST

/****************************************************************************
Module
DCMotor.c
Revision
1.0.1
Description
This is a template file for implementing a simple service under the
Gen2 Events and Services Framework.
Notes
History
When Who
--------------------
****************************************************************************/

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

/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include <stdio.h>
#include <math.h>
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_Port.h"
#include "ES_DeferRecall.h"
#include "ES_Timers.h"
#include "termio.h"
#include "BITDEFS.h" // standard bit definitions to make things more readable

// the headers to access the GPIO subsystem

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_nvic.h"

// the headers to access the TivaWare Library

#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"

// headers from service files

#include "DCMotor.h"
#include "GameMasterSM.h"

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

//DCMotor Channels:
//PB6 : Left Motor
//PB7 : Right Motor
#define Forward 0
#define Backward 1
#define CWTurn 2
#define CCWTurn 3
#define LeftMotor 0
#define RightMotor 1

//for convert RPM

#define MAX_RPM 23
#define TicksPerSec 40000000
#define SecInMin 60
#define PulsePerRev 12
#define GearRatio 50

//#define ALL_BITS (0xff<<2)

// 40,000 ticks per mS assumes a 40Mhz clock, we will use SysClk/32 for PWM
#define PWMTicksPerMS 40000/32
// set 1000 Hz frequency so 1mS period
#define PeriodInMS 1

// program generator A to go to 1 at rising comare A, 0 on falling compare A

#define GenA_Normal (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO )
// program generator B to go to 1 at rising comare B, 0 on falling compare B
#define GenB_Normal (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO )
// for convenience to write data to registers
#define BitsPerNibble 4

// timer length constants

#define TicksPerMS 40000
#define TEN_MSEC 10
#define ONE_SEC 1000
#define DC_TIME 10*TEN_MSEC // Pot reading frequency: 10Hz
#define FW_TIME 3*ONE_SEC
#define BW_TIME 1.5*ONE_SEC

//distance unit is cm in this module

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
static void InitPWM(void);
static void InitIN1Pins(void);
static void InitLeftEncoderCapture(void);
static void InitRightEncoderCapture(void);
static void InitPeriod2MSTimer(void);
static void InitLeftOneShotTimer(void);
static void InitRightOneShotTimer(void);
//static void InitPositionCheckTimer(void);
static void SetDutyCycle(uint8_t which, uint32_t PWM);

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

static uint32_t LPeriod;

static uint32_t LBuffer[5];
static uint32_t LLastCapture;
static uint32_t LeftCount = 0;

static uint32_t RPeriod;

static uint32_t RBuffer[5];
static uint32_t RLastCapture;
static uint32_t RightCount = 0;

static uint16_t LeftTargetRPM;

static uint16_t RightTargetRPM;
static uint32_t LeftCurrentRPM;
static uint32_t RightCurrentRPM;

static float LeftRPMError = 0.0;

static float RightRPMError = 0.0;
static float LeftIntegralError = 0.0;
static float RightIntegralError = 0.0;
static float Kp = 1.758; //need to tune
static float Ki = 0.005; //need to tune
static int LeftRequestedDuty;
static int RightRequestedDuty;
static bool JustChangedDirection = false;

//Position control

static float TurnRadius = 142.875; // radius between robot wheels in mm

static uint16_t TargetX = 0;

static uint16_t TargetY = 0;
static uint16_t TargetTheta = 45;
static double RobotX = 146, RobotY = 146; //init positon is at the corner

static double theta = (double)45/(double)180*3.1415; //init angle is 135 deg, we need

to convert to rad to do calculations
static double PulseDistance =
(double)70*3.1415*4.15/(double)GearRatio/(double)PulsePerRev; // distance
traveled in one encoder pulse, 4.15 is a manual scaling factor
static uint8_t RobotDirection = Forward;
static uint32_t PulseCounts = 0;

static bool AtXGoal = false;

static bool AtYGoal = false;
static bool AtHeadingGoal = false;

//static bool isLeftStop = false;

//static bool isRightStop = false;

/*------------------------------ Module Code ------------------------------*/

void InitMotors(void){
//init hardware for PWM channels, IN1 pins, motor encoders, oneshot timers and
periodic timer
InitPWM();
InitIN1Pins();
InitLeftEncoderCapture();
InitRightEncoderCapture();
InitPeriod2MSTimer();
InitLeftOneShotTimer();
InitRightOneShotTimer();

#ifdef DCMOTOR_TEST
RobotX = 0.0; RobotY = 0.0;
#endif

SetRobotDir(Forward);
}

void StopRobot(void){
//set duty cycle to both motors 0
HWREG( PWM0_BASE+PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ZERO;
HWREG( PWM0_BASE+PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ZERO;
// kill the closed loop speed updating timer
HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;

void SetRobotSpeed (uint8_t which, uint16_t RPM){

//enable speed update timer

HWREG(WTIMER3_BASE+TIMER_O_CTL) |= TIMER_CTL_TAEN;
 //if we are setting LeftMotor speed, set LeftTargetRPM to the passed in RPM
if(which == LeftMotor) LeftTargetRPM = RPM;
else RightTargetRPM = RPM; //else set RightTargetRPM to the passed in RPM

void SetRobotDir(uint8_t Dir)

{
//if passed in Dir is not the same as current direction
if (Dir != RobotDirection)
{
JustChangedDirection = true; //set JustChangedDirection to true
}
//if Dir is Backward
if (Dir == Backward)
{
//Set Left Motor to counterclockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT0LO;
HWREG(PWM0_BASE + PWM_O_INVERT) &= ~PWM_INVERT_PWM0INV;
//Set Right Motor to clockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT1HI;
HWREG(PWM0_BASE + PWM_O_INVERT) |= PWM_INVERT_PWM1INV;
}
//if Dir is Forward
else if (Dir == Forward)
{
//Set Left Motor to clockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT0HI;
HWREG(PWM0_BASE + PWM_O_INVERT) |= PWM_INVERT_PWM0INV;
//Set Right Motor to counterclockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT1LO;
HWREG(PWM0_BASE + PWM_O_INVERT) &= ~PWM_INVERT_PWM1INV;
}
//if Dir is CCWTurn
else if (Dir == CCWTurn)
{
//Set Left Motor to counterclockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT0LO;
HWREG(PWM0_BASE + PWM_O_INVERT) &= ~PWM_INVERT_PWM0INV;
//Set Right Motor to counterclockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT1LO;
HWREG(PWM0_BASE + PWM_O_INVERT) &= ~PWM_INVERT_PWM1INV;
}
//if Dir is CWTurn
else if (Dir == CWTurn)
{
//Set Left Motor to clockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT0HI;
HWREG(PWM0_BASE + PWM_O_INVERT) |= PWM_INVERT_PWM0INV;
//Set Right Motor to clockwise
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT1HI;
HWREG(PWM0_BASE + PWM_O_INVERT) |= PWM_INVERT_PWM1INV;
}
//set RobotDirection to Dir
RobotDirection = Dir;
}

void SetTargetPosition(uint16_t GoalX, uint16_t GoalY)

{
//set module level variables TargetX to passed-in GoalX and TargetY to passed-in
GoalY
TargetX = GoalX;
TargetY = GoalY;
 //set module level AtXGoal and AtYGoal to false
AtXGoal = false;
AtYGoal = false;
}

void SetTargetHeading(int GoalTheta)

{
//set module level TargetTheta to passed-in GoalTheta
TargetTheta = GoalTheta;
//set AtHeadingGoal to false
AtHeadingGoal = false;
}

void LeftCaptureResponse(void){
uint32_t ThisCapture;
uint32_t ThisPeriod;
uint8_t Divisor;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_CAECINT;

// now grab the captured value and calculate the period

ThisCapture = HWREG(WTIMER2_BASE+TIMER_O_TAR);
ThisPeriod = ThisCapture - LLastCapture;
if (ThisPeriod > 80000 && ThisPeriod < 8000000)
{
LPeriod = ThisPeriod;
}
<<<<<<< HEAD
// update LLastCapture to prepare for the next edge
=======

// update LastCapture to prepare for the next edge

>>>>>>> f29af7f9960f9fe8b5c3510637aac2512db19a11
LLastCapture = ThisCapture;

// ES_Event_t EventToPost;
// EventToPost.EventType = DEBUG_ENCODER;
// EventToPost.EventParam = ThisCapture;
// PostGameMasterSM(EventToPost);

HWREG(WTIMER4_BASE+TIMER_O_TAV) = HWREG(WTIMER4_BASE+TIMER_O_TAILR);
// now kick the one-shot timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
// isLeftStop = false;
}

void RightCaptureResponse(void){
uint32_t ThisCapture;
uint32_t ThisPeriod;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER2_BASE+TIMER_O_ICR) = TIMER_ICR_CBECINT;

// now grab the captured value and calculate the period

ThisCapture = HWREG(WTIMER2_BASE+TIMER_O_TBR);
ThisPeriod = ThisCapture - RLastCapture;
if (ThisPeriod > 80000 && ThisPeriod < 8000000)
{
RPeriod = ThisPeriod;
}
// update LastCapture to prepare for the next edge
RLastCapture = ThisCapture;
//restart one shot timer so that it does't times out
HWREG(WTIMER4_BASE+TIMER_O_TBV) = HWREG(WTIMER4_BASE+TIMER_O_TBILR);
// now kick the one-shot timer off by enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
// isRightStop = false;

//calculate current position based on encoder counts

PulseCounts++;

if (RobotDirection == Backward)
{
RobotX -= PulseDistance * cos(theta);
RobotY -= PulseDistance * sin(theta);
}
else if (RobotDirection == Forward)
{
RobotX += PulseDistance * cos(theta);
RobotY += PulseDistance * sin(theta);
}
else if (RobotDirection == CWTurn)
{
theta -= PulseDistance * 0.9/ TurnRadius;
}
else if (RobotDirection == CCWTurn)
{
theta += PulseDistance * 0.9 / TurnRadius;
}

int XError;
int YError;
float HeadingError;

//Check to see if target position or Heading has been reached

if (RobotDirection == Backward || RobotDirection == Forward)
{
XError = (int)RobotX - TargetX;
YError = (int)RobotY - TargetY;
if ((abs(XError) < 2) && (AtXGoal == false))
{
AtXGoal = true;
ES_Event_t ReturnEvent;
ReturnEvent.EventType = EV_X_TARGET_REACHED;
PostGameMasterSM(ReturnEvent);
}
if ((abs(YError) < 2) && (AtYGoal == false))
{
AtYGoal = true;
ES_Event_t ReturnEvent;
ReturnEvent.EventType = EV_Y_TARGET_REACHED;
PostGameMasterSM(ReturnEvent);
}
}
else
{
HeadingError = (theta*180/3.1415) - (float)TargetTheta;
if (fabs(HeadingError) < 0.8 && (AtHeadingGoal == false))
{
AtHeadingGoal = true;
ES_Event_t ReturnEvent;
ReturnEvent.EventType = EV_ANGLE_TARGET_REACHED;
PostGameMasterSM(ReturnEvent);
}
}
}

void SpeedUpdateISR(void){
// static uint16_t LeftRequestedDuty;
// static uint16_t RightRequestedDuty;
// start by clearing the source of the interrupt
HWREG(WTIMER3_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;

LeftCurrentRPM = SecInMin*(TicksPerSec)/(PulsePerRev*LPeriod) / GearRatio; //

Calculate the current RPM based on Period
RightCurrentRPM = SecInMin*(TicksPerSec)/(PulsePerRev*RPeriod)/ GearRatio;

LeftRPMError = ((float)LeftTargetRPM - (float)LeftCurrentRPM);

LeftIntegralError += LeftRPMError;

RightRPMError = ((float)RightTargetRPM - (float)RightCurrentRPM);

RightIntegralError += RightRPMError;
float LeftIntegralTerm = Ki*LeftIntegralError;
float RightIntegralTerm = Ki*RightIntegralError;

// add anti-windup for the integrators for left wheel

if (LeftIntegralTerm > 100.0f)
{
LeftIntegralTerm = 100.0f;
} else if (LeftIntegralTerm < -100.0f)
{
LeftIntegralTerm = -100.0f;
}

// add anti-windup for the integrators for right wheel

if (RightIntegralTerm > 100.0f)
{
RightIntegralTerm = 100.0f;
} else if (RightIntegralTerm < -100.0f)
{
RightIntegralTerm = -100.0f;
}

LeftRequestedDuty = (int)(Kp * (LeftRPMError + LeftIntegralTerm));

RightRequestedDuty = (int)(Kp * (RightRPMError + RightIntegralTerm));

// add duty cycle clamp for left wheel

if (LeftRequestedDuty > 100) {
LeftRequestedDuty = 100;
//LeftIntegralError -= LeftRPMError;
} else if (LeftRequestedDuty < 0) {
LeftRequestedDuty = 0;
//LeftIntegralError -= LeftRPMError;
}

// add duty cycle clamp for right wheel

if (RightRequestedDuty > 100) {
RightRequestedDuty = 100;
//RightIntegralError -= RightRPMError;
} else if (RightRequestedDuty < 0) {
RightRequestedDuty = 0;
//RightIntegralError -= RightRPMError;
}
SetDutyCycle(LeftMotor, LeftRequestedDuty);
SetDutyCycle(RightMotor, RightRequestedDuty);
}

//void PositionCheckISR(void)
//{
// // start by clearing the source of the interrupt
// HWREG(WTIMER3_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT;
// int XError;
// int YError;
// int HeadingError;
//
// //Check to see if target position or Heading has been reached
// if (RobotDirection == Backward || RobotDirection == Forward)
// {
// XError = (int)RobotX - TargetX;
// YError = (int)RobotY - TargetY;
// if ((abs(XError) < 10) && (abs(YError) < 10))
// {
// ES_Event_t ReturnEvent;
// ReturnEvent.EventType = EV_TARGET_REACHED;
// PostGameMasterSM(ReturnEvent);
// HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;

// }
// }
// else
// {
// HeadingError = (int)(theta*180/pi) - TargetTheta;
// if (abs(HeadingError) < 3)
// {
// ES_Event_t ReturnEvent;
// ReturnEvent.EventType = EV_TARGET_REACHED;
// PostGameMasterSM(ReturnEvent);
// HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;

// }
// }
//}

void LeftOneShotTimerISR(void){
// start by clearing the source of the interrupt
HWREG(WTIMER4_BASE+TIMER_O_ICR) = TIMER_ICR_TATOCINT;

//if we just changed left motor direction

if (JustChangedDirection == true)
{
//set LeftCurrentRPM to be 0
LeftCurrentRPM = 0;
//set JustChangedDirection to be false
JustChangedDirection = false;
}
else
{ //else post EV_MOTOR_STOP to GameMasterSM
ES_Event_t Event;
Event.EventType = EV_MOTOR_STOP;
PostGameMasterSM(Event);
}
}

void RightOneShotTimerISR(void){
// start by clearing the source of the interrupt
HWREG(WTIMER4_BASE+TIMER_O_ICR) = TIMER_ICR_TBTOCINT;
//if we just changed right motor direction
if (JustChangedDirection == true)
{
//set RightCurrentRPM to be 0
RightCurrentRPM = 0;
 //set JustChangedDirection to be false
JustChangedDirection = false;
}else{ //else post EV_MOTOR_STOP to GameMasterSM
ES_Event_t Event;
Event.EventType = EV_MOTOR_STOP;
PostGameMasterSM(Event);
}
}

void printRPM(void){
//the origianl calculation is float LRPM =
(SecInMin*TicksPerSec)/(PulsePerRev*LPeriod)/GearRatio;
//to convert to float arithmetic and prevent overflow, I calculate
SecInMin/GearRatio = 1.2 first
printf("LPeriod = %d, RPeriod = %d \n\r", LPeriod, RPeriod);
printf("LeftRequestedDuty = %d, LeftTargetRPM = %d, LeftCurrentRPM = %d \n\r",
LeftRequestedDuty, LeftTargetRPM, LeftCurrentRPM);
printf("RightRequestedDuty = %d, RightTargetRPM = %d, RightCurrentRPM = %d \n\r",
RightRequestedDuty, RightTargetRPM, RightCurrentRPM);
printf("CurrentXPosition = %.2f, CurrentYPosition = %.2f, CurrentAngle = %.2f
\n\r", RobotX, RobotY, theta);
printf("EncoderPulses = %d \n\r", PulseCounts);
printf("targettheta = %d, theta = %.2f\n\r\n\r", TargetTheta,
(int)theta*180/3.1415);
}

/******************************************private
functions********************************************/
static void SetDutyCycle(uint8_t which, uint32_t PWM){
if(which == LeftMotor){ //if we are setting LeftMotor PWM
if (PWM <= 0){ //if required PWM is equal or less than 0
//set PWM to 0
HWREG( PWM0_BASE+PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ZERO;
} else if (PWM >= 100) { //else if required PWM is equal or greater than 100
//set PWM to 100
HWREG( PWM0_BASE+PWM_O_0_GENA) = PWM_0_GENA_ACTZERO_ONE;
} else { //else
//set PWM to proper value
HWREG( PWM0_BASE+PWM_O_0_GENA) = GenA_Normal;
HWREG( PWM0_BASE+PWM_O_0_CMPA) = (HWREG(PWM0_BASE+PWM_O_0_LOAD)) * (100 -
PWM)/100;
}
}

if(which == RightMotor){ //if we are setting LeftMotor PWM

if (PWM <= 0){ //if required PWM is equal or less than 0
//set PWM to 0
HWREG( PWM0_BASE+PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ZERO;
} else if (PWM >= 100) { //else if required PWM is equal or greater than 100
//set PWM to 100
HWREG( PWM0_BASE+PWM_O_0_GENB) = PWM_0_GENB_ACTZERO_ONE;
} else { //else
//set PWM to proper value
HWREG( PWM0_BASE+PWM_O_0_GENB) = GenB_Normal;
HWREG( PWM0_BASE+PWM_O_0_CMPB) = (HWREG(PWM0_BASE+PWM_O_0_LOAD)) * (100 -
PWM)/100;
}
}
}

static void InitPWM(void){

// PB6(M0PWM0) & PB7(M0PWM1) are used to generate PWM (up-down counting)
 // start by enabling the clock to the PWM Module (PWM0)
HWREG(SYSCTL_RCGCPWM) |= SYSCTL_RCGCPWM_R0;

// enable the clock to Port B

HWREG(SYSCTL_RCGCGPIO) |= BIT1HI;

// Select the PWM clock as System Clock/32

HWREG(SYSCTL_RCC) = (HWREG(SYSCTL_RCC) & ~SYSCTL_RCC_PWMDIV_M) |
(SYSCTL_RCC_USEPWMDIV | SYSCTL_RCC_PWMDIV_32);

// make sure that the PWM module clock has gotten going
while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0)
;

// disable the PWM while initializing

HWREG( PWM0_BASE+PWM_O_0_CTL ) = 0;

// program generators to go to 1 at rising compare A/B, 0 on falling compare A/B

// GenA_Normal = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO )
HWREG( PWM0_BASE+PWM_O_0_GENA) = GenA_Normal;
// GenB_Normal = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO )
HWREG( PWM0_BASE+PWM_O_0_GENB) = GenB_Normal;

// Set the PWM period. Since we are counting both up & down, we initialize
// the load register to 1/2 the desired total period. We will also program
// the match compare registers to 1/2 the desired high time
HWREG( PWM0_BASE+PWM_O_0_LOAD) = ((PeriodInMS * PWMTicksPerMS))>>1; // Period

// Set the initial Duty cycle on A to 50% by programming the compare value
// to 1/2 the period to count up (or down). Technically, the value to program
// should be Period/2 - DesiredHighTime/2, but since the desired high time is
1/2
// the period, we can skip the subtract
HWREG( PWM0_BASE+PWM_O_0_CMPA) = HWREG( PWM0_BASE+PWM_O_0_LOAD)>>1; // Left
Motor Duty cycle 50%

// Set the initial Duty cycle on B to 0%

HWREG( PWM0_BASE+PWM_O_0_CMPB) = HWREG( PWM0_BASE+PWM_O_0_LOAD)>>1; //Right
Motor Duty cycle 50%

// enable the PWM outputs

HWREG( PWM0_BASE+PWM_O_ENABLE) |= (PWM_ENABLE_PWM0EN | PWM_ENABLE_PWM1EN);

// now configure the Port B pins to be PWM outputs

// start by selecting the alternate function for PB6 & PB7
HWREG(GPIO_PORTB_BASE+GPIO_O_AFSEL) |= (BIT6HI | BIT7HI);

// now choose to map PWM to those pins, this is a mux value of 4 that we
// want to use for specifying the function on bits 6 and 7
HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) =
(HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) & 0x00ffffff) +
(4<<(7*BitsPerNibble)) +
(4<<(6*BitsPerNibble));

// Enable pins 7 & 6 on Port B for digital I/O

HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (BIT7HI | BIT6HI);

// make pins 7 & 6 on Port B into outputs

HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (BIT7HI |BIT6HI);

// set the up/down count mode, enable the PWM generator and make
// both generator updates locally synchronized to zero count
HWREG(PWM0_BASE+ PWM_O_0_CTL) = (PWM_0_CTL_MODE | PWM_0_CTL_ENABLE |
PWM_0_CTL_GENAUPD_LS |
PWM_0_CTL_GENBUPD_LS);
}

static void InitIN1Pins(void){

//IN1 for left motor is PB0, IN1 for right motor is PB1
// enable the clock to Port B
HWREG(SYSCTL_RCGCGPIO) |= BIT1HI;
while((HWREG(SYSCTL_PRGPIO)& SYSCTL_PRGPIO_R1) != SYSCTL_PRGPIO_R1);
//Set PB0 & PB1 to be digital pins
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (BIT0HI | BIT1HI);
//Set PB0 & PB1 to be output pins
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (BIT0HI | BIT1HI);
//Pull PB0 and PB1 LO at initialization
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= ~(BIT0HI |BIT1HI);
}

/******************************************************Encoders*****************
*********************************************/
//Left encoder is PD0 (WT2CCP0)
static void InitLeftEncoderCapture(void){
// start by enabling the clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2;
// enable the clock to Port D
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R3;
// since we added this Port D clock init, we can immediately start
// into configuring the timer, no need for further delay
// make sure that timer (Timer A) is disabled before configuring
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER2_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// we want to use the full 32 bit count, so initialize the Interval Load
// register to 0xffffffff (its default value)
HWREG(WTIMER2_BASE+TIMER_O_TAILR) = 0xffffffff;
// set up timer A in capture mode (TAMR=3, TAAMS = 0),
// for edge time (TACMR = 1) and up-counting (TACDIR = 1)
HWREG(WTIMER2_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER2_BASE+TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) |
(TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
// To set the event to rising edge, we need to modify the TAEVENT bits
// in GPTMCTL. Rising edges = 00, so we set the TAEVENT bits
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
// Now Set up the port to do the capture (clock was enabled earlier)
// start by setting the alternate function for Port D bit 0 (WT2CCP0)
HWREG(GPIO_PORTD_BASE+GPIO_O_AFSEL) |= BIT0HI;
// Then, map bit 4's alternate function to WT0CCP0
// 7 is the mux value to select WT0CCP0, 0 to shift it over to the
// right nibble for bit 0 (4 bits/nibble * 0 bits)
HWREG(GPIO_PORTD_BASE+GPIO_O_PCTL) = (HWREG(GPIO_PORTD_BASE+GPIO_O_PCTL) &
0xfffffff0) + (7<<0);
// Enable pin on Port D for digital I/O
HWREG(GPIO_PORTD_BASE+GPIO_O_DEN) |= BIT0HI;
// make pin 0 on Port D into an input
HWREG(GPIO_PORTD_BASE+GPIO_O_DIR) &= BIT0LO;
// back to the timer to enable a local capture interrupt
HWREG(WTIMER2_BASE+TIMER_O_IMR) |= TIMER_IMR_CAEIM;
// enable the Timer A in Wide Timer 2 interrupt in the NVIC
// it is interrupt number 98 so appears in EN2 at bit 30
HWREG(NVIC_EN3) |= BIT2HI;
// make sure interrupts are enabled globally
__enable_irq();
// Start the input capture timer later when needed
 // now kick the InputCaptureTimer off by enabling it and
//enabling the timer to stall while stopped by the debugger
HWREG(WTIMER2_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}

//right encoder is PD1 (WT2CCP1)

static void InitRightEncoderCapture(void){
// start by enabling the clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2;
// enable the clock to Port D
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R3;
// since we added this Port D clock init, we can immediately start
// into configuring the timer, no need for further delay
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER2_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
// we want to use the full 32 bit count, so initialize the Interval Load
// register to 0xffffffff (its default value)
HWREG(WTIMER2_BASE+TIMER_O_TBILR) = 0xffffffff;
// set up timer B in capture mode (TBMR=3, TBAMS = 0),
// for edge time (TBCMR = 1) and up-counting (TBCDIR = 1)
HWREG(WTIMER2_BASE+TIMER_O_TBMR) =
(HWREG(WTIMER2_BASE+TIMER_O_TBMR) & ~TIMER_TBMR_TBAMS) |
(TIMER_TBMR_TBCDIR | TIMER_TBMR_TBCMR | TIMER_TBMR_TBMR_CAP);
// To set the event to rising edge, we need to modify the TAEVENT bits
// in GPTMCTL. Rising edges = 00, so we set the TAEVENT bits
HWREG(WTIMER2_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEVENT_M;
// Now Set up the port to do the capture (clock was enabled earlier)
// start by setting the alternate function for Port D bit 1 (WT2CCP1)
HWREG(GPIO_PORTD_BASE+GPIO_O_AFSEL) |= BIT1HI;
// Then, map bit 4's alternate function to WT0CCP0
// 7 is the mux value to select WT0CCP0, 4 to shift it over to the
// right nibble for bit 1 (4 bits/nibble * 1 bits)
HWREG(GPIO_PORTD_BASE+GPIO_O_PCTL) = (HWREG(GPIO_PORTD_BASE+GPIO_O_PCTL) &
0xffffff0f) + (7<<4);
// Enable pin on Port D for digital I/O
HWREG(GPIO_PORTD_BASE+GPIO_O_DEN) |= BIT1HI;
// make pin 0 on Port D into an input
HWREG(GPIO_PORTD_BASE+GPIO_O_DIR) &= BIT1LO;
// back to the timer to enable a local capture interrupt
HWREG(WTIMER2_BASE+TIMER_O_IMR) |= TIMER_IMR_CBEIM;
// enable the Timer A in Wide Timer 2 interrupt in the NVIC
// it is interrupt number 99 so appears in EN2 at bit 30
HWREG(NVIC_EN3) |= BIT3HI;
// make sure interrupts are enabled globally
__enable_irq();
// Start the input capture timer later when needed
// now kick the InputCaptureTimer off by enabling it and
//enabling the timer to stall while stopped by the debugger
HWREG(WTIMER2_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}

//period timer is WT3A, period is 2ms, for motor speed control

static void InitPeriod2MSTimer(void) {
// start by enabling the clock to the timer (Wide Timer 3)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R3;
// kill a few cycles to let the clock get going
while((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R3) != SYSCTL_PRWTIMER_R3);

// make sure that timer (Timer A) is disabled before configuring

 HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;

// set it up in 32bit wide (individual, not concatenated) mode

// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER3_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT; //bits 0-2 = 0x04

// set up timer A in periodic mode so that it repeats the time-outs

HWREG(WTIMER3_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER3_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|
TIMER_TAMR_TAMR_PERIOD;

// set timeout to be 50mS

HWREG(WTIMER3_BASE+TIMER_O_TAILR) = TicksPerMS * 2;

// enable a local timeout interrupt

HWREG(WTIMER3_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;

// enable the Timer A in Wide Timer 3 interrupt in the NVIC

// it is interrupt number 100 so appears in EN3 at bit 4
HWREG(NVIC_EN3) |= BIT4HI;

// lower priority
//HWREG(NVIC_PRI24) = (HWREG(NVIC_PRI24) & ~NVIC_PRI24_INTC_M) + (1<<21);
// make sure interrupts are enabled globally
__enable_irq();

// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger.
HWREG(WTIMER3_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);

//one shot timer for detecting if left motor stop WT4A

static void InitLeftOneShotTimer(void){
// start by enabling the clock to the timer (Wide Timer 4)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R4;
// kill a few cycles to let the clock get going
while((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R4) != SYSCTL_PRWTIMER_R4);
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER4_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER4_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT; //bits 0-2 = 0x04
// set up timer A in 1-shot mode so that it disables timer on timeouts
// first mask off the TBMR field (bits 0:1) then set the value for
// 1-shot mode = 0x01
HWREG(WTIMER4_BASE+TIMER_O_TAMR) =
(HWREG(WTIMER4_BASE+TIMER_O_TAMR)& ~TIMER_TAMR_TAMR_M)|
TIMER_TAMR_TAMR_1_SHOT;
// set timeout
HWREG(WTIMER4_BASE+TIMER_O_TAILR) = TicksPerMS * 100;
// enable a local timeout interrupt. TATOIM = bit 0
HWREG(WTIMER4_BASE+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// enable the Timer B in Wide Timer 0 interrupt in the NVIC
// it is interrupt number 102 so appears in EN3 at bit 6
HWREG(NVIC_EN3) |= BIT6HI;
// lower priority
// HWREG(NVIC_PRI23) = (HWREG(NVIC_PRI23) & ~NVIC_PRI23_INTD_M) + (2<<29);
// make sure interrupts are enabled globally
__enable_irq();

// now kick the timer off by enabling it and enabling the timer to
 // stall while stopped by the debugger.
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}

//one shot timer for detecting if right motor stop WT4B

static void InitRightOneShotTimer(void){
// start by enabling the clock to the timer (Wide Timer 4)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R4;
// kill a few cycles to let the clock get going
while((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R4) != SYSCTL_PRWTIMER_R4);
// make sure that timer (Timer B) is disabled before configuring
HWREG(WTIMER4_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
// set it up in 32bit wide (individual, not concatenated) mode
// the constant name derives from the 16/32 bit timer, but this is a 32/64
// bit timer so we are setting the 32bit mode
HWREG(WTIMER4_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT; //bits 0-2 = 0x04
// set up timer A in 1-shot mode so that it disables timer on timeouts
// first mask off the TBMR field (bits 0:1) then set the value for
// 1-shot mode = 0x01
HWREG(WTIMER4_BASE+TIMER_O_TBMR) =
(HWREG(WTIMER4_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|
TIMER_TBMR_TBMR_1_SHOT;
// set timeout
HWREG(WTIMER4_BASE+TIMER_O_TBILR) = TicksPerMS * 100;
// enable a local timeout interrupt. TATOIM = bit 0
HWREG(WTIMER4_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
// enable the Timer B in Wide Timer 0 interrupt in the NVIC
// it is interrupt number 103 so appears in EN3 at bit 6
HWREG(NVIC_EN3) |= BIT7HI;
// lower priority
// HWREG(NVIC_PRI23) = (HWREG(NVIC_PRI23) & ~NVIC_PRI23_INTD_M) + (2<<29);
// make sure interrupts are enabled globally
__enable_irq();

// now kick the timer off by enabling it and enabling the timer to
// stall while stopped by the debugger.
HWREG(WTIMER4_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}

////ppsition timer is WT3B, 250ms

//static void InitPositionCheckTimer(void){
// // start by enabling the clock to the timer (Wide Timer 3)
// HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R3;
// // kill a few cycles to let the clock get going
// while((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R3) != SYSCTL_PRWTIMER_R3);
//
// // make sure that timer (Timer B) is disabled before configuring
// HWREG(WTIMER3_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
//
// // set it up in 32bit wide (individual, not concatenated) mode
// // the constant name derives from the 16/32 bit timer, but this is a 32/64
// // bit timer so we are setting the 32bit mode
// HWREG(WTIMER3_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT; //bits 0-2 = 0x04
//
// // set up timer B in periodic mode so that it repeats the time-outs
// HWREG(WTIMER3_BASE+TIMER_O_TBMR) =
// (HWREG(WTIMER3_BASE+TIMER_O_TBMR)& ~TIMER_TBMR_TBMR_M)|
// TIMER_TBMR_TBMR_PERIOD;
//
// // set timeout to be 250ms
// HWREG(WTIMER3_BASE+TIMER_O_TBILR) = TicksPerMS * 250;
//
// // enable a local timeout interrupt
// HWREG(WTIMER3_BASE+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
//
// // enable the Timer A in Wide Timer 3 interrupt in the NVIC
// // it is interrupt number 101 so appears in EN3 at bit 4
// HWREG(NVIC_EN3) |= BIT5HI;
//
// // lower priority
// //HWREG(NVIC_PRI24) = (HWREG(NVIC_PRI24) & ~NVIC_PRI24_INTC_M) + (1<<21);
// // make sure interrupts are enabled globally
// __enable_irq();
//
// // now kick the timer off by enabling it and enabling the timer to
// // stall while stopped by the debugger.
// HWREG(WTIMER3_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
//}
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/