/****************************************************************************
 Module
   Drive_Motor_Speed_Control_Module.c

 Revision
   1.0.0

 Description
   This module will initialize the drive motor hardware and 
	 contain the functions to control those motors

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/16/12 09:58 jec     began conversion from TemplateFSM.c
 01/25/17	08:10	mch			create Motor Module
****************************************************************************/
/*----------------------------- 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
*/

// the common headers for C99 types 
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
#include <math.h>

// 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"
#include "driverlib/pwm.h"
#include "termio.h"

#include "BITDEFS.H"

#include "ES_Port.h"

//My Includes
#include "PWM_Module.h"
#include "Hardware.h"
#include "Constants.h"
#include "ADMulti.h"
#include "Drive_Motor_Speed_Control_Module.h"

/*----------------------------- Module Defines ----------------------------*/
// Timer defines
#define CLOCK_FULL_LOAD 0xFFFFFFFF
#define NOT_MOVING_PER 100
#define NOT_MOVING_TIMEOUT (40000*NOT_MOVING_PER)
#define SEC_PER_MIN 60

// Encoder defines
#define PULSE_PER_REV 5.0f
#define GEAR_RATIO 50.0f
#define LEN_SPEED_ARRAY 3
#define LEN_SPEED_ARRAY_F 3.0f

// Speed control
#define SPEED_CONTROL_UPDATE_PERIOD 10
#define SPEED_GAIN_P 0.3f
#define SPEED_GAIN_I 1.1f
#define SPEED_MAX_EFFORT 100.0f
#define SPEED_MIN_EFFORT 0.0f
#define MOTOR_STOP			0.0f

// Wire follow control
#define WIRE_CONTROL_UPDATE_PERIOD 10
#define WIRE_GAIN_P 125.5f
#define WIRE_GAIN_D 45.5f
#define SPEED_MAX_DIFF 25.0f
#define SPEED_MIN_DIFF -25.0f

// Alternate pin function defines
#define AFSEL_MUX_PD2_M 0x00000F00
#define AFSEL_WTIM3_PD2 (7 << (2*BITS_IN_NIBBLE))

#define AFSEL_MUX_PD3_M 0x0000F000
#define AFSEL_WTIM3_PD3 (7 << (3*BITS_IN_NIBBLE))

// PWM Defines
#define MOTOR_PWM_FREQ 8000

// ADC Defines
#define LEFT_WIRE_SENSOR 	0
#define RIGHT_WIRE_SENSOR 1

// Test harness
//#define TEST

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/
static void InitEncoderCapture( void );
static void InitEncoderCapturePin( void );
static void InitControllerTimers( void );
static void InitMotorStoppedTimers( void );
static void InitMotorSpeedControl( void );
void SetDesiredSpeed( float newSpeed );

static float Period2Speed( float period );
void SetIndividualMotorSpeed( float LeftSpeed, float RightSpeed );

/*---------------------------- Module Variables ---------------------------*/
// Motor state variables
static volatile uint32_t 	LastCaptureLeft;
static volatile uint32_t 	EncodePeriodLeft = 0;
static volatile int16_t		LeftEncoderTicks = 0;
static volatile float 		EncoderSpeedLeft = 0;
static volatile float 		SpeedLeftArray[LEN_SPEED_ARRAY]={0,0,0};

static volatile uint32_t 	LastCaptureRight;
static volatile uint32_t 	EncodePeriodRight = 0;
static volatile int16_t		RightEncoderTicks = 0;
static volatile float 		EncoderSpeedRight = 0;
static volatile float 		SpeedRightArray[LEN_SPEED_ARRAY]={0,0,0};

static volatile float 		DesiredSpeed;
static volatile float 		DesiredSpeedLeft;
static volatile float 		DesiredSpeedRight;

static volatile bool			LeftMotorStopFlag = true;
static volatile bool			RightMotorStopFlag = true;
static volatile bool			LeftMotorReversed = false;
static volatile bool			RightMotorReversed = false;

static uint32_t 					WireSensorMeasurements[4];
static volatile float			LastWireSensorDifference=0;
static volatile float			WireSensorDerivative;

static bool 							controllersInit = false;
	
// Clock ticks per ms
static uint16_t ticksPerMS;

