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

Module
TemplateService.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 What/Why
-------------- --- --------
01/16/12 09:58 jec began conversion from TemplateFSM.c
****************************************************************************/
/*----------------------------- 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 "MotorService.h"
#include "dbprintf.h"

#include "PIC32PortHAL.h"
#include "PWM_PIC32.h"
#include "PIC32_AD_Lib.h"

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

// TICS_PER_MS assumes a 20MHz PBClk /8 = 2.5MHz clock rate
#define TICS_PER_MS 2500

// these are the initial extents of servo motion

#define FULL_CW ((uint16_t)(0.7*TICS_PER_MS))
#define FULL_CCW ((uint16_t)(2.25*TICS_PER_MS))
#define MID_POINT (cwLimit+((ccwLimit-cwLimit)/2))

typedef enum
{
Waiting2Begin,
Steering
} ES_MotorState_t;
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/

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

// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
static uint8_t CurrentState;
// track where we are in number of PWM ticks
static uint16_t CurrentPosition;
//array for reading analog to digital output
static uint32_t ConversionResults[2];
//limits for servo
static uint16_t cwLimit = FULL_CW;
static uint16_t ccwLimit = FULL_CCW;
//variable for analog to digital for pot
static uint32_t ConversionResults[2];
//factor to convert pot values to PWM ticks
const float factor = (0.7*2500 - 2.25*2500)/1023;
//last position of the pot
static uint16_t LastPos;
static uint16_t LastPostedPos;

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

/****************************************************************************
Function
InitTemplateService

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, and does any
other required initialization for this service
Notes

Author
J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitMotorService(uint8_t Priority)
{
ES_Event_t ThisEvent;
bool ReturnVal = true;
MyPriority = Priority;
/********************************************
in here you write your initialization code
*******************************************/
/********************************************
Initialization sequence for timers to do servo drive
*******************************************/
CurrentState = Waiting2Begin;

if(!PWMSetup_BasicConfig(1))
{
ReturnVal = false;
}else if(!PWMSetup_SetFreqOnTimer(50, _Timer3_)) // set freq to 50Hz
{
ReturnVal = false;
}else if(!PWMSetup_AssignChannelToTimer(1, _Timer3_))
{
ReturnVal = false;
}else if(!PWMOperate_SetPulseWidthOnChannel(MID_POINT,1))
{
ReturnVal = false;
}else if(!PWMSetup_MapChannelToOutputPin(1, PWM_RPB3))
{
ReturnVal = false;
}else
{
 // no else clause
}
CurrentPosition = MID_POINT;

// post the initial transition event

ThisEvent.EventType = ES_INIT;
if (ES_PostToService(MyPriority, ThisEvent) == true)
{
//no clause
}
else
{
ReturnVal = false;
}
return ReturnVal;
}

/****************************************************************************
Function
PostTemplateService

Parameters
EF_Event_t ThisEvent ,the event to post to the queue

Returns
bool false if the Enqueue operation failed, true otherwise

Description
Posts an event to this state machine's queue
Notes

Author
J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
bool PostMotorService(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
Function
RunTemplateService

Parameters
ES_Event_t : the event to process

Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

Description
add your description here
Notes

Author
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunMotorService(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
 ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

ES_MotorState_t NextState; //variable to keep track of next state

NextState = CurrentState;
/********************************************
in here you write your service code
*******************************************/
//state machine
if (CurrentState == Waiting2Begin)
{
if (ThisEvent.EventType == ES_INIT)
{
//get initial position of pot
ADC_MultiRead(ConversionResults);
LastPos = ConversionResults[0];
LastPostedPos = LastPos;
NextState = Steering;
}
}
if (CurrentState == Steering)
{
if (ThisEvent.EventType == ES_WHEEL_TURN)
{
uint16_t ServoVal = ccwLimit + factor*ThisEvent.EventParam;
CurrentPosition = ServoVal;
PWMOperate_SetPulseWidthOnChannel(ServoVal,1);

}
}
CurrentState = NextState;
return ReturnEvent;
}

bool CheckWheelTurn(void)
{
bool ReturnVal = false; //initialize local variables
uint16_t CurrentPos;

ADC_MultiRead(ConversionResults);
CurrentPos = ConversionResults[0]; //read the current state of pot
if (CurrentPos != LastPos) //if the state has changed
{
//printf("\r%d, %d\n", CurrentPos, LastPos);
ES_Event_t ThisEvent; //post falling edge event
ThisEvent.EventType = ES_WHEEL_TURN;
ThisEvent.EventParam = CurrentPos;
ES_PostToService(MyPriority, ThisEvent);
ReturnVal = true;
}
uint16_t delta = abs(CurrentPos - LastPostedPos);

if (delta > 100)

{
printf("%d\r\n", delta);
LastPostedPos = CurrentPos;
if (CurrentState == Steering)
{
//Reset Activity Timer
ES_Timer_InitTimer(ACTIVITY_TIMER,30000);
 }
}
LastPos = CurrentPos;

// if (delta > 60)

// {
// printf("Delta: %d\r\n", delta);
// printf("LastPos: %d\r\n", LastPos);
// printf("CurrentPos: %d\r\n", CurrentPos);
// ES_Event_t ThisEvent; //post falling edge event
// ThisEvent.EventType = ES_WHEEL_TURN;
// ThisEvent.EventParam = CurrentPos;
// ES_PostToService(MyPriority, ThisEvent);
// ReturnVal = true;
// LastPos = CurrentPos;
// }

return ReturnVal;
}
/***************************************************************************
private functions
***************************************************************************/

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

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