1 /****************************************************************************
 2  Module
 3    UltrasonicService.c
 4 /*----------------------------- Include Files -----------------------------*/
 5 // This module
 6 #include "UltrasonicService.h"
 7 #include "MotorSM.h"
 8 
 9 // debugging printf()
10 //#include "dbprintf.h"
11 
12 #include "ES_Configure.h"
13 #include "ES_Framework.h"
14 #include "ES_DeferRecall.h"
15 #include "sys/attribs.h"
16 #include "PIC32PortHAL.h"
17 #include "MotorSM.h"
18 #include "FollowerService.h"
19 
20 /*----------------------------- Module Defines ----------------------------*/
21 #define ONE_SEC 1000
22 #define HALF_SEC 500
23 #define THIRD_SEC 300
24 #define MILLI_SEC 1
25 
26 #define CLK_HZ 20000000
27 #define PRESCALE 64
28 
29 #define CM_FACTOR 58
30 #define IN_FACTOR 148
31 #define US_IN_S 1000000
32 
33 // US hysteresis constants
34 #define NEAR_SENSOR_THRESHOLD 25 // in cm 
35 #define FAR_SENSOR_THRESHOLD 35 // in cm 
36 
37 #define ENTER_POST     ((MyPriority<<3)|0)
38 #define ENTER_RUN      ((MyPriority<<3)|1)
39 #define ENTER_TIMEOUT  ((MyPriority<<3)|2)
40 
41 #define TEST_INT_POST
42 /*---------------------------- Module Functions ---------------------------*/
43 static void InitUltrasonic(void);
44 static void InitTMR2(void);
45 static void InitIC3(void);
46 static void InitIC4(void);
47 /*---------------------------- Module Variables ---------------------------*/
48 // with the introduction of Gen2, we need a module level Priority variable
49 static uint8_t MyPriority;
50 // add a deferral queue for up to 3 pending deferrals +1 to allow for overhead
51 static ES_Event_t DeferralQueue[3 + 1];
52 
53 static uint32_t LastFrontTime, LastRearTime, FrontPulseWidth, RearPulseWidth;
54 static bool IsFrontRisingEdge, IsRearRisingEdge, UltrasonicReady;
55 static bool HasSentFrontNearEvent, HasSentRearNearEvent, HasSentFrontFarEvent, HasSentRearFarEvent;
56 
57 volatile counter FrontCounter;
58 volatile counter RearCounter;
59 /*------------------------------ Module Code ------------------------------*/
60 bool InitUltrasonicService(uint8_t Priority) {
61     ES_Event_t ThisEvent;
62 
63     MyPriority = Priority;
64 
65     IsFrontRisingEdge = true;
66     IsRearRisingEdge = true;
67     
68     HasSentFrontNearEvent = true;
69     HasSentRearNearEvent = true;
70     HasSentFrontFarEvent = true;
71     HasSentRearFarEvent = true;
72     
73     InitUltrasonic();
74     InitIC3();
75     InitIC4();
76 
77     // post the initial transition event
78     ThisEvent.EventType = ES_INIT;
79     if (ES_PostToService(MyPriority, ThisEvent) == true) {
80         return true;
81     } else {
82         return false;
83     }
84 }
85 
86 bool PostUltrasonicService(ES_Event_t ThisEvent) {
87     return ES_PostToService(MyPriority, ThisEvent);
88 }
89 
90 ES_Event_t RunUltrasonicService(ES_Event_t ThisEvent) {
91     ES_Event_t ReturnEvent;
92     ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
93 
94     switch (ThisEvent.EventType) {
95         case ES_INIT:
96         {
97             ES_Timer_InitTimer(ULTRASONIC_TIMER, THIRD_SEC);
98         }
99         break;
100         case ES_TIMEOUT: // re-start timer & announce
101         {
102             UltrasonicReady = true;
103             LATBCLR = 1 << 9;
104             LATBCLR = 1 << 4;
105             
106             ES_Timer_InitTimer(ULTRASONIC_TIMER, THIRD_SEC);
107         }
108         break;
109 //        case EV_NEW_KEY: // announce
110 //        {
111 //            printf("EV_NEW_KEY received with -> %c <- in Service 0\r\n",
112 //                    (char) ThisEvent.EventParam);
113 //            LATBCLR = 1 << 9;
114 //        }
115 //            break;
116         case EV_ECHO:
117         {
118             float PulseWidth = (float) ThisEvent.EventParam * PRESCALE * US_IN_S / CLK_HZ / CM_FACTOR;
119             if(PulseWidth < NEAR_SENSOR_THRESHOLD && !HasSentFrontNearEvent){
120                 ES_Event_t FrontSensorNearEvent = {HW_FRONTSENSORNEAR, PulseWidth};
121                 printf("Sending front sensor near event to motor with pulse width: %f\r\n", PulseWidth);
122                 HasSentFrontNearEvent = true;
123                 PostMotor(FrontSensorNearEvent);
124             } else if (PulseWidth > FAR_SENSOR_THRESHOLD && !HasSentFrontFarEvent) {
125                 ES_Event_t FrontSensorFarEvent = {HW_FRONTSENSORFAR, PulseWidth};
126                 printf("Sending front sensor far event to motor with pulse width: %f\r\n", PulseWidth);
127                 HasSentFrontFarEvent = true;
128                 PostMotor(FrontSensorFarEvent);
129             }
130         }
131         break;
132         case EV_REAR_ECHO:
133         {
134             float PulseWidth = (float) ThisEvent.EventParam * PRESCALE * US_IN_S / CLK_HZ / CM_FACTOR;
135             if(PulseWidth < NEAR_SENSOR_THRESHOLD && !HasSentRearNearEvent){
136                 ES_Event_t RearSensorNearEvent = {HW_REARSENSORNEAR, PulseWidth};
137                 printf("Sending rear sensor near event to motor with pulse width: %f\r\n", PulseWidth);
138                 HasSentRearNearEvent = true;
139                 PostMotor(RearSensorNearEvent);
140             }
141             else if(PulseWidth > FAR_SENSOR_THRESHOLD && !HasSentRearFarEvent){
142                 ES_Event_t RearSensorFarEvent = {HW_REARSENSORFAR, PulseWidth};
143                 printf("Sending rear sensor far event to motor with pulse width: %f\r\n", PulseWidth);
144                 HasSentRearFarEvent = true;
145                 PostMotor(RearSensorFarEvent);
146             }
147             
148         }
149         break;
150         default:
151         {
152         }
153             break;
154     }
155 
156     return ReturnEvent;
157 }
158 
159 void SetHasSentFrontNearEvent(bool val) {
160     HasSentFrontNearEvent = val;
161 }
162 void SetHasSentFrontFarEvent(bool val) {
163     HasSentFrontFarEvent = val;
164 }
165 void SetHasSentRearNearEvent(bool val) {
166     HasSentRearNearEvent = val;
167 }
168 void SetHasSentRearFarEvent(bool val) {
169     HasSentRearFarEvent = val;
170 }
171 
172 /***************************************************************************
173  private functions
174  ***************************************************************************/
175 
176 void InitUltrasonic(void) {
177     UltrasonicReady = false;
178     
179     // front sensor = IC3
180     PortSetup_ConfigureDigitalInputs(_Port_B, _Pin_5); // echo is input
181     PortSetup_ConfigureDigitalOutputs(_Port_B, _Pin_9); // trigger is output
182     LATBSET = 1 << 9;
183     
184     // rear sensor = IC4
185     PortSetup_ConfigureDigitalInputs(_Port_B, _Pin_3); // rear echo RB3 is input
186     PortSetup_ConfigureDigitalOutputs(_Port_B, _Pin_4); // rear trigger RB4 is output
187     LATBSET = 1 << 4;
188     
189 }
190 
191 void InitIC3(void) {
192     IC3CONbits.ON = 0;
193 
194     IC3BUF = 0;
195     IC3CONbits.FEDGE = 1; // rising edge
196     IC3CONbits.C32 = 0;
197     IC3CONbits.ICTMR = 1;
198     IC3CONbits.ICI = 0;
199     IC3CONbits.ICOV = 0;
200     IC3CONbits.ICBNE = 0;
201     IC3CONbits.SIDL = 0;
202     IC3CONbits.ICM = 0b110; // every edge starting with FEDGE
203 
204     IC3R = 0b0001; // RPB5
205     IEC0SET = _IEC0_IC3IE_MASK;
206     IFS0CLR = _IFS0_IC3IF_MASK;
207     INTCONbits.MVEC = 1;
208     IPC3bits.IC3IP = 7;
209 
210     IC3CONbits.ON = 1;
211 }
212 
213 void InitIC4(void){
214     IC4CONbits.ON = 0;
215 
216     IC4BUF = 0;
217     IC4CONbits.FEDGE = 1; // rising edge
218     IC4CONbits.C32 = 0;
219     IC4CONbits.ICTMR = 1;
220     IC4CONbits.ICI = 0;
221     IC4CONbits.ICOV = 0;
222     IC4CONbits.ICBNE = 0;
223     IC4CONbits.SIDL = 0;
224     IC4CONbits.ICM = 0b110; // every edge starting with FEDGE
225 
226     IC4R = 0b0001; // RPB3
227     IEC0SET = _IEC0_IC4IE_MASK;
228     IFS0CLR = _IFS0_IC4IF_MASK;
229     INTCONbits.MVEC = 1;
230     IPC4bits.IC4IP = 7;
231 
232     IC4CONbits.ON = 1;
233 }
234 
235 /***************************************************************************
236  interrupt service routines
237  ***************************************************************************/
238 
239 void __ISR(_INPUT_CAPTURE_3_VECTOR, IPL7SOFT) _HW_IC3Handler(void) {
240     __builtin_disable_interrupts();
241 
242     do {
243         LATBSET = 1 << 9;
244         FrontCounter.components.buf = (uint16_t) IC3BUF;
245         if (IFS0bits.T2IF && FrontCounter.components.buf < 0x8000) {
246             ++KeepTime.ByBytes.Rollover;
247             ++KeepTime2.ByBytes2.Rollover2;
248             ++FrontCounter.components.rollover;  
249             ++RearCounter.components.rollover;
250             IFS0CLR = _IFS0_T2IF_MASK;
251         }
252 
253         FrontPulseWidth = FrontCounter.all - LastFrontTime;
254         LastFrontTime = FrontCounter.all;
255         if (!IsFrontRisingEdge) {
256             ES_Event_t EchoEvent;
257             EchoEvent.EventParam = FrontPulseWidth;
258             EchoEvent.EventType = EV_ECHO;
259             if (UltrasonicReady) {
260                 PostUltrasonicService(EchoEvent);
261             }
262         }
263         
264         IFS0CLR = _IFS0_IC3IF_MASK;
265 
266         IsFrontRisingEdge = !IsFrontRisingEdge;
267         
268     } while (IC3CONbits.ICBNE != 0);
269 
270     IFS0CLR = _IFS0_IC3IF_MASK;
271 
272     __builtin_enable_interrupts();
273 }
274 
275 void __ISR(_INPUT_CAPTURE_4_VECTOR, IPL7SOFT) _HW_IC4Handler(void) {
276     __builtin_disable_interrupts();
277 
278     do {
279         LATBSET = 1 << 4;
280         RearCounter.components.buf = (uint16_t) IC4BUF;
281         if (IFS0bits.T2IF && RearCounter.components.buf < 0x8000) {
282             ++KeepTime.ByBytes.Rollover;
283             ++KeepTime2.ByBytes2.Rollover2;
284             ++FrontCounter.components.rollover;
285             ++RearCounter.components.rollover;
286             IFS0CLR = _IFS0_T2IF_MASK;
287         }
288 
289         RearPulseWidth = RearCounter.all - LastRearTime;
290         LastRearTime = RearCounter.all;
291         if (!IsRearRisingEdge) {
292             ES_Event_t EchoEvent;
293             EchoEvent.EventParam = RearPulseWidth;
294             EchoEvent.EventType = EV_REAR_ECHO;
295             if (UltrasonicReady) {
296                 PostUltrasonicService(EchoEvent);
297             }
298         }
299         
300         IFS0CLR = _IFS0_IC4IF_MASK;
301 
302         IsRearRisingEdge = !IsRearRisingEdge;
303         
304     } while (IC4CONbits.ICBNE != 0);
305 
306     IFS0CLR = _IFS0_IC4IF_MASK;
307 
308     __builtin_enable_interrupts();
309 }