/*----------------------------- Public Functions --------------------------*/
/****************************************************************************
 Function
     StartLineFollowControl

 Parameters
     nothing

 Returns
     nothing

 Description
     Initializes everything needed for low level motor speed control
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void StartLineFollowControl( float desSpeed ){
	// Try to initialize motor controllers
	InitMotorSpeedControl();
	
	// Set desired speed
	SetDesiredSpeed( desSpeed );
	
	PWM_SetPolarity( PWM_NORMAL_POL, MOTOR_0PWM );
	PWM_SetPolarity( PWM_INV_POL, MOTOR_1PWM );
	
	// Unmask interrupts
	//HWREG( SPEED_CTRL_TIM+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
	HWREG( WIRE_CTRL_TIM+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
}

/****************************************************************************
 Function
     StartLineFollowControl

 Parameters
     nothing

 Returns
     nothing

 Description
     Initializes everything needed for low level motor speed control
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void StopLineFollowControl( void ){
	// Mask interrupts
	HWREG( SPEED_CTRL_TIM+TIMER_O_IMR) &= ~TIMER_IMR_TATOIM;
	HWREG( WIRE_CTRL_TIM+TIMER_O_IMR) &= ~TIMER_IMR_TBTOIM;
	
	// Clear interrupts
	HWREG( SPEED_CTRL_TIM+TIMER_O_ICR ) = TIMER_ICR_TATOCINT;
	HWREG( WIRE_CTRL_TIM+TIMER_O_ICR ) = TIMER_ICR_TBTOCINT;
	
	// Turn both motors off
	PWM_SetDutyCycle( MOTOR_STOP, MOTOR_0PWM, CHANNEL_A );
	PWM_SetDutyCycle( MOTOR_STOP, MOTOR_1PWM, CHANNEL_A );
}

/****************************************************************************
 Function
     InitMotorSpeedControl

 Parameters
     nothing

 Returns
     nothing

 Description
     Initializes everything needed for low level motor speed control
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void InitMotorSpeedControl( void ){
	// Check for previous init
	if ( controllersInit == false ){
		// Init AD service
		ADC_MultiInit( NUMBER_OF_ADC );

		// Controller timer interrupt
		InitControllerTimers();
		
		// PWM
		PWM_SetFrequency( MOTOR_PWM_FREQ, MOTOR_0PWM);
		PWM_SetFrequency( MOTOR_PWM_FREQ, MOTOR_1PWM);
		
		// Init encoders
		InitEncoderCapture();
		
		// Init stopped timers
		InitMotorStoppedTimers();
		
		// Set initialize flag
		controllersInit = true;
	}
}

/****************************************************************************
 Function
     SetIndividualMotorSpeed

 Parameters
     nothing

 Returns
     nothing

 Description
     Initializes everything needed for low level motor speed control
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void SetDesiredSpeed( float newSpeed ){
	// Update motor speeds
	DesiredSpeed = newSpeed;
}

/*----------------------------- Private Functions -------------------------*/
/****************************************************************************
 Function
     LeftDriveEncoderISR

 Parameters
     nothing

 Returns
     nothing

 Description
     Responds to edge capture interrupt for left motor encoder
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void LeftDriveEncoderISR( void ){
	// Local variable for this capture timer
	uint8_t counter=0;
	uint32_t ThisCapture;
	float currentSpeed;
	float sumSpeed=0;
	
	// Clear capture interrupt
	HWREG( ENCODER_MOT0_TIM+TIMER_O_ICR ) = TIMER_ICR_CAECINT;
	//HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_ICR ) = TIMER_ICR_CAECINT;
	
	// Get capture time and update period
	ThisCapture = HWREG( ENCODER_MOT0_TIM+TIMER_O_TAR );
	EncodePeriodLeft = ThisCapture - LastCaptureLeft;
	
	// Calculate speed
	currentSpeed = Period2Speed( EncodePeriodLeft );
	
	// Update array with new value - throw out last one
	while ( counter < (LEN_SPEED_ARRAY-1) ){
		SpeedLeftArray[counter+1] = SpeedLeftArray[counter];
		sumSpeed += SpeedLeftArray[counter];
		counter++;
	}
	
	SpeedLeftArray[0] = currentSpeed;
	sumSpeed += currentSpeed;
	
	// Take average speed
	EncoderSpeedLeft = sumSpeed / LEN_SPEED_ARRAY_F;
	
	if ( currentSpeed > 1 ){
		// Motor stop flag reset
		LeftMotorStopFlag = false;
		
		// Reset Timer B and enable it
		HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_TAV ) = 0;
		HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
	}
	
	// Update encoder ticks
	if ( LeftMotorReversed == false ) LeftEncoderTicks++;
	else LeftEncoderTicks--;
	
	// Update LastCapture
	LastCaptureLeft = ThisCapture;
}

/****************************************************************************
 Function
     LeftMotorStoppedISR

 Parameters
     nothing

 Returns
     nothing

 Description
     Responds to edge capture interrupt
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void LeftMotorStoppedISR( void ){
	uint8_t counter = 0;
	float sumSpeed;
	
	// Clear capture interrupt
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_ICR ) = TIMER_ICR_TAMCINT;
	
	// Ensure timer is off
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_CTL ) &= ~TIMER_CTL_TAEN;
	
	// Set motor stop flag
	LeftMotorStopFlag = true;
	
	// Update array with new value - throw out last one
	while ( counter < (LEN_SPEED_ARRAY-1) ){
		SpeedLeftArray[counter+1] = SpeedLeftArray[counter];
		sumSpeed += SpeedLeftArray[counter];
		counter++;
	}
	
	SpeedLeftArray[0] = 0.0f;
	
	EncoderSpeedLeft = 0.0f;
}

/****************************************************************************
 Function
     RightDriveEncoderISR

 Parameters
     nothing

 Returns
     nothing

 Description
     Responds to edge capture interrupt for left motor encoder
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void RightDriveEncoderISR( void ){
	// Local variable for this capture timer
	uint8_t counter=0;
	uint32_t ThisCapture;
	float currentSpeed;
	float sumSpeed;
	
	// Clear capture interrupt
	HWREG( ENCODER_MOT1_TIM+TIMER_O_ICR ) = TIMER_ICR_CBECINT;
	
	// Get capture time and update period
	ThisCapture = HWREG( ENCODER_MOT1_TIM+TIMER_O_TBR );
	EncodePeriodRight = ThisCapture - LastCaptureRight;
	
	// Calculate speed
	currentSpeed = Period2Speed( EncodePeriodRight );
	
	// Update array with new value - throw out last one
	while ( counter < (LEN_SPEED_ARRAY-1) ){
		SpeedRightArray[counter+1] = SpeedRightArray[counter];
		sumSpeed += SpeedRightArray[counter];
		counter++;
	}
	
	SpeedRightArray[0] = currentSpeed;
	sumSpeed += currentSpeed;
	
	// Take average speed
	EncoderSpeedRight = sumSpeed / LEN_SPEED_ARRAY_F;
	
	if ( currentSpeed > 1 ){
		// Motor stop flag reset
		RightMotorStopFlag = false;
		
		// Reset Timer B and enable it
		HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_TAV ) = 0;
		HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
	}
	
	// Update encoder ticks
	if ( RightMotorReversed == false ) RightEncoderTicks++;
	else RightEncoderTicks--;
	
	// Update LastCapture
	LastCaptureRight = ThisCapture;
}

/****************************************************************************
 Function
     RightMotorStoppedISR

 Parameters
     nothing

 Returns
     nothing

 Description
     Responds to edge capture interrupt
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void RightMotorStoppedISR( void ){
	uint8_t counter=0;
	float sumSpeed;
	
	// Clear capture interrupt
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_ICR ) = TIMER_ICR_TAMCINT;
	
	// Ensure timer is off
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_CTL ) &= ~TIMER_CTL_TAEN;
	
	// Set motor stop flag
	RightMotorStopFlag = true;
	
	// Update array with new value - throw out last one
	while ( counter < (LEN_SPEED_ARRAY-1) ){
		SpeedRightArray[counter+1] = SpeedRightArray[counter];
		sumSpeed += SpeedRightArray[counter];
		counter++;
	}
	
	SpeedRightArray[0] = 0.0f;
	
	EncoderSpeedRight = 0.0f;
}

/****************************************************************************
 Function
   SpeedControllerISR
 Parameters
   nothing
 Returns
   nothing
 Description
   Interrupt response for motor speed controller
 Notes
   none
 Author
  
John A Talbot, 01/12/17, 12:12
****************************************************************************/
// SpeedControllerISR takes nothing
void SpeedControllerISR( void ){	
	/*
	// Declare local variables
	float speedErrorLeft;
	float speedErrorRight;
	static float sumErrorLeft=0;
	static float sumErrorRight=0;
	float controlEffortLeft;
	float controlEffortRight;
	
	// Clear interrupt
	HWREG( SPEED_CTRL_TIM+TIMER_O_ICR ) = TIMER_ICR_TATOCINT;
	
	// Calculate speed error
	speedErrorLeft = fabsf(DesiredSpeedLeft) - EncoderSpeedLeft;
	speedErrorRight = fabsf(DesiredSpeedRight) - EncoderSpeedRight;
	
	// Add error to integral term
	sumErrorLeft += speedErrorLeft;
	sumErrorRight += speedErrorRight;
	
	// Calculate new control effort
	controlEffortLeft = SPEED_GAIN_P * (speedErrorLeft + SPEED_GAIN_I*sumErrorLeft);
	controlEffortRight = SPEED_GAIN_P * (speedErrorRight + SPEED_GAIN_I*sumErrorRight);
	
	// Check for control effort saturation
	if ( controlEffortLeft > SPEED_MAX_EFFORT ){
		// Reset integral term
		sumErrorLeft -= speedErrorLeft;
		
		// Clamp control effort at 100%
		controlEffortLeft = SPEED_MAX_EFFORT;
	}
	else if ( controlEffortLeft < SPEED_MIN_EFFORT ){
		// Reset integral term
		sumErrorLeft -= speedErrorLeft;
		
		// Clamp control effort at 0%
		controlEffortLeft = SPEED_MIN_EFFORT;
	}
	
	if ( controlEffortRight > SPEED_MAX_EFFORT ){
		// Reset integral term
		sumErrorRight -= speedErrorRight;
		
		// Clamp control effort at 100%
		controlEffortRight = SPEED_MAX_EFFORT;
	}
	else if ( controlEffortRight < SPEED_MIN_EFFORT ){
		// Reset integral term
		sumErrorRight -= speedErrorRight;
		
		// Clamp control effort at 0%
		controlEffortRight = SPEED_MIN_EFFORT;
	}
	
	// Check for correct direction of PWM
	if ( DesiredSpeedLeft < 0 ){
		PWM_SetPolarity(PWM_INV_POL, MOTOR_0PWM);
		LeftMotorReversed = true;
	}	else {
		PWM_SetPolarity(PWM_NORMAL_POL, MOTOR_0PWM);
		LeftMotorReversed = false;
	}
	
	if ( DesiredSpeedRight < 0 ){
		PWM_SetPolarity(PWM_NORMAL_POL, MOTOR_1PWM);
		RightMotorReversed = true;
	}	else {
		PWM_SetPolarity(PWM_INV_POL, MOTOR_1PWM);	
		RightMotorReversed = false;
	}
	  
	// Update duty cycle with new control effort
	//PWM_SetDutyCycle( fabsf(controlEffortLeft), MOTOR_0PWM, CHANNEL_A );
	//PWM_SetDutyCycle( fabsf(controlEffortRight), MOTOR_1PWM, CHANNEL_A );
	*/
}

