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

Module
LineFollowingSM.c

Description
This module contains the line following functions.

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

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

#include "MotorDrive.h"
#include "ADMulti.h"
#include "LineFollowingSM.h"
#include "MasterSM.h"
#include "math.h"
#include "MotorEncoders.h"

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

#define TicksPerMS 40000
#define NOMINAL_DC 43 //60

#define LINE_ON_THRESHOLD 3500 //2.4

#define LINE_ALIGNED_THRESHOLD 3000 //2.4 // line sensing threshold [V]
#define LINE_ALIGNED_DIFF_THRESHOLD 100 //line sensing diff threshold [ADC ticks]
#define COUNTS_PER_VOLT 1241.0

#define SAMPLE_TIME 10
#define DUTY_ADD_TOL 5

// Motor rotation direction looking from wheel side

#define CW 1
#define CCW 0

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

void InitLineFollowingIntResponse(void);

static void EnableLineFollowing(void);

static void DisableLineFollowing(void);

static ES_Event_t DuringFollowingTheLine(ES_Event_t Event);

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

static LineFollowingState_t CurrentState;
// Target magnetic field detection difference (TargetDelta) is 0.0
static float TargetDelta = 0.0;
static float LastDelta;
static float Delta;
// PI gains
//static const float iGain = 0.0;
//static const float pGain = 0.07;
//static const float dGain = .7;
//// term for moving average
//static const float BETA = 0.7;

// PI gains
static const float iGain = 0.0;
static const float pGain = 0.08;
static const float dGain = 0.5;
// term for moving average
static const float BETA = 0.7;

// PI terms
static float IntegralTerm = 0.0;
static float DerivativeTerm = 0.0;
static float DutyAdd = 0.0;
// nominal Duty Cycle of the motors at 30% for line following
static float LMotorDC = NOMINAL_DC;
static float RMotorDC = NOMINAL_DC;

// Event Checker variables

static uint32_t CurrentSensorReading_Left;
static uint32_t CurrentSensorReading_Right;
static uint32_t LastSensorReading_Left = 0;
static uint32_t LastSensorReading_Right = 0;
static uint32_t CurrentSensorReading_Diff;
static bool LCR_CONTROL = false;

static bool AllowLineOn = true;

static bool AllowLineAlign = false;

// Get AD readings of the sensors

static uint32_t LineSensors[2];

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

/****************************************************************************
Function
InitLineFollowingSM

Parameters
uint8_t: the priorty of this service

Returns
boolean: True if no error, False otherwise

Description
Saves away the priority, and starts
the top level state machine. Hardware init for LineFollowingSM
Notes

Author
S. Park
****************************************************************************/
void InitLineFollowingSM(void)
{
//Initialize the line following control
InitLineFollowingIntResponse();
 // Initialize the AD ports (PE0 and PE1)
ADC_MultiInit(2);
}

/****************************************************************************
Function
RunLineFollowingSM

Parameters
ES_Event_t ThisEvent, event to process

Returns
ES_Event_t ThisEvent, event to return
Description
Posts an event to the queue of the SM
Notes

Author
S. Park
****************************************************************************/
ES_Event_t RunLineFollowingSM(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition?
*/
LineFollowingState_t NextState = CurrentState;
ES_Event_t EntryEventKind = { ES_ENTRY, 0 }; // default to normal entry
to new state
ES_Event_t ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error

switch (CurrentState)
{
case FollowingTheLine: // If current state is TurningLeft
{ // Execute During function for TurningLeft
ReturnEvent = CurrentEvent = DuringFollowingTheLine(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
//If both bumpers are hit
if ((CurrentEvent.EventType == LEFT_BUMPER_HIT) || (CurrentEvent.EventType
== RIGHT_BUMPER_HIT))
{
// Reload station reached, stop motors
StopDrive();
// Transition to Line Following Complete
ReturnEvent.EventType = LINE_FOLLOW_COMPLETE;
}
}
}
break;
}

// If we are making a state transition

if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunLineFollowingSM(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
RunLineFollowingSM(EntryEventKind);
 }

// in the absence of an error the top level state machine should

// always return ES_NO_EVENT, which we initialized at the top of func
return ReturnEvent;
}

