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

Module
ISR.c

Revision
1.0.1

Description
This is a template file for implementing flat state machines under the
Gen2 Events and Services Framework.

Notes

History
When Who What/Why
-------------- --- --------
01/15/12 11:12 jec revisions for Gen2 framework
11/07/11 11:26 jec made the queue static
10/30/11 17:59 jec fixed references to CurrentEvent in RunTemplateSM()
10/23/11 18:20 jec began conversion from SMTemplate.c (02/20/07 rev)
****************************************************************************/
/*----------------------------- 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 "ES_Configure.h"
#include "ES_Framework.h"
#include "PWMService.h"
// the common headers for C99 types
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include "ES_Port.h"
#include "termio.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 "BITDEFS.H"

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

#define TicksPerMS 40000
#define PPR 12
#define CLK_TICKS_PER_MIN 2400000000
#define MAXSPEED_PERIOD 85000
#define MINSPEED_PERIOD 4000000
#define LeftPropGain 1.0
#define RightPropGain 1.0
#define LeftIntegGain 0.1
#define RightIntegGain 0.1
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine.They should be functions
relevant to the behavior of this state machine
*/

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

// everybody needs a state variable, you may need others as well.
// type of state variable should match htat of enum in header file

static uint32_t PeriodLeftEnc, PeriodRightEnc, PeriodW3TA, PeriodW3TB, PeriodW5TA;

static uint32_t LastCaptureLeftEnc, LastCaptureRightEnc, LastCaptureW3TA,
LastCaptureW3TB, LastCaptureW5TA;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static int32_t LeftRPMError;
static uint32_t LeftTargetRPM;
static uint32_t LeftCurrentRPM;
static int32_t RightRPMError;
static uint32_t RightTargetRPM;
static uint32_t RightCurrentRPM;
static uint8_t RightNewDuty;
static uint8_t LeftNewDuty;
static float LeftIntegTerm = 0.0;
static float LeftLastError;

//static float LeftNewDuty;

static float RightIntegTerm = 0.0;
static float RightLastError;

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

/****************************************************************************
Function
InitInputCapture Period
Parameters

Returns

Description
Intializes the necessary hardware and software for Input Capture Response interrupt
Notes

Author
J. Edward Carryer, 10/23/11, 18:55
****************************************************************************/
void InitInputCapturePeriod(void)
{
// start by enabling the clock to the timer (Wide Timer 1,3,5)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R1;
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R3;
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R5;
// enable the clock to Port C and Port D
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2;
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R3;
// kill a few cycles to let the clock get going
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R1) != SYSCTL_PRWTIMER_R1)
{}
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R3) != SYSCTL_PRWTIMER_R3)
{}
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R5) != SYSCTL_PRWTIMER_R5)
{}
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R2) != SYSCTL_PRGPIO_R2)
{}
 while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R3) != SYSCTL_PRGPIO_R3)
{}

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

