root/RF_BT_Tail/Moves.c

Revision 244, 12.6 kB (checked in by phil, 4 years ago)

added initial Tail Source Package

Line 
1 // File: Moves.c
2 //------------------------------------------------------------------------------
3 // Author: Giovanni de Sanctis
4 // Email: info@lateral-technologies.com
5 //------------------------------------------------------------------------------
6 // Date: Dec 2018
7 // Manages the tail moves and LED patterns
8 //------------------------------------------------------------------------------
9
10
11 #include "Moves.h"
12 #include "Servos.h"
13 #include "Power.h"
14 #include "LEDs.h"
15 #include "mcc_generated_files/device_config.h"
16
17 #include <stdlib.h>
18 #include <string.h>                     //For memset
19
20 #define  N_ATTEMPTS 100         //#attempts to draw a random number in range
21
22 #define  DD             15                      //Delay for morse dot
23 #define  DL             30                      //Delay for morse line
24 #define  DP             60                      //Delay for morse pause
25 #define  DF0    10                      //Delay for flame
26 #define  DF1    20                      //Delay for flame
27 #define  DF2    40                      //Delay for flame
28
29 const MoveType TAIL_HOME=       { 1,PL,P8, 0, 0, 0,       PL,P0, 0, 0, 0,       100, 0, 0, 0, 0};
30 const MoveType SLOW_WAG1=       { 3,P1,P7, 0, 0, 0,   P1,P7, 0, 0, 0,   75, 75, 0, 0, 0};
31 const MoveType SLOW_WAG2=       { 5,P2,P6, 0, 0, 0,       P2,P6, 0, 0, 0,       25, 25, 0, 0, 0};
32 const MoveType SLOW_WAG3=       { 5,P2,P6, 0, 0, 0,       P2,P6, 0, 0, 0,       33, 33, 0, 0, 0};
33
34 const MoveType FAST_WAG =       {20,P0,P8, 0, 0, 0,       P0,P8, 0, 0, 0,       10, 10, 0, 0, 0};
35 const MoveType SHORT_WAG=       {20,P2,P6, 0, 0, 0,       P2,P6, 0, 0, 0,        7,  7, 0, 0, 0};
36 const MoveType HAPPY_WAG=       {10,P8,P4,P8,P8,P8,       P0,P0,P0,P2,P0,       10, 10,10,10, 0};
37
38 const MoveType ERECT    =       { 1,P0,P0, 0, 0, 0,       P8,P8, 0, 0, 0,  -100,100, 0, 0, 0};
39 const MoveType ERECT_PULSE=     {10,P8,P0, 0, 0, 0,       P8,P0, 0, 0, 0,       20, 20, 0, 0, 0};
40 const MoveType TREMBLE1 =       {25,P8,P7,P8,P8,P8,       P0,P0,P0,P1,P0,        3,  1, 1, 1, 1};
41 const MoveType TREMBLE2 =       {10,P8,P8,P3,P3, 0,       P0,P0,P1,P1, 0,       10,      1,10, 0, 0};
42 const MoveType ERECT_TREM =     {30,P3,P3,P7,P7, 0,       P5,P5,P2,P2, 0,        3,  3, 0, 0, 0};
43
44 //Move groups (used in RF mode)
45 const MoveType* const moveGrpSlow[MAX_GRP_SLOW] = {&SLOW_WAG1,&SLOW_WAG2,&SLOW_WAG3};
46 const MoveType* const moveGrpFast[MAX_GRP_FAST] = {&FAST_WAG,&SHORT_WAG,&HAPPY_WAG};
47 const MoveType* const moveGrpTrem[MAX_GRP_TREM] = {&TREMBLE1,&TREMBLE2,&ERECT_TREM};
48 const MoveType* const moveGrpErec[MAX_GRP_EREC] = {&ERECT};
49
50 //Move groups for 'auto moves' (used in BT mode)
51 const MoveType* const moveGrpBT[MAX_GRP_BT][MAX_MOV_BT] =
52 {
53   {&SLOW_WAG1,  &SLOW_WAG2,     &SLOW_WAG3,     &SLOW_WAG2},            //Relaxed
54   {&FAST_WAG,   &SHORT_WAG,     &HAPPY_WAG,     &ERECT},                        //Excited
55   {&TREMBLE1,   &TREMBLE2,      &ERECT_TREM,&ERECT_PULSE}               //Tense
56 };
57
58 //Random moves (used in RF mode)
59 const MoveType* const moveRndGrp[MAX_GRP_RAND] =
60 {
61   &SLOW_WAG1,   &SHORT_WAG,  &FAST_WAG,   &SLOW_WAG2,
62   &HAPPY_WAG,   &FAST_WAG,   &SHORT_WAG,  &SLOW_WAG3,
63   &SLOW_WAG2,   &SLOW_WAG3,  &HAPPY_WAG,  &SHORT_WAG,
64   &FAST_WAG,   &SLOW_WAG3,   &SLOW_WAG1,  &SLOW_WAG2
65 };
66
67 //Time intervals for random moves
68 const uint8_t moveRndGrpT[MAX_GRP_RAND]=
69 {
70   35,17,26,40,18,17,36,32,25,34,28,31,31,42,32,33
71 };
72
73 const LedsType LEDS_OFF =       {1,
74   0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
75   100,  0,  0,  0,      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,      0
76 };
77 const LedsType LEDS_ON = {10,  //10 cycles*100*20ms = 20 seconds
78   0xF,0xF,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
79   100,  0,  0,  0,      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,      0
80 };
81 const LedsType LEDS_RECT=       {15, //15 cycles*(50+50)*20ms = 30 seconds
82   0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
83   -50,-50,      0,      0,      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,      0
84 };
85 const LedsType LEDS_TRI =       {15, //15 cycles*(50+50)*20ms = 30 seconds
86   0x0,0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
87    50, 50,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0
88 };
89 const LedsType LEDS_SAW =       {15, //15 cycles*(50+50)*20ms = 30 seconds
90   0x0,0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
91   100,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0
92 };
93 const LedsType LEDS_BEA =       {15,
94   0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
95    -5,-95,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0
96 };
97 const LedsType LEDS_SOS =       {3,
98   0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
99   -DD,-DD,-DD,-DD,-DD,-DP,-DL,-DL,-DL,-DL,-DL,-DP,-DD,-DD,-DD,-DD,-DD,-DP,-DP,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0
100 };
101 const LedsType LEDS_FLA =       {2,
102   0x7,0x8,0x6,0x5,0x3,0x7,0x8,0x7,0x5,0x2,0x6,0x8,0x6,0x5,0x8,0x5,0x2,0x4,0x8,0x5,0x1,0x7,0x8,0x7,0x4,0x8,0x7,0x8,0x8,0x4,0x5,0x3,
103   DF1,DF2,DF1,DF2,DF0,DF2,DF0,DF2,DF0,DF0,DF0,DF2,DF2,DF0,DF1,DF1,DF1,DF1,DF2,DF1,DF0,DF2,DF2,DF2,DF0,DF2,DF2,DF2,DF1,DF2,DF0,DF0
104 };
105
106 const LedsType LEDS_STR =       {250,
107   0x8,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,
108    -3, -3,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0
109 };
110
111 const LedsType* const ledsGrpA[MAX_LED_GRPA] = {&LEDS_ON, &LEDS_OFF};
112 const LedsType* const ledsGrpB[MAX_LED_GRPB] = {&LEDS_RECT, &LEDS_TRI, &LEDS_SAW};
113 const LedsType* const ledsGrpC[MAX_LED_GRPC] = {&LEDS_BEA, &LEDS_STR};
114
115
116 MoveType userMove[MAX_USER_MOVES];
117 LedsType userLeds[MAX_USER_LEDS];
118 MoveStruct currMove;
119 LedsStruct currLeds;
120
121 AutoStruct autoMode;
122
123 const MoveCmd MOVE_CMDS[MAX_MOVE_CMDS]=
124 {
125   "TAILHM",&TAIL_HOME,
126   "TAILS1",&SLOW_WAG1,
127   "TAILS2",&SLOW_WAG2,
128   "TAILS3",&SLOW_WAG3,
129   "TAILFA",&FAST_WAG,
130   "TAILSH",&SHORT_WAG,
131   "TAILHA",&HAPPY_WAG,
132   "TAILER",&ERECT,
133   "TAILEP",&ERECT_PULSE,
134   "TAILT1",&TREMBLE1,
135   "TAILT2",&TREMBLE2,
136   "TAILET",&ERECT_TREM,
137   "TAILU1",&userMove[0],
138   "TAILU2",&userMove[1],
139   "TAILU3",&userMove[2],
140   "TAILU4",&userMove[3],
141 };
142
143 const LedsCmd LEDS_CMDS[MAX_LEDS_CMDS]=
144 {
145   "LEDOFF",&LEDS_OFF,
146   "LEDON",&LEDS_ON,
147   "LEDREC",&LEDS_RECT,
148   "LEDTRI",&LEDS_TRI,
149   "LEDSAW",&LEDS_SAW,
150   "LEDSOS",&LEDS_SOS,
151   "LEDFLA",&LEDS_FLA,
152   "LEDSTR",&LEDS_STR,
153   "LEDBEA",&LEDS_BEA,
154   "LEDUS1",&userLeds[0],
155   "LEDUS2",&userLeds[1],
156   "LEDUS3",&userLeds[2],
157   "LEDUS4",&userLeds[3]
158 };
159
160 void movesInit()
161 {
162   initServos();   //Inits the PWM peripherals
163   //setMove(&TAIL_HOME);
164   startLEDs();
165   setLEDsBrightness(0);
166   stopLEDs();
167   autoMode.timeout=0;           //Makes sure 'auto mode' is off
168 }
169
170 uint8_t setUserMove(uint8_t preset,uint8_t len,uint8_t cycles,int8_t *pos1,int8_t *pos2,int8_t *steps)
171 {
172   uint8_t i;
173  
174   if (preset>=MAX_USER_MOVES) return 1;
175   memset(&userMove[preset],0,sizeof(MoveType));
176   userMove[preset].cycles=cycles;
177   if (len>MAX_MOVES) len=MAX_MOVES;
178   for (i=0; i<len; i++)
179   {
180         userMove[preset].pos1[i]=pos1[i];
181         userMove[preset].pos2[i]=pos2[i];
182         userMove[preset].steps[i]=steps[i];
183   }
184   return 0;
185 }
186
187 uint8_t setUserLeds(uint8_t preset,uint8_t len,uint8_t cycles,int8_t *pos,int8_t *steps)
188 {
189   uint8_t i;
190  
191   if (preset>=MAX_USER_LEDS) return 1;
192   memset(&userLeds[preset],0,sizeof(LedsType));
193   userLeds[preset].cycles=cycles;
194   if (len>MAX_MOVES) len=MAX_MOVES;
195   for (i=0; i<len; i++)
196   {
197         userLeds[preset].pos[i]=pos[i];
198         userLeds[preset].steps[i]=steps[i];
199   }
200   return 0;
201 }
202
203 void setRandMove()
204 {
205   uint8_t i,g,m;
206  
207   //Select group
208   for (i=0; i<N_ATTEMPTS; i++)
209   {
210         m = (uint8_t)rand();
211         g = (m >> 2)&0x03u;
212         if ((1<<g)&autoMode.grps) break;
213   }
214   if ((1<<g)&autoMode.grps == 0) g=1;
215  
216   //Select move within group
217   m = (m & 0x03u);
218  
219   setMove(moveGrpBT[g][m]);
220 }
221
222 void setRandPause()
223 {
224   uint8_t i,p;
225   for (i=0; i<N_ATTEMPTS; i++)
226   {
227         p = (uint8_t)rand();
228         if (p>=autoMode.t1 && p<=autoMode.t2) break;
229   }
230   if (p<autoMode.t1 || p>autoMode.t2) p=autoMode.t1;
231  
232   autoMode.pause = p * F_SERVO;
233 }
234
235 //Sets up automatic mode (random moves for a specified length of time)
236 void setAuto(uint8_t t1, uint8_t t2, uint8_t dur, uint8_t grps)
237 {
238   autoMode.timeout = (uint16_t)dur * F_SERVO;
239   autoMode.t1 = t1;
240   autoMode.t2 = t2;
241   autoMode.pause = 1;                           //Will start by setting up a move
242   autoMode.grps = grps;
243 }
244
245 void abortAuto()
246 {
247   autoMode.timeout = 0;
248 }
249
250 //Sets up move variables for the ISR
251 void setMove(const MoveType* movePtr)
252 {
253   currMove.movePtr = movePtr;
254   currMove.cycIter=movePtr->cycles;
255   currMove.moveIter=0;
256   currMove.stepIter=0;
257   //if (movePtr!=&TAIL_HOME)
258   //{
259 //      currMove.pos1=POS2SERVO(SERVO1_HOME);
260 //      currMove.pos2=POS2SERVO(SERVO2_HOME);
261 //  }
262   startServos();
263 }
264
265 AckEnum updateMove()
266 {
267   if (autoMode.timeout)
268   {
269         --autoMode.timeout;
270         if (autoMode.timeout == 0) return ACK_END_AUTO;
271        
272         if (autoMode.pause)
273         {
274           --autoMode.pause;
275           if (autoMode.pause == 0) setRandMove();
276           //return ACK_RUNNING;
277         }
278   }
279                  
280   if (currMove.movePtr && currMove.cycIter)
281   {
282         if (currMove.stepIter==0)
283         {
284           int16_t pos;
285           //Copy new pos, unless it's a PL (keeps the last position)
286           pos=currMove.movePtr->pos1[currMove.moveIter];
287           if (pos!=PL) currMove.pos1=POS2SERVO(pos);
288           pos=currMove.movePtr->pos2[currMove.moveIter];
289           if (pos!=PL) currMove.pos2=POS2SERVO(pos);
290                  
291           currMove.steps=(int16_t)currMove.movePtr->steps[currMove.moveIter];
292                  
293           if (currMove.steps>0)
294           {
295                 uint8_t p1=currMove.moveIter+1u;
296                 if (p1>=MAX_MOVES) /*|| currMove.movePtr->steps[p1]==0)*/ p1=0;
297                 currMove.d1=(POS2SERVO(currMove.movePtr->pos1[p1])-currMove.pos1)/currMove.steps;
298                 currMove.d2=(POS2SERVO(currMove.movePtr->pos2[p1])-currMove.pos2)/currMove.steps;
299           }
300           else
301           {
302                 currMove.steps = -currMove.steps;
303                 currMove.d1 = 0;
304                 currMove.d2 = 0;
305           }     
306         }
307        
308         setServo1(currMove.pos1);
309         setServo2(currMove.pos2);
310         currMove.pos1+=currMove.d1;
311         currMove.pos2+=currMove.d2;
312         currMove.stepIter++;
313        
314        
315         //if (currMove.stepIter>=currMove.movePtr->steps[currMove.moveIter] || currMove.steps==0 || currMove.pos1==0 || currMove.pos2==0)
316         if (currMove.stepIter>=currMove.steps || currMove.steps==0)
317         {
318           currMove.stepIter=0;
319           currMove.moveIter++;
320           //if (currMove.moveIter>=MAX_MOVES || currMove.steps==0)
321           if (currMove.moveIter>=MAX_MOVES || currMove.movePtr->steps[currMove.moveIter]==0)
322           {
323                 currMove.moveIter=0;
324                 currMove.cycIter--;
325                 if (currMove.cycIter==0)
326                 {
327                   if (currMove.movePtr==&TAIL_HOME)
328                   {
329                         currMove.movePtr=0;
330                         if (autoMode.timeout) setRandPause();
331                         stopServos();
332                         return ACK_END_MOVE;
333                   }
334                   else
335                   {
336                         setMove(&TAIL_HOME);
337                         return ACK_RUNNING;
338                   }
339                 }
340           }
341         }
342         return ACK_RUNNING;
343   }
344   else stopServos();
345   return ACK_WAITING;
346 }
347
348 //Sets up led pattern variables for updateLeds()
349 void setLEDs(const LedsType* ledsPtr)
350 {
351   currLeds.ledsPtr = ledsPtr;
352   currLeds.cycIter=ledsPtr->cycles;
353   currLeds.moveIter=0;
354   currLeds.stepIter=0;
355   startLEDs();
356 }
357
358 AckEnum updateLEDs()
359 {
360   if (currLeds.ledsPtr && currLeds.cycIter)
361   {
362         if (currLeds.stepIter==0)
363         {
364           currLeds.pos=POS2LEDS(currLeds.ledsPtr->pos[currLeds.moveIter]);
365           currLeds.steps=currLeds.ledsPtr->steps[currLeds.moveIter];
366          
367           if (currLeds.steps>0)
368           {
369                 uint8_t p1=currLeds.moveIter+1u;
370                 if (p1>=MAX_PATTERNS) /*|| currLeds.ledsPtr->steps[p1]==0)*/ p1=0;
371                 currLeds.d=(POS2LEDS(currLeds.ledsPtr->pos[p1])-currLeds.pos)/currLeds.steps;
372           }
373           else
374           {
375                 currLeds.steps = -currLeds.steps;
376                 currLeds.d = 0;
377           }
378         }
379         setLEDsBrightness2(currLeds.pos);
380         currLeds.pos+=currLeds.d;
381         currLeds.stepIter++;
382        
383         //if (currLeds.stepIter>=currLeds.steps || currLeds.steps==0)
384         if (currLeds.stepIter>=currLeds.steps || currLeds.ledsPtr->steps[currLeds.moveIter]==0)
385         {
386           currLeds.stepIter=0;
387           currLeds.moveIter++;
388           if (currLeds.moveIter>=(MAX_PATTERNS) || currLeds.steps==0)
389           {
390                 currLeds.moveIter=0;
391                 currLeds.cycIter--;
392                 if (currLeds.cycIter==0)
393                 {
394                   currLeds.ledsPtr=0;
395                   return ACK_END_LEDS;
396                 }
397           }
398         }
399         return ACK_RUNNING;                                                                                                                     return ACK_RUNNING;
400   }
401   else stopLEDs();
402   return ACK_WAITING;
403 }
Note: See TracBrowser for help on using the browser.