/****************************************************************************
Function
StartLineFollowingSM

Parameters
ES_Event_t CurrentEvent

Returns
nothing

Description
Does any required initialization for this state machine
Notes

Author
S. Park
****************************************************************************/
void StartLineFollowingSM(ES_Event_t CurrentEvent)
{
// Start with motors stopped
StopDrive();
EnableLineFollowing();

//Initialize to Waiting2LineFollow
CurrentState = FollowingTheLine;

// Run the line following service

RunLineFollowingSM(CurrentEvent);

return;
}

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

static ES_Event_t DuringFollowingTheLine(ES_Event_t Event)

{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// Drive Forward using Line Following control
EnableLineFollowing();
}
else if (Event.EventType == ES_EXIT)
{
// Stop Line Following control
DisableLineFollowing();
}
else
{
// No standard during actions
}
 // return Event without remapping
return ReturnEvent;
}

static void EnableLineFollowing(void)

{
//Reset line following control variables
LastDelta = 0.0;
IntegralTerm = 0.0;
DutyAdd = 0.0;
LMotorDC = NOMINAL_DC;
RMotorDC = NOMINAL_DC;

//Enable timer interrupt and encoder timeouts

SetDriving(true);
LCR_CONTROL = true;
}

static void DisableLineFollowing(void)

{
//Disable timer interrupt
//HWREG(NVIC_DIS3) = BIT2HI;
LCR_CONTROL = false;
SetDriving(false);
}

bool Check4LineOn(void)
{
// ES_Event_t LineEvent;
// // Get AD readings of the sensors
//
// CurrentSensorReading_Left = LineSensors[0];
// CurrentSensorReading_Right = LineSensors[1];

// printf("%u\t%u\n\r",LineSensors[0],LineSensors[1]);
// if ((CurrentSensorReading_Left != LastSensorReading_Left)
// || (CurrentSensorReading_Right != LastSensorReading_Right))
// {
// if (AllowLineOn)
// {
// // check if either sensor is above threshold
// if ((CurrentSensorReading_Left > LINE_ON_THRESHOLD*COUNTS_PER_VOLT)
// || (CurrentSensorReading_Right > LINE_ON_THRESHOLD*COUNTS_PER_VOLT))
// {
// AllowLineOn = false;
// printf("found line \r\n");
// LineEvent.EventType = LINE_ON;
// PostMasterSM(LineEvent);
// return true;
// }
// }
// }
//
//
// LastSensorReading_Left = CurrentSensorReading_Left;
// LastSensorReading_Right = CurrentSensorReading_Right;

return false;
}

bool Check4LineAlign(void)
{
// ES_Event_t LineEvent;
// // Get AD readings of the sensors
//
// //TODO: UNCOMMENT IF WE WANT
//
// CurrentSensorReading_Left = LineSensors[0];
// CurrentSensorReading_Right = LineSensors[1];
// CurrentSensorReading_Diff = abs(CurrentSensorReading_Left -
CurrentSensorReading_Right);
// //printf("%u\t%u\t%u\r\n",CurrentSensorReading_Left,
CurrentSensorReading_Right, CurrentSensorReading_Diff);
//
// if (AllowLineAlign)
// {
// if ((CurrentSensorReading_Left != LastSensorReading_Left)
// || (CurrentSensorReading_Right != LastSensorReading_Right))
// {
// // took out && (CurrentSensorReading_Diff<LINE_ALIGNED_DIFF_THRESHOLD)
// // check if either sensor is above 1.9V and values are equal
// if (
// ((CurrentSensorReading_Left > LINE_ALIGNED_THRESHOLD*COUNTS_PER_VOLT)
// || (CurrentSensorReading_Right >
LINE_ALIGNED_THRESHOLD*COUNTS_PER_VOLT))
// )
// {
// AllowLineAlign = false;
// printf("LINE_ALIGNED\r\n");
// LineEvent.EventType = LINE_ALIGNED;
// PostMasterSM(LineEvent);
// return true;
// }
// }
// }

// LastSensorReading_Left = CurrentSensorReading_Left;
// LastSensorReading_Right = CurrentSensorReading_Right;

