/****************************************************************************
Module
ServoService.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
*/
//This Module
#include "Servo.h"
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "PIC32_AD_Lib.h"
#include "dbprintf.h"
#include "ES_Port.h"
#include "terminal.h"
#include "PWM_PIC32.h"
#include "MasterService.h"
/*----------------------------- Module Defines ----------------------------*/
#define ONE_SEC 1000
#define SERVO_UPDATE_FREQUENCY 10
#define SERVO_UPDATE_SEC (ONE_SEC /SERVO_UPDATE_FREQUENCY)
#define MAX_PWM_VALUE 6350
#define MIN_PWM_VALUE 1350
#define NEUTRAL_PWM_VALUE ((MAX_PWM_VALUE + MIN_PWM_VALUE)/2)
#define SHIP_MOVING_RANGE (MAX_PWM_VALUE - MIN_PWM_VALUE)
#define MAX_SPEED 5000
#define ROWS_LEDMATRIX 32
#define UNIT_PWM_VALUE_STEP ((MAX_PWM_VALUE - MIN_PWM_VALUE) / (ROWS_LEDMATRIX -1))
#define Tolerance 0.2
#define INERTIA 23
/*---------------------------- 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 int32_t PWMValue = NEUTRAL_PWM_VALUE;
static int32_t ship_pos = SHIP_MOVING_RANGE/2;
static int16_t last_ship_pos = SHIP_MOVING_RANGE/2;
static int16_t last_vel = 0;
static int16_t crew_pos = 0;
static int16_t T_ship = 600;
static uint8_t num_crew = 8;
static int16_t alpha_ship = 0;
static int16_t direction_results = 0;
ServoState_t CurrentServoState = Init_Servo;
ServoState_t NextServoState = Init_Servo;
/*------------------------------ 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 InitServo(uint8_t Priority)
{
Set MyPriority to Priority
Configure initial settings:
- Set ANSA0 bit to 1 (Analog mode for a specific pin)
- Set pin RB12 as input (using TRISB |= 1<<12)
- Configure PWM:
- Set up basic PWM configuration
- Assign PWM channel 1 to Timer 2
- Set PWM period on Timer 2 to 25000
- Map PWM channel 1 to output pin RPB15
Create and initialize an event ThisEvent with EventType = ES_INIT
Post ThisEvent to this service using MyPriority
If posting succeeds:
Return true
Else:
Return false
}
/****************************************************************************
Function
PostServoService
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 PostServo(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunServoService
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 RunServo(ES_Event_t ThisEvent)
{
Initialize ReturnEvent as ES_NO_EVENT
Initialize GameResultEvent
Switch on CurrentServoState:
Case Init_Servo:
If ThisEvent is GAME_START:
Initialize ship parameters ( position, velocity, crew position)
Set PWM to neutral
Start SHIP_MOVE timer
Transition to Running_Servo state
Case Running_Servo:
If ThisEvent is NEW_CREW_POSITION:
Update crew position based on input
If ThisEvent is ES_TIMEOUT AND Timer is SHIP_MOVE:
Update ship velocity and position using physics formulas
Clamp ship position within valid range
Update PWM value if within bounds
Restart SHIP_MOVE timer
If ship position exceeds win/lose thresholds:
Post LOSE event
Transition to Completion_Lose state
If ThisEvent is WIN:
Transition to Completion_Win state
If ThisEvent is USER_LEFT:
Transition to Completion_User_Left state
Case Completion_Win:
If ThisEvent is ES_TIMEOUT AND Timer is SHIP_MOVE:
Animate win effect by oscillating ship position within tolerance
Update PWM value
Restart SHIP_MOVE timer
If ThisEvent is GAMERESULT_OVER:
Reset PWM to neutral
Transition to Init_Servo state
Case Completion_Lose:
If ThisEvent is ES_TIMEOUT AND Timer is SHIP_MOVE:
Animate lose effect by bouncing ship position
Update PWM value
Restart SHIP_MOVE timer with reduced interval
If ThisEvent is GAMERESULT_OVER:
Reset PWM to neutral
Transition to Init_Servo state
Case Completion_User_Left:
If ThisEvent is ES_TIMEOUT AND Timer is SHIP_MOVE:
Reset PWM to neutral
Transition to Init_Servo state
If ThisEvent is GAMERESULT_OVER:
Reset PWM to neutral
Transition to Init_Servo state
Default: No action
Update CurrentServoState to NextServoState
Return ReturnEvent
}
/***************************************************************************
private functions
***************************************************************************/
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/