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

Module
MotorEncoders.c

Description
This module implements encoder close loop drive

****************************************************************************/
/*----------------------------- 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 "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 "MotorEncoders.h"
#include "MotorDrive.h"
#include "MasterSM.h"

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

// these times assume a 1.000mS/tick timing
#define TICKS_MS 40000

#define MAX_DUTY 100

#define BitsPerNibble 4

#define PULSE_OUTPUT_ROTATION 300

#define PRI_A_SHIFT 4
#define PRI_B_SHIFT 12
#define PRI_C_SHIFT 20
#define PRI_D_SHIFT 28

#define BETA 0.9

#define REASONABLE_RPM 90
#define NUM_PULSES_PITCHED 15

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

/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
static void RestartLeftStopShot(void);
static void RestartRightStopShot(void);

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

// with the introduction of Gen2, we need a module level Priority variable

// static varialbes (Left then Right in array index)

static uint32_t LastEncoderEdge[2] = { 0, 0 };

static uint32_t ThisEncoderEdge[2] = { 0, 0 };
static uint32_t EncoderPeriod[2];
static uint32_t EncoderCount[2] = { 0, 0 };

static float ActRPM[2] = { 0, 0 };

static float SetRPM[2];
static float Error[2] = { 0, 0 };
static float DerivativeTerm[2] = { 0, 0 };
static float ErrorSum[2] = { 0, 0 };
static float LastError[2] = { 0, 0 };
static bool Integrate[2] = { true, true };
static float NewSum[2] = { 0, 0 };
static uint8_t PWMDuty[2];
static uint8_t Direction[2];
static uint32_t CountLimit;
static bool limit = false;
static bool NewEdge[2] = { false, false };
static bool Driving[2] = { false, false };

static uint32_t ClockSpeed;

static const uint32_t OneShotTimeout = 1000 * TICKS_MS;

static const uint32_t CONTROL_INTERVAL = 10 * TICKS_MS;
static const uint32_t FIRST = 1;

static const float CountsPerInch = (19.5 / 2);

static const float DegreesPerInch = 10.8;

static const float kp = 1; //1

static const float ki = 0.5; //.5
static const float kd = 5; //5

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

/******************************************************************/
// handles encoder pulses from the left motor
void LeftEncoderResponse(void)
{
// clear source of the interrupt
HWREG(WTIMER0_BASE + TIMER_O_ICR) = TIMER_ICR_CAECINT;

// now grab the captured value and calculate the period

ThisEncoderEdge[0] = HWREG(WTIMER0_BASE + TIMER_O_TAR);
EncoderPeriod[0] = (ThisEncoderEdge[0] - LastEncoderEdge[0]);

// update LastEdge
LastEncoderEdge[0] = ThisEncoderEdge[0];

// Increment count
EncoderCount[0]++;

// restart oneshot
RestartLeftStopShot();
NewEdge[0] = true;
}

/******************************************************************/
// handles encoder pulses from the right motor
void RightEncoderResponse(void)
{
// clear source of the interrupt
HWREG(WTIMER0_BASE + TIMER_O_ICR) = TIMER_ICR_CBECINT;

// now grab the captured value and calculate the period

ThisEncoderEdge[1] = HWREG(WTIMER0_BASE + TIMER_O_TBR);
 EncoderPeriod[1] = (ThisEncoderEdge[1] - LastEncoderEdge[1]);

// update LastEdge
LastEncoderEdge[1] = ThisEncoderEdge[1];

// Increment count
EncoderCount[1]++;

// restart oneshot
RestartRightStopShot();
NewEdge[1] = true;
}

/******************************************************************/
// handles the left one shot interrupt
void LeftStopShotResponse(void)
{
// clear the source of the interrupt
HWREG(WTIMER3_BASE + TIMER_O_ICR) = TIMER_ICR_TATOCINT;

// set RPM to 0
ActRPM[0] = 0;
//printf("Left \r\n");
if (Driving[0])
{
Driving[0] = false;
ES_Event_t NewEvent;
NewEvent.EventType = LEFT_BUMPER_HIT;
PostMasterSM(NewEvent);
}

// restart the oneshot timer

RestartLeftStopShot();
}

