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 enum  { // Stati della FMS
  21     middle,   // centrale
  22     sxin,     // transizione a sx
  23     sx,       // sx
  24     dxin,     // transizione a dx
  25     dx        // dx
  26 } ailstate  = middle;
  27
  28 // Un LED RGB
  29 RGBLed ailerons(11,10,9);
  30
  31 const byte ailPin = A4;
  32 int ail ; // Valore a 8bit per ailerons
  33 int ailIn ; // Valore rilevato del 4 Ch della RX
  34
  35
  36 unsigned long now; // timestamp reference for millis
  37 unsigned long pausa = 1000;
  38
  39 int mid_point = 1500 ; // centro del segnale, trimmato nel setup
  40 const int deviation = 50 ; // deviazione per entrare nello stato succiessivo
  41
  42 void setup() {
  43     /* Bisognerebbe introdurre una calibrazione per compensare i TRIM
  44        ed eventualmente i dual rates.
  45
  46     - attivarla se allo start un ale e' al massimo
  47     - fargli leggere i valori massimi
  48     - salvarli in eprom
  49     - per i dual rates: si potrebbe intercettare valori oltre al max
  50       e in base a questi traslare le soglie automaticamente
  51
  52     Hint: leggere la soglia di rollio significativo in volo
  53           e inserirla nei riferimenti.
  54     */
  55
  56     Serial.begin(9600);
  57
  58 /* START Calibrazione TRIM canale:
  59    Lettura di 10 smaple
  60    calcolo del valore medio esclusi gli 0
  61  */
  62     byte a = 0;
  63     Serial.println(">> Calibrazione: ");
  64     while (a < 10) {
  65         ailIn = pulseIn(ailPin, HIGH, 25000);
  66         if (ailIn != 0 ) {
  67             ail = ail + ailIn ;
  68             a++ ;
  69             Serial.print(a);
  70             Serial.print(": ");
  71             Serial.println(ail);
  72             digitalWrite(13, !digitalRead(13));
  73             delay(10);
  74         }
  75     }
  76     mid_point = ail / 10 ;
  77     Serial.print(">> Fine Calibrazione: ");
  78     Serial.print(mid_point);
  79     Serial.println("--");
  80 // END calibrazione
  81
  82     // Funzione relativa a calibrazione:
  83 //mid_point =  calibraTrim(ailPin) ; // Pin a cui e' collegato il canale
  84 //mid_point =  calibraTrim(aliPin,11) ; // + LED di servizio per monitor calibrazione
  85 }
  86
  87 void loop() {
  88
  89 // Lettura Aileron channel: FAKE con un potenziometro 10K
  90 //ailIn = analogRead(3);
  91 //ail = constrain(aliIn * 2 , 0, 2000) ;
  92
  93 // Lettura ailerons channel
  94     ailIn = pulseIn(ailPin, HIGH, 25000);
  95     if (ailIn != 0) {
  96         ail = constrain(ailIn, 1000, 2000);
  97     } ;
  98     // con un altra ricevente, fare una calibrazione nel caso.
  99     // Middle = 1512
 100
 101
 102     switch (ailstate) {
 103     case middle:
 104         // Alettoni piatti
 105         if (ail > mid_point + deviation + deviation /3) {
 106             ailstate = sxin;
 107             now = millis() ;
 108             goto sxin ;
 109         }
 110         else if (ail < mid_point - deviation - deviation / 3) {
 111             ailstate = dxin;
 112             now = millis() ;
 113             goto dxin ;
 114         } ;
 115         ailerons.White();
 116
 117         break;
 118
 119     case sxin:
 120 sxin:
 121         // Transizione a sx
 122         ailerons.Off();
 123         if (millis() - pausa > now ) {
 124             ailstate = sx;
 125         }
 126         break;
 127
 128     case sx:
 129         // dx
 130         ailerons.Green();
 131         if (ail < mid_point + deviation) {
 132             ailstate = middle;
 133         }
 134         else if (ail < mid_point - deviation) {
 135             now = millis() ;
 136             ailstate = dxin;
 137         } ;
 138         break;
 139
 140     case dxin:
 141         // Transizione a dx
 142 dxin:
 143         ailerons.Off();
 144         if (millis() - pausa > now ) {
 145             ailstate = dx;
 146         }
 147         break;
 148
 149     case dx:
 150         // sx
 151         ailerons.Blue();
 152         if (ail > mid_point - deviation) {
 153             ailstate = middle;
 154         }
 155         else if (ail > mid_point + deviation) {
 156             now = millis() ;
 157             ailstate = dxin;
 158         } ;
 159         break;
 160     }
 161
 162     Serial.print("ailIn: ");
 163     Serial.print(ailIn);
 164     Serial.print("\tail: ");
 165     Serial.print(ail);
 166     Serial.print("\t ailstate:");
 167     Serial.println(ailstate);
 168     //  delay(200);
 169 }