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