HWREG(WTIMER1_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
HWREG(WTIMER1_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
HWREG(WTIMER3_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
HWREG(WTIMER3_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEN;
HWREG(WTIMER5_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(WTIMER1_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;
HWREG(WTIMER3_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;
HWREG(WTIMER5_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;
// we want to use the full 32 bit count, so initialize the Interval Load
// register to 0xffff.ffff (its default value :-)
HWREG(WTIMER1_BASE + TIMER_O_TAILR) = 0xffffffff;
HWREG(WTIMER1_BASE + TIMER_O_TBILR) = 0xffffffff;
HWREG(WTIMER3_BASE + TIMER_O_TAILR) = 0xffffffff;
HWREG(WTIMER3_BASE + TIMER_O_TBILR) = 0xffffffff;

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

HWREG(WTIMER5_BASE + TIMER_O_TAMR) = (HWREG(WTIMER5_BASE + TIMER_O_TAMR) &
~TIMER_TAMR_TAMR_M) | TIMER_TAMR_TAMR_PERIOD;
// set timeout to 2 mS
HWREG(WTIMER5_BASE + TIMER_O_TAILR) = TicksPerMS * 2;
// enable a local timeout interrupt
HWREG(WTIMER5_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// HWREG(NVIC_PRI24) = (HWREG(NVIC_PRI24) & ~NVIC_PRI24_INTA_M) |
NVIC_PRI24_INTA_S;

// set up timer A in capture mode (TAMR=3, TAAMS = 0),

// for edge time (TACMR = 1) and up-counting (TACDIR = 1)
HWREG(WTIMER1_BASE + TIMER_O_TAMR) = (HWREG(WTIMER1_BASE + TIMER_O_TAMR) &
~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
HWREG(WTIMER1_BASE + TIMER_O_TBMR) = (HWREG(WTIMER1_BASE + TIMER_O_TBMR) &
~TIMER_TBMR_TBAMS) | (TIMER_TBMR_TBCDIR | TIMER_TBMR_TBCMR | TIMER_TBMR_TBMR_CAP);
HWREG(WTIMER3_BASE + TIMER_O_TAMR) = (HWREG(WTIMER3_BASE + TIMER_O_TAMR) &
~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
HWREG(WTIMER3_BASE + TIMER_O_TBMR) = (HWREG(WTIMER3_BASE + TIMER_O_TBMR) &
~TIMER_TBMR_TBAMS) | (TIMER_TBMR_TBCDIR | TIMER_TBMR_TBCMR | TIMER_TBMR_TBMR_CAP);
// HWREG(WTIMER5_BASE + TIMER_O_TAMR) = (HWREG(WTIMER5_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 edge = 00, so we clear the TAEVENT bits
HWREG(WTIMER1_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
HWREG(WTIMER1_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEVENT_M;
HWREG(WTIMER3_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
HWREG(WTIMER3_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEVENT_M;
// HWREG(WTIMER5_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 C bit 4 (WT0CCP0)
HWREG(GPIO_PORTC_BASE + GPIO_O_AFSEL) |= BIT6HI;
HWREG(GPIO_PORTC_BASE + GPIO_O_AFSEL) |= BIT7HI;
HWREG(GPIO_PORTD_BASE + GPIO_O_AFSEL) |= BIT2HI;
HWREG(GPIO_PORTD_BASE + GPIO_O_AFSEL) |= BIT3HI;
// HWREG(GPIO_PORTD_BASE + GPIO_O_AFSEL) |= BIT6HI;
// Then, map bit 4's alternate function to WT0CCP0
// 7 is the mux value to select WT0CCP0, 16 to shift it over to the
// right nibble for bit 4 (4 bits/nibble * 4 bits)
HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) &
0xfff0ffff) + (7 << 24);
HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) &
0xfff0ffff) + (7 << 28);
HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) &
0xfff0ffff) + (7 << 8);
HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) &
0xfff0ffff) + (7 << 12);
// HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTD_BASE + GPIO_O_PCTL) &
0xfff0ffff) + (7 << 24);
// Enable pin on Port C for digital I/O
HWREG(GPIO_PORTC_BASE + GPIO_O_DEN) |= (BIT6HI | BIT7HI);
HWREG(GPIO_PORTD_BASE + GPIO_O_DEN) |= (BIT2HI | BIT3HI);
// make pin 4 on Port C into an input
HWREG(GPIO_PORTC_BASE + GPIO_O_DIR) &= (BIT7LO & BIT6LO);
HWREG(GPIO_PORTD_BASE + GPIO_O_DIR) &= (BIT2LO & BIT3LO);
// back to the timer to enable a local capture interrupt
HWREG(WTIMER1_BASE + TIMER_O_IMR) |= TIMER_IMR_CAEIM;
HWREG(WTIMER1_BASE + TIMER_O_IMR) |= TIMER_IMR_CBEIM;
HWREG(WTIMER3_BASE + TIMER_O_IMR) |= TIMER_IMR_CAEIM;
HWREG(WTIMER3_BASE + TIMER_O_IMR) |= TIMER_IMR_CBEIM;
// HWREG(WTIMER5_BASE + TIMER_O_IMR) |= TIMER_IMR_CAEIM;

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

// it is interrupt number 94 so appears in EN2 at bit 30
HWREG(NVIC_EN3) |= (BIT0HI | BIT1HI | BIT4HI | BIT5HI | BIT8HI);
HWREG(NVIC_PRI24) &= (BIT5LO & BIT6LO & BIT7LO & BIT13LO & BIT14LO);
HWREG(NVIC_PRI24) |= BIT15HI;
HWREG(NVIC_PRI25) &= (BIT5LO & BIT7LO & BIT13LO);
HWREG(NVIC_PRI25) |= BIT6HI | BIT14HI | BIT15HI;
HWREG(NVIC_PRI26) |= BIT5HI | BIT6HI | BIT7HI;
// 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(WTIMER1_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
HWREG(WTIMER1_BASE + TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
HWREG(WTIMER3_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
HWREG(WTIMER3_BASE + TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
// HWREG(WTIMER5_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
// now kick the timer offby enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER5_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
printf("Interrupts enabled");
}

void InitPeriodicInt(void)
{
// start by enabling the clock to the timer (Wide Timer 1)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R5;
// wait for the clock to get going
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R5) != SYSCTL_PRWTIMER_R5)
{}
// make sure that timer (Timer A)is disabled before configuring
HWREG(WTIMER5_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// set it up in 32bit wide (individual, not concatenated) mode
HWREG(WTIMER5_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;
// set up timer A in periodic mode so that itrepeats the time-outs
HWREG(WTIMER5_BASE + TIMER_O_TAMR) = (HWREG(WTIMER5_BASE + TIMER_O_TAMR) &
~TIMER_TAMR_TAMR_M) | TIMER_TAMR_TAMR_PERIOD;
// set timeout to 2 mS
HWREG(WTIMER5_BASE + TIMER_O_TAILR) = TicksPerMS * 2;
// enable a local timeout interrupt
HWREG(WTIMER5_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM;
// HWREG(NVIC_PRI24) = (HWREG(NVIC_PRI24) & ~NVIC_PRI24_INTA_M) |
NVIC_PRI24_INTA_S;
// enable the Timer A in Wide Timer 1 interrupt in the NVIC
// it is interrupt number 96 so appears in EN3 at bit 0
HWREG(NVIC_EN3) |= BIT8HI;
// make sure interrupts are enabled globally
// __enable_irq();
// now kick the timer offby enabling it and enabling the timer to
// stall while stopped by the debugger
HWREG(WTIMER5_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN |
TIMER_CTL_TASTALL);
}

/****************************************************************************
Function
InputCaptureResponse

Parameters

Returns

Description
Calculates the period between two encoder signals
Notes

Author
J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
void InputCaptureResponseLeftEnc(void)
{
uint32_t ThisCapture;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER1_BASE + TIMER_O_ICR) = TIMER_ICR_CAECINT;
// now grab the captured value and calculate the period
ThisCapture = HWREG(WTIMER1_BASE + TIMER_O_TAR);
PeriodLeftEnc = ThisCapture - LastCaptureLeftEnc;

if (PeriodLeftEnc > MINSPEED_PERIOD)

{
PeriodLeftEnc = MINSPEED_PERIOD;
}
if (PeriodLeftEnc < MAXSPEED_PERIOD)
{
PeriodLeftEnc = MAXSPEED_PERIOD;
}
// update LastCapture to prepare for the next edge
//printf("\n\rPeriod in W1TA is %d", PeriodW1TA);
//printf("Interrupt");
LastCaptureLeftEnc = ThisCapture;
}

void InputCaptureResponseRightEnc(void)
{
uint32_t ThisCapture;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER1_BASE + TIMER_O_ICR) = TIMER_ICR_CBECINT;
// now grab the captured value and calculate the period
ThisCapture = HWREG(WTIMER1_BASE + TIMER_O_TBR);
PeriodRightEnc = ThisCapture - LastCaptureRightEnc;
if (PeriodRightEnc > MINSPEED_PERIOD)
{
PeriodRightEnc = MINSPEED_PERIOD;
}
 if (PeriodRightEnc < MAXSPEED_PERIOD)
{
PeriodRightEnc = MAXSPEED_PERIOD;
}
// update LastCapture to prepare for the next edge

LastCaptureRightEnc = ThisCapture;
}

void InputCaptureResponseW3TA(void)
{
uint32_t ThisCapture;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER3_BASE + TIMER_O_ICR) = TIMER_ICR_CAECINT;
// now grab the captured value and calculate the period
ThisCapture = HWREG(WTIMER3_BASE + TIMER_O_TAR);
PeriodW3TA = ThisCapture - LastCaptureW3TA;
// update LastCapture to prepare for the next edge
//printf("\n\rPeriod in W3TA is %d", PeriodW3TA);
LastCaptureW3TA = ThisCapture;
}

void InputCaptureResponseW3TB(void)
{
uint32_t ThisCapture;
// start by clearing the source of the interrupt, the input capture event
HWREG(WTIMER3_BASE + TIMER_O_ICR) = TIMER_ICR_CBECINT;
// now grab the captured value and calculate the period
ThisCapture = HWREG(WTIMER3_BASE + TIMER_O_TBR);
PeriodW3TB = ThisCapture - LastCaptureW3TB;
// update LastCapture to prepare for the next edge
//printf("\n\rPeriod in W3TB is %d", PeriodW3TB);
LastCaptureW3TB = ThisCapture;
}

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

static uint8_t CurrentPB0;

static uint8_t CurrentPB1;

//static float RightNewDuty;

//printf("Interrupt Periodic");

LeftCurrentRPM = ((600000 / (12 * 115)) * 40000) / PeriodLeftEnc;

LeftRPMError = LeftTargetRPM - LeftCurrentRPM;

RightCurrentRPM = ((600000 / (12 * 115)) * 40000) / PeriodRightEnc;

RightRPMError = RightTargetRPM - RightCurrentRPM;

LeftIntegTerm += LeftIntegGain * LeftRPMError;

if (LeftIntegTerm > 100)

{
LeftIntegTerm = 100;
}
else if (LeftIntegTerm < 0)
{
LeftIntegTerm = 0;
}
 LeftNewDuty = LeftPropGain * LeftRPMError + LeftIntegTerm;

if (LeftNewDuty > 100)

{
LeftNewDuty = 100;
}
else if (LeftNewDuty < 0)
{
LeftNewDuty = 0;
}
CurrentPB0 = HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) & BIT0HI;
if (CurrentPB0)
{
LeftNewDuty = 100 - LeftNewDuty;
}

LeftLastError = LeftRPMError;
// SetDutyPB4((LeftNewDuty));

RightIntegTerm += RightIntegGain * RightRPMError;

if (RightIntegTerm > 100)

{
RightIntegTerm = 100;
}
else if (RightIntegTerm < 0)
{
RightIntegTerm = 0;
}

//duty_pterm = RightPropGain*RightRPMError;
//printf("P term Duty %f \r\n", QueryDutyPterm());
RightNewDuty = RightPropGain * RightRPMError + RightIntegTerm;
CurrentPB1 = HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) & BIT1HI;
if (RightNewDuty > 100)
{
RightNewDuty = 100;
}
else if (RightNewDuty < 0)
{
RightNewDuty = 0;
}

if (CurrentPB1)
{
RightNewDuty = 100 - RightNewDuty;
}
// printf("\n\r Left Target is %d and Left Actual is
%d",LeftTargetRPM,LeftCurrentRPM);
// printf("\n\r Right Target is %d and Right Actual is
%d",RightTargetRPM,RightCurrentRPM);
RightLastError = RightRPMError;
// SetDutyPB5((RightNewDuty));
}

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

/***************************************************************************
private functions
***************************************************************************/
uint32_t QueryPeriodW1TA(void)
{
return PeriodLeftEnc;
}
uint32_t QueryPeriodW3TA(void)
{
return PeriodW3TA;
}

uint32_t QueryPeriodW5TA(void)
{
return PeriodW5TA;
}

uint32_t QueryPeriodW1TB(void)
{
return PeriodRightEnc;
}

uint32_t QueryPeriodW3TB(void)
{
return PeriodW3TB;
}

void SetLeftSpeed(uint32_t RPM)

{
LeftTargetRPM = RPM;
}

void SetRightSpeed(uint32_t RightSpeed)

{
RightTargetRPM = RightSpeed;
}

uint8_t QuerySpeedLeft(void)
{
return LeftCurrentRPM;
}

uint8_t QuerySpeedRight(void)
{
return RightCurrentRPM;
}