/******************************************************************/
// handles the right one shot interrupt
void RightStopShotResponse(void)
{
// clear the source of the interrupt
HWREG(WTIMER3_BASE + TIMER_O_ICR) = TIMER_ICR_TBTOCINT;

// set RPM to 0
ActRPM[1] = 0;
//printf("Right \r\n");
if (Driving[1])
{
Driving[1] = false;
ES_Event_t NewEvent;
NewEvent.EventType = RIGHT_BUMPER_HIT;
PostMasterSM(NewEvent);
}

// restart the oneshot timer

RestartRightStopShot();
}

/******************************************************************/
// Initializes timer subsystem to do input capture for Both Encoders
void InitBothEncoderCapture(void)
{
// enable clock to WideTimer 0
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R0;
 // enable the clock to port C
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2;

// wait for timer modlue to initialize

while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R0) != SYSCTL_PRWTIMER_R0)
{}
;
// disable wide timer 0/A
HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
// disable wide timer 0/B
HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEN;

// set to 32bit wide mode

HWREG(WTIMER0_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;

// use full 32bit count

HWREG(WTIMER0_BASE + TIMER_O_TAILR) = 0xffffffff;
HWREG(WTIMER0_BASE + TIMER_O_TBILR) = 0xffffffff;

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

// for edge time (TACMR=1) and up-counting (TACDIR=1)
HWREG(WTIMER0_BASE + TIMER_O_TAMR) =
(HWREG(WTIMER0_BASE + TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) |
(TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
// set up timer B in capture mode
HWREG(WTIMER0_BASE + TIMER_O_TBMR) =
(HWREG(WTIMER0_BASE + TIMER_O_TBMR) & ~TIMER_TBMR_TBAMS) |
(TIMER_TBMR_TBCDIR | TIMER_TBMR_TBCMR | TIMER_TBMR_TBMR_CAP);

// set event to rising edge 00 (clear TAEVENT bits)

HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
// set event to rising edge 00 (clear TBEVENT bits)
HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEVENT_M;

// now set up the port to do input capture for each

HWREG(GPIO_PORTC_BASE + GPIO_O_AFSEL) |= (BIT4HI | BIT5HI);

// map bit 4's alternate function to WT0CCP0

HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) =
(HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) & 0xfff0ffff) + (7 << (4 *
BitsPerNibble));
// map bit 5's alternate function to WT0CCP0
HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) =
(HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) & 0xff0fffff) + (7 << (5 *
BitsPerNibble));

// enable digital I/O

HWREG(GPIO_PORTC_BASE + GPIO_O_DEN) |= (BIT4HI | BIT5HI);

// make pin 4&5 inputs

HWREG(GPIO_PORTC_BASE + GPIO_O_DIR) |= (BIT4HI | BIT5HI);

// enable a local capture interrupt

HWREG(WTIMER0_BASE + TIMER_O_IMR) |= (TIMER_IMR_CAEIM | TIMER_IMR_CBEIM);

// enable timerA/B Wtimer0 interrupt in NVIC (NUMBER 94 & 95)

HWREG(NVIC_EN2) |= (BIT30HI | BIT31HI);

// ensure enabled globally

__enable_irq();

// kick off the timer by enabling and enable timer stall during debug TODO removed
enable TIMER_CTL_TAEN TIMER_CTL_TBEN
HWREG(WTIMER0_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
 HWREG(WTIMER0_BASE + TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}

//initialize oneshot timers for each encoder to give us a RPM of 0

void InitBothStopShot(void)
{
// enable clock to WideTimer 3
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R3;
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R3) != SYSCTL_PRWTIMER_R3)
{}
;

// disable wide timer 3/A&B

HWREG(WTIMER3_BASE + TIMER_O_CTL) &= (~TIMER_CTL_TAEN & ~TIMER_CTL_TBEN);

// set to 32bit wide mode

