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