return false;
}

void InitLineFollowingIntResponse(void)
{
// enable clock to the timer (Wide Timer 2)
HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R2;
// kill a few cycles
while ((HWREG(SYSCTL_PRWTIMER) & SYSCTL_PRWTIMER_R2) != SYSCTL_PRWTIMER_R2)
{}

// disable Timer A before configuring

HWREG(WTIMER2_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;

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

HWREG(WTIMER2_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;

// set up timer A in periodic mode

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

// set timeout to 2ms

HWREG(WTIMER2_BASE + TIMER_O_TAILR) = TicksPerMS * SAMPLE_TIME;

// enable a local timeout interrupt

HWREG(WTIMER2_BASE + TIMER_O_IMR) |= TIMER_IMR_TATOIM;

// enable the Timer B in Wide Timer 2 interrupt in the NVIC

 //HWREG(NVIC_EN3) = BIT2HI;
//HWREG(NVIC_PRI24) |= BIT22HI;
HWREG(NVIC_EN3) |= BIT2HI;

// make sure interrupts are enabled globally

__enable_irq();

//enable timer and to stall while stopped by debugger

HWREG(WTIMER2_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
}

float clamp(float val, float clampL, float clampH)

{
if (val > clampH)
{
return clampH;
}
else if (val < clampL)
{
return clampL;
}
return val;
}

void LineFollowingIntResponse(void)
{
ES_Event_t LineEvent;
ADC_MultiRead(LineSensors);

// clear the source of the interrupt

HWREG(WTIMER2_BASE + TIMER_O_ICR) = TIMER_ICR_TATOCINT;

if (AllowLineOn)
{
// check if either sensor is above threshold
if ((LineSensors[0] > LINE_ON_THRESHOLD)
|| (LineSensors[1] > LINE_ON_THRESHOLD))
{
AllowLineOn = false;
//printf("found line \r\n");
LineEvent.EventType = LINE_ON;
PostMasterSM(LineEvent);
}
}
if (AllowLineAlign)
{
if ((LineSensors[0] > LINE_ALIGNED_THRESHOLD))
{
AllowLineAlign = false;
//printf("found line \r\n");
LineEvent.EventType = LINE_ALIGNED;
PostMasterSM(LineEvent);
}
}

if (LCR_CONTROL)
{
//printf("following line \r\n");
// calculate the sensor reading difference (Right - Left)
Delta = ((float)LineSensors[1]) - (float)(LineSensors[0]);
//printf("%d\t%d\t%f\n\r",LineSensors[0],LineSensors[1],Delta);
//printf("%d\n\r", LineSensors[0]);
// calculate the integral term & clamp between 0 and nominal DC
IntegralTerm += iGain * Delta;
 //caculate with moving average (filters the value)
DerivativeTerm = BETA * DerivativeTerm + (1 - BETA) * (Delta - LastDelta);
IntegralTerm = clamp(IntegralTerm, -50, 50);

// calculate the control effort & clamp between 0 and nominal DC

DutyAdd = pGain * Delta + IntegralTerm + dGain * DerivativeTerm;
DutyAdd = clamp(DutyAdd, -50, 50);
//printf("%f\r\n",DutyAdd);
//printf("%f\t%f\t", Delta,IntegralTerm);

// set new duty cycle to slow down motor

// Right side is closer, slow down right wheel
if (DutyAdd >= 0.0)
{
RMotorDC = NOMINAL_DC;
LMotorDC = NOMINAL_DC + DutyAdd;
SetRightDrive((int)RMotorDC, CW);
SetLeftDrive((int)LMotorDC, CCW);
}
// Left side is closer, slow down left wheel
else if (DutyAdd < 0.0)
{
RMotorDC = NOMINAL_DC - DutyAdd;
LMotorDC = NOMINAL_DC;
SetRightDrive((int)RMotorDC, CW);
SetLeftDrive((int)LMotorDC, CCW);
}
// update Delta
LastDelta = Delta;
}
}

//function to enable LineChecking

void EnableLineOn(void)
{
AllowLineOn = true;
}

//fucntion to enable LineAlign

void EnableLineAlign(void)
{
AllowLineAlign = true;
}