git.piffa.net Git - aerei/blob - esempi/ailerons_state_rgb/ailerons_state_rgb.ino

   1 /* Ailerons state machine
   2
   3 Pilotare un LED RGB in base al canale degli alettoni:
   4
   5 = 3 stati + 2 transizioni:
   6 - piatto
   7 - roll a sx
   8 - roll a dx
   9
  10 NOTE: uso di goto all'interno dell FSM.
  11
  12 TODO:
  13
  14 * clean up magic numbers
  15
  16 */
  17
  18 #include <common.h>
  19
  20 // Variabili:
  21 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  22
  23 // Un LED RGB
  24 RGBLed ailerons(11,10,9,255);
  25
  26 // Variabili per lettura canale servo
  27 const byte ailPin = A4;
  28 int ail ; // Valore a 8bit per ailerons
  29 int ailIn ; // Valore rilevato del 4 Ch della RX
  30 unsigned long ailTimer ; // millis per ail
  31
  32
  33 // FSM gestione alettoni
  34 enum  { // Stati della FMS
  35     middle,   // centrale
  36     sxin,     // transizione a sx
  37     sx,       // sx
  38     dxin,     // transizione a dx
  39     dx        // dx
  40 } ailstate  = middle;
  41
  42 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  43 unsigned long pausa = 1000;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  44 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  45 const int deviation = 50 ; // deviazione dal punto medio
  46         //per entrare nello stato successivo dal centro
  47
  48 ///////////////////////////////////////////////////////////
  49 void setup() {
  50
  51 //   Serial.begin(9600);
  52 //   #define DEBUG
  53
  54     // Funzione relativa a calibrazione:
  55     mid_point =  calibraTrim(ailPin) ; // + LED di servizio per monitor calibrazione
  56 }
  57
  58 void loop() {
  59     currentMillis = millis(); // Timestamp per tutto il loop
  60
  61 // Lettura ailerons channel ogni 200ms
  62     if (currentMillis - ailTimer>= 200) {
  63         ailTimer = currentMillis ;
  64
  65         ailIn = pulseIn(ailPin, HIGH, 25000);
  66         if (ailIn != 0 && ailIn > 1000 && ailIn <2000)  {
  67             // get only resonable values
  68             ail = ailIn;
  69         } ;
  70 // Lettura Aileron channel: FAKE con un potenziometro 10K
  71 // ailIn = analogRead(3);
  72 // ail = 1000 + ailIn
  73     }
  74
  75
  76
  77     switch (ailstate) {
  78     case middle:
  79         // Alettoni piatti
  80         if (ail > mid_point + deviation + deviation /3) {
  81             // extra margine per avere un po' di gioco
  82             ailstate = sxin;
  83             FSM_lastMillis = currentMillis;
  84         }
  85         else if (ail < mid_point - deviation - deviation / 3) {
  86             ailstate = dxin;
  87             FSM_lastMillis = currentMillis ;
  88         } ;
  89         ailerons.Red();
  90         break;
  91
  92     case sxin:
  93         // Transizione a sx
  94         ailerons.Off();
  95         if (currentMillis - pausa > FSM_lastMillis ) {
  96             ailstate = sx;
  97         }
  98         break;
  99
 100     case sx:
 101         // dx
 102         ailerons.Green();
 103         if (ail < mid_point + deviation) {
 104             ailstate = middle;
 105         }
 106         else if (ail < mid_point - deviation) {
 107             FSM_lastMillis = currentMillis;
 108             ailstate = dxin;
 109         } ;
 110         break;
 111
 112     case dxin:
 113         // Transizione a dx
 114         ailerons.Off();
 115         if (currentMillis - pausa > FSM_lastMillis ) {
 116             ailstate = dx;
 117         }
 118         break;
 119
 120     case dx:
 121         // sx
 122         ailerons.Blue();
 123         if (ail > mid_point - deviation) {
 124             ailstate = middle;
 125         }
 126         else if (ail > mid_point + deviation) {
 127             FSM_lastMillis = currentMillis;
 128             ailstate = dxin;
 129         } ;
 130         break;
 131     }
 132 #ifdef DEBUG
 133     Serial.print("ailIn: ");
 134     Serial.print(ailIn);
 135     Serial.print("\tail: ");
 136     Serial.print(ail);
 137     Serial.print("\t ailstate:");
 138     Serial.println(ailstate);
 139 #endif
 140 }