HWREG(WTIMER3_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;

// use full 32bit count

HWREG(WTIMER3_BASE + TIMER_O_TAILR) = 0xffffffff;
HWREG(WTIMER3_BASE + TIMER_O_TBILR) = 0xffffffff;

// set up timer A in in one shot mode

HWREG(WTIMER3_BASE + TIMER_O_TAMR) =
(HWREG(WTIMER3_BASE + TIMER_O_TAMR) & ~TIMER_TAMR_TAMR_M) |
TIMER_TAMR_TAMR_1_SHOT;
// set up timer B in one shot mode
HWREG(WTIMER3_BASE + TIMER_O_TBMR) =
(HWREG(WTIMER3_BASE + TIMER_O_TBMR) & ~TIMER_TBMR_TBMR_M) |
TIMER_TBMR_TBMR_1_SHOT;

// set timout
HWREG(WTIMER3_BASE + TIMER_O_TAILR) = OneShotTimeout;
HWREG(WTIMER3_BASE + TIMER_O_TBILR) = OneShotTimeout;

// enable a local timeout interrupt

HWREG(WTIMER3_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM;
HWREG(WTIMER3_BASE + TIMER_O_IMR) |= TIMER_IMR_TBTOIM;

// enable timerb Wtimer3 A/B interrupt in NVIC (NUMBERS 100 &101)

HWREG(NVIC_EN3) |= (BIT4HI | BIT5HI);

//set lower priority (4*25+0) interrupt A in group 25

HWREG(NVIC_PRI25) = (HWREG(NVIC_PRI25) & ~NVIC_PRI25_INTA_M) + (BIT1HI <<
PRI_A_SHIFT);
//set lower priority (4*25+1) interrupt B in group 25
HWREG(NVIC_PRI25) = (HWREG(NVIC_PRI25) & ~NVIC_PRI25_INTB_M) + (BIT1HI <<
PRI_B_SHIFT);

// ensure enabled globally

__enable_irq();

// kick off the timer by enabling and enable timer stall during debug
HWREG(WTIMER3_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
HWREG(WTIMER3_BASE + TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}

/****************************************************************************/
//Initialize periodic timer interrupt for control law
void InitPIControl(void)
{
// get clock speed
ClockSpeed = SysCtlClockGet();
 // enable clock to WideTimer 4
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R4;
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R4) != SYSCTL_PRWTIMER_R4)
{}

// disable wide timer 4/A

HWREG(WTIMER4_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;

// set to 32bit wide mode

HWREG(WTIMER4_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;

// use full 32bit count

HWREG(WTIMER4_BASE + TIMER_O_TAILR) = 0xffffffff;

// set up timer A in periodic mode

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

// set timout
HWREG(WTIMER4_BASE + TIMER_O_TAILR) = CONTROL_INTERVAL;

// enable a local timeout interrupt

HWREG(WTIMER4_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM;

// enable widetimer 4A interrupt in NVIC (NUMBER 102)

HWREG(NVIC_EN3) |= (BIT6HI);
//set lower priority (4*25+2) interrupt C in group 25

HWREG(NVIC_PRI25) = (HWREG(NVIC_PRI25) & ~NVIC_PRI25_INTC_M) + (BIT0HI <<

PRI_C_SHIFT);

// ensure enabled globally

__enable_irq();

// kick off the timer by enabling and enable timer stall during debug
HWREG(WTIMER4_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}

/****************************************************************************/
void PIControlResponse(void)
{
// clear the source of the interrupt
HWREG(WTIMER4_BASE + TIMER_O_ICR) = TIMER_ICR_TATOCINT;

// do for both channels

for (uint8_t i = 0; i < 2; i++)
{
// send event once encoder counts reached
if (limit && (EncoderCount[i] > CountLimit))
{
ES_Event_t NewEvent;
NewEvent.EventType = ENCODER_LIMIT_REACHED;
PostMasterSM(NewEvent);
StopDrive();
break;
}

//calculate error and resulting duty

Integrate[i] = true;
if (NewEdge[i])
{
ActRPM[i] = (((float)ClockSpeed * 60) / (EncoderPeriod[i] *
PULSE_OUTPUT_ROTATION));
 if (i)
{
printf("%f\n\r", ActRPM[i]);
}
if ((ActRPM[i] > REASONABLE_RPM) || (EncoderCount[i] <= NUM_PULSES_PITCHED))
//EncoderCount[i] <= 1
{
// prevent short first period from setting ActRPM too high
ActRPM[i] = 0;
}
//if(i) printf("%f\n\r", ActRPM[i]);
}

Error[i] = SetRPM[i] - ActRPM[i];

NewSum[i] = ErrorSum[i] + Error[i];

DerivativeTerm[i] = BETA * DerivativeTerm[i] + (1 - BETA) * (Error[i] -

LastError[i]);
PWMDuty[i] = (uint8_t)(kp * (Error[i] + (ki * NewSum[i])) + kd *
DerivativeTerm[i]);

// perform anti windup if outside max/min duty

if (PWMDuty[i] > MAX_DUTY)
{
PWMDuty[i] = MAX_DUTY;
if (Error[i] > 0)
{
Integrate[i] = false;
}
}
else if (PWMDuty[i] < 0)
{
PWMDuty[i] = 0;
if (Error[i] < 0)
{
Integrate[i] = false;
}
}

// only integrate error if not driving outside range

if (Integrate[i])
{
ErrorSum[i] = NewSum[i];
}

// Set New Duty for both motors

if (!i)
{
SetLeftDrive(PWMDuty[i], Direction[i]);
}
else
{
SetRightDrive(PWMDuty[i], Direction[i]);
//printf("\t %u \n\r", PWMDuty[i]);
}
NewEdge[i] = false;
LastError[i] = Error[i];
}
}

/****************************************************************************/
// use to turn on Encoder Control of the Motors
void EnableEncoderControl(void)
{
 // Resets Encoder count
EncoderCount[0] = 0;
EncoderCount[1] = 0;

// Enables Encoder control

HWREG(WTIMER4_BASE + TIMER_O_CTL) |= TIMER_CTL_TAEN;
}

/****************************************************************************/
// use to turn off Encoder Control of the Motors
void DisableEncoderControl(void)
{
// disables Encoder control
HWREG(WTIMER4_BASE + TIMER_O_CTL) &= (~TIMER_CTL_TAEN);
}

/****************************************************************************/
// distance is in inches
void SetDistanceLimit(uint16_t Distance)
{
//turn on distance control
limit = true;

//convert the distance to encoder counts

CountLimit = (uint32_t)CountsPerInch * (float)Distance;
}

/****************************************************************************/
// angle is in degrees
void SetAngleLimit(uint16_t Angle)
{
//turn on distance control
limit = true;

//convert the angle to encoder counts

CountLimit = (uint32_t)CountsPerInch * ((float)Angle / DegreesPerInch);
}

/****************************************************************************/
// no limit on the number of encoder counts
void DisableLimit(void)
{
//turn off distance control
limit = false;
}

/****************************************************************************/
void SetLeftRPM(uint8_t RPM, uint8_t Dir)
{
Direction[0] = Dir;
if (RPM)
{
Driving[0] = true;
}
else
{
Driving[0] = false;
}
SetRPM[0] = RPM;
}

/****************************************************************************/
void SetRightRPM(uint8_t RPM, uint8_t Dir)
{
 Direction[1] = Dir;
if (RPM)
{
Driving[1] = true;
}
else
{
Driving[1] = false;
}

SetRPM[1] = RPM;
}

// turn on and off stall control

void SetDriving(bool driving)
{
Driving[0] = driving;
Driving[1] = driving;
}

/***************************************************************************
private functions
***************************************************************************/
/******************************************************************/
//restart the Left One Shot timer
static void RestartLeftStopShot(void)
{
// disable wide timer 3/A
HWREG(WTIMER3_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;

// reset OneShot duration

HWREG(WTIMER3_BASE + TIMER_O_TAILR) = OneShotTimeout;

// kick off the timer by enabling and enable timer stall during debug
HWREG(WTIMER3_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN);
}

/******************************************************************/
// restart the Right One Shot timer
static void RestartRightStopShot(void)
{
// disable wide timer 3/B
HWREG(WTIMER3_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TBEN;

// reset OneShot duration

HWREG(WTIMER3_BASE + TIMER_O_TBILR) = OneShotTimeout;

// kick off the timer by enabling and enable timer stall during debug
HWREG(WTIMER3_BASE + TIMER_O_CTL) |= (TIMER_CTL_TBEN);
}

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

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