/****************************************************************************
 Function
   PoseControllerISR
 Parameters
   nothing
 Returns
   nothing
 Description
   Interrupt response for motor speed controller
 Notes
   none
 Author
  
John A Talbot, 01/12/17, 12:12
****************************************************************************/
// SpeedControllerISR takes nothing
void WireFollowControllerISR( void ){
	// Initialize local variables
	float leftVoltage;
	float rightVoltage;
	float WireSensorDifference;
	float SpeedDifference;
	
	// Clear interrupt
	HWREG( WIRE_CTRL_TIM+TIMER_O_ICR ) = TIMER_ICR_TBTOCINT;
	
	// Get ADC values
	ADC_MultiRead( WireSensorMeasurements );
	
	// Left voltage
	leftVoltage = (WireSensorMeasurements[LEFT_WIRE_SENSOR]/4096.f) - 1.5f;
	
	// Right voltage
	rightVoltage = (WireSensorMeasurements[RIGHT_WIRE_SENSOR]/4096.f) - 1.5f;	
	
	//printf("Left Voltage = %f\t\tRight Voltage = %f\r\n\n", leftVoltage, rightVoltage);
	
	// Calculate Difference
	WireSensorDifference = leftVoltage - rightVoltage;
	
	// Calculate Derivative
	WireSensorDerivative = (WireSensorDifference - LastWireSensorDifference);
	
	// Calculate control effort
	SpeedDifference = (WIRE_GAIN_P * WireSensorDifference) + (WIRE_GAIN_D * WireSensorDerivative);
	
	// Update motor speed controllers
	DesiredSpeedLeft = DesiredSpeed - SpeedDifference;
	DesiredSpeedRight = DesiredSpeed + SpeedDifference;
	
	// Check for control effort saturation
	if ( DesiredSpeedLeft > SPEED_MAX_EFFORT ){
		
		// Clamp control effort at 100%
		DesiredSpeedLeft = SPEED_MAX_EFFORT;
	}
	else if ( DesiredSpeedLeft < SPEED_MIN_EFFORT ){
		
		// Clamp control effort at 0%
		DesiredSpeedLeft = SPEED_MIN_EFFORT;
	}
	
	if ( DesiredSpeedRight > SPEED_MAX_EFFORT ){
		
		// Clamp control effort at 100%
		DesiredSpeedRight = SPEED_MAX_EFFORT;
	}
	else if ( DesiredSpeedRight < SPEED_MIN_EFFORT ){
		
		// Clamp control effort at 0%
		DesiredSpeedRight = SPEED_MIN_EFFORT;
	}
	  
	// Update duty cycle with new control effort
	PWM_SetDutyCycle( fabsf(DesiredSpeedLeft), MOTOR_0PWM, CHANNEL_A );
	PWM_SetDutyCycle( fabsf(DesiredSpeedRight), MOTOR_1PWM, CHANNEL_A );
	
	// Update last difference
	LastWireSensorDifference = WireSensorDifference;
}

