1 /****************************************************************************
 2  Module
 3    ServoService.c
 4  ****************************************************************************/
 5 /*----------------------------- Include Files -----------------------------*/
 6 
 7 #include "ES_Configure.h"
 8 #include "ES_Framework.h"
 9 #include "ES_DeferRecall.h"
10 #include "ServoService.h"
11 #include "sys/attribs.h"
12 #include "PIC32_Port_HAL.h"
13 #include "EyeColor.h"
14 #include "PWM_PIC32.h"
15 
16 /*----------------------------- Module Defines ----------------------------*/
17 #define TICS_PER_MS 2500
18 
19 #define FULL_CW ((uint16_t)(0.5*TICS_PER_MS))
20 #define FULL_CCW ((uint16_t)(2.25*TICS_PER_MS))
21 #define MID_POINT (FULL_CW+((FULL_CCW-FULL_CW)/2))
22 #define EYEBROW_SCALAR 0.3
23 
24 
25 /*---------------------------- Module Functions ---------------------------*/
26 
27 /*---------------------------- Module Variables ---------------------------*/
28 static uint8_t MyPriority;
29 
30 static ES_Event_t DeferralQueue[5 + 1];
31 
32 static int ServoStatus[4] = {0, 1, 0, 0}; // 0 = CW, 1 = CCW {L, R, X, D}
33 /*------------------------------ Module Code ------------------------------*/
34 
35 bool InitServoService(uint8_t Priority) {
36     ES_Event_t ThisEvent;
37     
38     MyPriority = Priority;
39     ES_InitDeferralQueueWith(DeferralQueue, ARRAY_SIZE(DeferralQueue));
40     
41     printf("Initializing Servo Service...\r\n");
42     
43     PWMSetup_BasicConfig(4);
44     PWMSetup_SetFreqOnTimer(50, _Timer2_);
45     PWMSetup_AssignChannelToTimer(SERVO_BROW_L, _Timer2_);
46     PWMSetup_AssignChannelToTimer(SERVO_BROW_R, _Timer2_);
47     PWMSetup_AssignChannelToTimer(SERVO_DUNK, _Timer2_);
48     PWMSetup_MapChannelToOutputPin(SERVO_BROW_L, PWM_RPB3); // OC1, 7
49     PWMSetup_MapChannelToOutputPin(SERVO_BROW_R, PWM_RPB11); // OC2, 22
50     PWMSetup_MapChannelToOutputPin(SERVO_DUNK, PWM_RPB2); // OC4, 6
51     PWMOperate_SetDutyOnChannel(50, SERVO_BROW_L);
52     PWMOperate_SetDutyOnChannel(50, SERVO_BROW_R);
53     PWMOperate_SetDutyOnChannel(50, SERVO_DUNK);
54     PWMOperate_SetPulseWidthOnChannel(FULL_CW, SERVO_BROW_L);
55     PWMOperate_SetPulseWidthOnChannel(FULL_CCW, SERVO_BROW_R);
56     PWMOperate_SetPulseWidthOnChannel(FULL_CW, SERVO_DUNK);
57 
58     ThisEvent.EventType = ES_INIT;
59     if (ES_PostToService(MyPriority, ThisEvent) == true) {
60         return true;
61     } else {
62         return false;
63     }
64 }
65 
66 bool PostServoService(ES_Event_t ThisEvent) {
67     return ES_PostToService(MyPriority, ThisEvent);
68 }
69 
70 ES_Event_t RunServoService(ES_Event_t ThisEvent) {
71     ES_Event_t ReturnEvent;
72     ReturnEvent.EventType = ES_NO_EVENT;
73     
74     if (ThisEvent.EventType == HW_TOGGLE_SERVO) {
75         uint16_t ServoIndex = ThisEvent.EventParam;
76         if (ServoStatus[ServoIndex - 1]) {
77             if(ServoIndex == SERVO_DUNK){
78                 PWMOperate_SetPulseWidthOnChannel(FULL_CW, ServoIndex);
79             }else{
80                 PWMOperate_SetPulseWidthOnChannel(FULL_CW * EYEBROW_SCALAR, ServoIndex);
81             }
82         } else {
83             if(ServoIndex == SERVO_DUNK){
84                 PWMOperate_SetPulseWidthOnChannel(FULL_CCW, ServoIndex);
85             }else{
86                 PWMOperate_SetPulseWidthOnChannel(FULL_CCW * EYEBROW_SCALAR, ServoIndex);
87             }
88             
89         }
90         ServoStatus[ServoIndex - 1] = !ServoStatus[ServoIndex - 1];
91     }
92     return ReturnEvent;
93 }
94 
95 /***************************************************************************
96  private functions
97  ***************************************************************************/
98 
99 /*------------------------------ End of file ------------------------------*/