#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ObstacleService.h"
#include "PWM_PIC32.h"
#include "PIC32_AD_Lib.h"
#include "dbprintf.h"
/*----------------------------- Module Defines ----------------------------*/
#define OBSTACLE_MAX_TICK 6250
#define OBSTACLE_MIN_TICK 3800
#define OBSTACLE_NEUTRAL_PULSE 3750
#define OBSTACLE_FAST_TICK 4600
#define OBSTACLE_SLOW_TICK 4200
#define OBSTACLE_SERVO_CH 3
#define POT_MAX 1023
/*---------------------------- 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 ObstacleState_t CurrentState;
static uint32_t adcResults[3];
// global variables
uint16_t currentObstacleTick = 0;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitTemplateFSM
Parameters
uint8_t : the priorty of this service
Returns
bool, false if error in initialization, true otherwise
Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes
Author
J. Edward Carryer, 10/23/11, 18:55
****************************************************************************/
bool InitObstacleService(uint8_t Priority)
{
ES_Event_t ThisEvent;
MyPriority = Priority;
// put us into the Initial PseudoState
CurrentState = OBSTACLE_INIT;
// post the initial transition event
ThisEvent.EventType = ES_INIT;
if (ES_PostToService(MyPriority, ThisEvent) == true)
{
return true;
}
else
{
return false;
}
}
/****************************************************************************
Function
PostTemplateFSM
Parameters
EF_Event_t ThisEvent , the event to post to the queue
Returns
boolean 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 PostObstacleService(ES_Event_t ThisEvent)
{
// DB_printf("posting to obstacle. CurrentState: %d. EventType: %d\n", CurrentState, ThisEvent.EventType);
return ES_PostToService(MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunTemplateFSM
Parameters
ES_Event_t : the event to process
Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise
Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunObstacleService(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (CurrentState)
{
case OBSTACLE_INIT:
{
if (ThisEvent.EventType == ES_INIT) // only respond to ES_Init
{
CurrentState = OBSTACLE_IDLE;
DB_printf("Obstacle initialized.\n");
}
}
break;
case OBSTACLE_IDLE:
{
switch (ThisEvent.EventType)
{
case ES_CALIB_DONE:
{
CurrentState = OBSTACLE_PLAYING;
ADC_MultiRead(adcResults);
int sample = (int)adcResults[1];
DB_printf("pot value: %d\n", sample);
uint16_t tick;
if (sample < 500) // turning CW is faster
{
tick = OBSTACLE_FAST_TICK;
currentObstacleTick = OBSTACLE_FAST_TICK;
}
else
{
tick = OBSTACLE_SLOW_TICK;
currentObstacleTick = OBSTACLE_SLOW_TICK;
}
DB_printf("speed tick: %d\n", tick);
PWMOperate_SetPulseWidthOnChannel(tick, OBSTACLE_SERVO_CH);
}
break;
// repeat cases as required for relevant events
default:
;
} // end switch on CurrentEvent
}
break;
case OBSTACLE_PLAYING:
{
switch (ThisEvent.EventType)
{
case ES_GAMEOVER_TIMEOUT:
case ES_NO_INPUT_TIMEOUT:
{
DB_printf("[Obstacle Service] Responding to TIMEOUT\n");
CurrentState = OBSTACLE_IDLE;
currentObstacleTick = OBSTACLE_NEUTRAL_PULSE;
PWMOperate_SetPulseWidthOnChannel(OBSTACLE_NEUTRAL_PULSE, OBSTACLE_SERVO_CH);
}
break;
default:
;
} // end switch on CurrentEvent
}
break;
// repeat state pattern as required for other states
default:
;
} // end switch on Current State
return ReturnEvent;
}
/***************************************************************************
private functions
***************************************************************************/
uint16_t PotToTick(uint16_t pot)
{
// 0 is fastest speed, 1023 is lowest speed
uint16_t span = OBSTACLE_MAX_TICK - OBSTACLE_MIN_TICK;
uint16_t scaledPot = (uint16_t)((uint32_t)pot * (uint32_t)span / (uint32_t)POT_MAX);
uint16_t tick = OBSTACLE_MIN_TICK + scaledPot;
return tick;
}