/*----------------------------- Private Functions --------------------------*/
/****************************************************************************
 Function
   InitControllerTimers
 Parameters
   nothing
 Returns
   nothing
 Description
   Initialize periodic hardware timer to trigger controller
	 update
 Notes
   none
 Author
  
John A Talbot, 01/12/17, 12:12
****************************************************************************/
// initControllerTimer takes nothing
void InitControllerTimers( void ){
	// Get clock ticks per ms
	ticksPerMS = SysCtlClockGet() / MS_PER_S;
	
	// Enable clock to wide timer 2
	HWREG( SYSCTL_RCGCWTIMER ) |= SYSCTL_RCGCWTIMER_R2;
	
	// Loop until timer hardware is ready
	while( (HWREG( SYSCTL_PRWTIMER ) & SYSCTL_PRWTIMER_R2 ) != SYSCTL_PRWTIMER_R2 );
	
	// Disable timer A before configuring
	HWREG( SPEED_CTRL_TIM+TIMER_O_CTL ) &= ~TIMER_CTL_TAEN;
	HWREG( WIRE_CTRL_TIM+TIMER_O_CTL ) &= ~TIMER_CTL_TBEN;
	
	// Configure timer for 32bit (individual instead of concatenated)
	// Macro define 16bit refers to individual timer rather than actual 16bit
	HWREG( SPEED_CTRL_TIM+TIMER_O_CFG) = TIMER_CFG_16_BIT;
	
	// Load 2ms period into ILR register to fire interrupt every
	// 2ms
	HWREG( SPEED_CTRL_TIM+TIMER_O_TAILR ) = ticksPerMS*SPEED_CONTROL_UPDATE_PERIOD;
	HWREG( WIRE_CTRL_TIM+TIMER_O_TBILR ) = ticksPerMS*WIRE_CONTROL_UPDATE_PERIOD;
	
	// Set Timer A into periodic mode
	HWREG( SPEED_CTRL_TIM+TIMER_O_TAMR ) = (HWREG( SPEED_CTRL_TIM+TIMER_O_TAMR ) &
		~TIMER_TAMR_TAAMS) | TIMER_TAMR_TAMR_PERIOD;
		
	HWREG( WIRE_CTRL_TIM+TIMER_O_TBMR ) = (HWREG( WIRE_CTRL_TIM+TIMER_O_TBMR ) &
		~TIMER_TBMR_TBAMS) | TIMER_TBMR_TBMR_PERIOD;
		
	// Set Timer A to count down
	HWREG( SPEED_CTRL_TIM+TIMER_O_TAMR ) &= ~TIMER_TAMR_TACDIR;
	HWREG( WIRE_CTRL_TIM+TIMER_O_TBMR ) &= ~TIMER_TBMR_TBCDIR;
		
	// Configure timeout interrupt in timer register
	//HWREG( SPEED_CTRL_TIM+TIMER_O_IMR) |= TIMER_IMR_TATOIM;
	HWREG( WIRE_CTRL_TIM+TIMER_O_IMR) |= TIMER_IMR_TBTOIM;
	
	// Enable interrupt in NVIC register
	HWREG( SPEED_CTRL_NVIC_REG ) |= SPEED_CTRL_NVIC_BIT;
	HWREG( WIRE_CTRL_NVIC_REG ) |= WIRE_CTRL_NVIC_BIT;
	
	// Set interrupt priority in NVIC vector table
	HWREG( SPEED_CTRL_PRI_REG ) = (HWREG( SPEED_CTRL_PRI_REG ) & ~SPEED_CTRL_PRI_M) +
		(SPEED_CTRL_PRI << SPEED_CTRL_PRI_S);
		
	HWREG( WIRE_CTRL_PRI_REG ) = (HWREG( WIRE_CTRL_PRI_REG ) & ~WIRE_CTRL_PRI_M) +
		(WIRE_CTRL_PRI << WIRE_CTRL_PRI_S);
	
	// Turn on interrupts globally
	__enable_irq();
	
	// Start timer and enable stall in debugging
	//HWREG( SPEED_CTRL_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
	HWREG( WIRE_CTRL_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}

/****************************************************************************
 Function
     InitMotorStoppedTimers

 Parameters
     nothing

 Returns
     nothing

 Description
     Configures timer capture for pin PC4 to use as encoder speed reading
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
static void InitMotorStoppedTimers( void ){
	// Enable clock to timer 1 and 2
	HWREG( SYSCTL_RCGCTIMER ) |= SYSCTL_RCGCTIMER_R1;
	HWREG( SYSCTL_RCGCTIMER ) |= SYSCTL_RCGCTIMER_R2;
	
	// Loop until timer hardware is ready
	while( (HWREG( SYSCTL_PRTIMER ) & SYSCTL_PRTIMER_R1 ) != SYSCTL_PRTIMER_R1 );
	while( (HWREG( SYSCTL_PRTIMER ) & SYSCTL_PRTIMER_R2 ) != SYSCTL_PRTIMER_R2 );
	
	// Disable timer 1 and 2 before configuring
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_CTL ) &= ~(TIMER_CTL_TAEN | TIMER_CTL_TBEN);
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_CTL ) &= ~(TIMER_CTL_TAEN | TIMER_CTL_TBEN);
	
	// Configure timer for 32bit (concatenated instead of individual)
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_CFG) = TIMER_CFG_32_BIT_TIMER;
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_CFG) = TIMER_CFG_32_BIT_TIMER;
	
	// Load highest value (0xFFFFFFFF) into load register
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_TAILR ) = CLOCK_FULL_LOAD;
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_TAILR ) = CLOCK_FULL_LOAD;
	
	// Load match value into timer 1 and 2
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_TAMATCHR ) = NOT_MOVING_TIMEOUT;
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_TAMATCHR ) = NOT_MOVING_TIMEOUT;
	
	// Set Timer 1 and 2 into one-shot mode
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_TAMR ) = (HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_TAMR ) &
		~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TAMIE | TIMER_TAMR_TAMR_1_SHOT);
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_TAMR ) = (HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_TAMR ) &
		~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TAMIE | TIMER_TAMR_TAMR_1_SHOT);
	
	// Set Timer 1 to count up and 2 to count up
	HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_TAMR ) |= TIMER_TAMR_TACDIR;
	HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_TAMR ) |= TIMER_TAMR_TACDIR;
	
	// Configure local capture interrupt in timer register
	//HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_IMR) |= TIMER_IMR_TAMIM;
	//HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_IMR) |= TIMER_IMR_TAMIM;
	
	// Enable interrupt in NVIC register
	HWREG( DR_ENCODE_STOP_NVIC_REG ) |= DR_ENCODE_STOP0_NVIC;
	HWREG( DR_ENCODE_STOP_NVIC_REG ) |= DR_ENCODE_STOP1_NVIC;
	
	// Set interrupt priority in NVIC vector table
	HWREG( DR_ENCODE_STOP_PRI_REG ) = (HWREG( DR_ENCODE_STOP_PRI_REG ) & ~DR_ENCODE_STOP0_PRI_M) +
		(DR_ENCODE_PRI << DR_ENCODE_STOP0_PRI_S);
		
	HWREG( DR_ENCODE_STOP_PRI_REG ) = (HWREG( DR_ENCODE_STOP_PRI_REG ) & ~DR_ENCODE_STOP1_PRI_M) +
		(DR_ENCODE_PRI << DR_ENCODE_STOP1_PRI_S);
	
	// Turn on interrupts globally
	__enable_irq();
	
	// Start timer and enable stall in debugging
	//HWREG( ENCODER_MOT0_STOP_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);	
	//HWREG( ENCODER_MOT1_STOP_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}

/****************************************************************************
 Function
     EncoderCapturePinInit()

 Parameters
     nothing

 Returns
     nothing

 Description
     Configures GPIO hardware for use with timer edge capture
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
static void InitEncoderCapturePin( void ){
	// Select alternate function for pin PC4
	HWREG( DRIVE_ENCODER_PORT+GPIO_O_AFSEL ) |= (ENCODER_MOT0_PIN | ENCODER_MOT1_PIN);
	
	// Configure MUX to map timer capture to PC4
	HWREG( DRIVE_ENCODER_PORT+GPIO_O_PCTL ) = (HWREG( DRIVE_ENCODER_PORT+GPIO_O_PCTL ) & 
		~AFSEL_MUX_PD2_M) | AFSEL_WTIM3_PD2;
	HWREG( DRIVE_ENCODER_PORT+GPIO_O_PCTL ) = (HWREG( DRIVE_ENCODER_PORT+GPIO_O_PCTL ) & 
		~AFSEL_MUX_PD3_M) | AFSEL_WTIM3_PD3;
	
	// Enable pin for digital IO
	HWREG( DRIVE_ENCODER_PORT+GPIO_O_DEN ) |= (ENCODER_MOT0_PIN | ENCODER_MOT1_PIN);
	
	// Set pin direction for input
	HWREG( DRIVE_ENCODER_PORT+GPIO_O_DIR ) &= ~(ENCODER_MOT0_PIN | ENCODER_MOT1_PIN);
}

/****************************************************************************
 Function
     InitEncoderCapture()

 Parameters
     nothing

 Returns
     nothing

 Description
     Configures GPIO hardware for use with timer edge capture
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
static void InitEncoderCapture ( void ){
	// Enable clock to wide timer 3
	HWREG( SYSCTL_RCGCWTIMER ) |= SYSCTL_RCGCWTIMER_R3;
	
	// Check if clock is enabled to port D - if not, enable it
	if ( (HWREG( SYSCTL_PRGPIO ) & SYSCTL_PRGPIO_R3) != SYSCTL_PRGPIO_R3 ){
		// Enable port D clock
		HWREG( SYSCTL_RCGCGPIO ) |= SYSCTL_RCGCGPIO_R3;
		
		// Loop until port is ready
		while( (HWREG( SYSCTL_PRGPIO ) & SYSCTL_PRGPIO_R3) != SYSCTL_PRGPIO_R3 );
	}
	
	// Loop until timer hardware is ready
	while( (HWREG( SYSCTL_PRWTIMER ) & SYSCTL_PRWTIMER_R3 ) != SYSCTL_PRWTIMER_R3 );
	
	// Disable timer A and B before configuring
	HWREG( ENCODER_MOT0_TIM+TIMER_O_CTL ) &= ~TIMER_CTL_TAEN;
	HWREG( ENCODER_MOT1_TIM+TIMER_O_CTL ) &= ~TIMER_CTL_TBEN;
	
	// Configure timer for 32bit (individual instead of concatenated)
	// Macro define 16bit refers to individual timer rather than actual 16bit
	HWREG( ENCODER_MOT0_TIM+TIMER_O_CFG) = TIMER_CFG_16_BIT;
	
	// Load highest value (0xFFFFFFFF) into load register
	HWREG( ENCODER_MOT0_TIM+TIMER_O_TAILR ) = CLOCK_FULL_LOAD;
	HWREG( ENCODER_MOT1_TIM+TIMER_O_TBILR ) = CLOCK_FULL_LOAD;
	
	// Set Timer A and Timer B into edge time capture mode
	HWREG( ENCODER_MOT0_TIM+TIMER_O_TAMR ) = (HWREG( ENCODER_MOT0_TIM+TIMER_O_TAMR ) &
		~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
	
	HWREG( ENCODER_MOT1_TIM+TIMER_O_TBMR ) = (HWREG( ENCODER_MOT1_TIM+TIMER_O_TBMR ) &
		~TIMER_TBMR_TBAMS) | (TIMER_TBMR_TBCMR | TIMER_TBMR_TBMR_CAP);
	
	// Set Timer A to count up and B to count up
	HWREG( ENCODER_MOT0_TIM+TIMER_O_TAMR ) |= TIMER_TAMR_TACDIR;
	HWREG( ENCODER_MOT1_TIM+TIMER_O_TBMR ) |= TIMER_TBMR_TBCDIR;
	
	// Set Event to rising edge capture
	HWREG( ENCODER_MOT0_TIM+TIMER_O_CTL ) = ( HWREG( ENCODER_MOT0_TIM+TIMER_O_CTL ) & 
		~TIMER_CTL_TAEVENT_M ) + TIMER_CTL_TAEVENT_POS;
		
	HWREG( ENCODER_MOT1_TIM+TIMER_O_CTL ) = ( HWREG( ENCODER_MOT1_TIM+TIMER_O_CTL ) & 
		~TIMER_CTL_TBEVENT_M ) + TIMER_CTL_TBEVENT_POS;
	
	// Configure GPIO hardware for capture
	InitEncoderCapturePin();
	
	// Configure local capture interrupt in timer register
	//HWREG( ENCODER_MOT0_TIM+TIMER_O_IMR) |= TIMER_IMR_CAEIM;
	//HWREG( ENCODER_MOT1_TIM+TIMER_O_IMR) |= TIMER_IMR_CBEIM;
	
	// Enable interrupt in NVIC register
	HWREG( DR_ENCODE_NVIC_REG ) |= DR_ENCODE_NVIC_TMRA;
	HWREG( DR_ENCODE_NVIC_REG ) |= DR_ENCODE_NVIC_TMRB;
	
	// Set interrupt priority in NVIC vector table
	HWREG( DR_ENCODE_PRI_REG ) = (HWREG( DR_ENCODE_PRI_REG ) & ~DR_ENCODE_A_PRI_M) +
		(DR_ENCODE_PRI << DR_ENCODE_A_PRI_S);
		
	HWREG( DR_ENCODE_PRI_REG ) = (HWREG( DR_ENCODE_PRI_REG ) & ~DR_ENCODE_B_PRI_M) +
		(DR_ENCODE_PRI << DR_ENCODE_B_PRI_S);
	
	// Turn on interrupts globally
	__enable_irq();
	
	// Start timer and enable stall in debugging
	//HWREG( ENCODER_MOT0_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);	
	//HWREG( ENCODER_MOT1_TIM+TIMER_O_CTL ) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}

/****************************************************************************
 Function
     Period2Speed()

 Parameters
     nothing

 Returns
     uint16_t speed

 Description
     Converts captured period to angular speed in rad/s
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
static float Period2Speed( float period ){
	// Local variable speed
	float localSpeed;
	
	// Calculate speed
	localSpeed = SysCtlClockGet() * SEC_PER_MIN / PULSE_PER_REV / GEAR_RATIO / period;
		
	return localSpeed;
}

/****************************************************************************
 Function
     SetIndividualMotorSpeed

 Parameters
     nothing

 Returns
     nothing

 Description
     Initializes everything needed for low level motor speed control
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
void SetIndividualMotorSpeed( float LeftSpeed, float RightSpeed ){
	// Update motor speeds
	DesiredSpeedLeft = LeftSpeed;
	DesiredSpeedRight = RightSpeed;	
}

/*------------------------------ Test Harness -----------------------------*/
#ifdef TEST
// **** NOTE: This test harness relies on a periodic wide timer timeout 
// interrupt to function correctly. To ensure function, add PWMTestISR
// to the ISR vector table corresponding to wide timer 0A in 
// startup_rvmdk.S

#define TEST_UPDATE 2
#define NVIC_TMRA_EN BIT30HI
#define BOTH_GENS 2

#define LED_PORT GPIO_PORTF_BASE
#define RED_PIN GPIO_PIN_1
#define BLU_PIN GPIO_PIN_2
#define GRN_PIN GPIO_PIN_3

#define clrScrn() 	printf("\x1b[2J")

// Begin test harness to test effectiveness of functions in PWM_module
int main ( void ){
	// Set the clock to run at 40MhZ using the PLL and 16MHz external crystal
	SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN
			| SYSCTL_XTAL_16MHZ);
	TERMIO_Init();
	clrScrn();
	
	// Variable so compiler doesn't optimize for loop pause away
	uint32_t dummyCount=0;
	
	// Print test harness information
	puts("\rStarting Test Harness for Wire Follow Controller\r\n");
	puts("\rRobot should move forward 2 ft, left 1 ft, and turn 90 degrees CCW\r\n");
	printf("%s %s\n",__TIME__, __DATE__);
	printf("\n\r\n");
	
	// Initialize PWM
	PWM_InitHardware();
	
	// Initialize ADC
	ADC_MultiInit( NUMBER_OF_ADC );
	
	// Initialize all hardware
	InitMotorSpeedControl();
	
	// Invert PWM on left motor
	PWM_SetPolarity( PWM_NORMAL_POL, MOTOR_0PWM );
	PWM_SetPolarity( PWM_INV_POL, MOTOR_1PWM );
	
	// Set desired speed to 20
	SetDesiredSpeed( 35.0f );
	
	// Pause for loop
	for ( uint64_t i=0; i<200000000; i++ ){
		dummyCount++;
	}
	
	// Loop forever
	while ( true );
}

// End of test harness
#endif