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

   1 /* Aereo Palla
   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(9,10,7,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 int ail ; // Valore per ailerons
  33 int ailIn ; // Valore rilevato del 4 Ch della RX
  34 unsigned long ailTimer ; // millis per ail
  35
  36
  37 // FSM gestione alettoni
  38 enum  { // Stati della FMS
  39     middle,   // centrale
  40     sxin,     // transizione a sx
  41     sx,       // sx
  42     dxin,     // transizione a dx
  43     dx        // dx
  44 } ailstate  = middle;
  45
  46 // Vars FSM
  47 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  48 unsigned long pausa = 1000;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  49
  50 // Variabili per interrupt 0 si PIN 2: Throttle
  51 volatile unsigned int chValue2 = 1500; // Valore computato
  52 volatile unsigned int chStart2 = 1500; // Inizio rilevamento
  53
  54 // Variabili per interrupt 1 su PIN 3: Ailerons
  55 volatile unsigned int ail = 1500; // Valore computato
  56 volatile unsigned int mid_point = 1560; // Inizio rilevamento
  57
  58
  59 // Variabili per autocalibrazione 1
  60 const byte chPin3 = 3; // PIN per la calibrazione
  61 int mid_point3 = 1000;
  62
  63 // Vars Alettoni
  64 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  65 const int deviation = 50 ; // deviazione dal punto medio
  66         //per entrare nello stato successivo dal centro
  67
  68 ///////////////////////////////////////////////////////////
  69 void setup() {
  70
  71 //   #define DEBUG
  72
  73 #ifdef DEBUG
  74    Serial.begin(9600);
  75 #endif
  76
  77 // Funzione relativa a calibrazione:
  78 mid_point =  calibraTrim(ailPin) ; // + LED di servizio per monitor calibrazione
  79 }
  80
  81 void loop() {
  82     currentMillis = millis(); // Timestamp per tutto il loop
  83
  84
  85
  86     switch (ailstate) {
  87     case middle:
  88         ailerons.White();
  89         // Alettoni piatti
  90         if (ail > mid_point + deviation + deviation /3) {
  91             // extra margine per avere un po' di gioco
  92             ailstate = sxin;
  93             FSM_lastMillis = currentMillis;
  94         }
  95         else if (ail < mid_point - deviation - deviation / 3) {
  96             ailstate = dxin;
  97             FSM_lastMillis = currentMillis ;
  98         } ;
  99         break;
 100
 101     case sxin:
 102         // Transizione a sx
 103         sxLamp.Blink(200);
 104         if (currentMillis - pausa > FSM_lastMillis ) {
 105             ailstate = sx;
 106         }
 107         break;
 108
 109     case sx:
 110         // dx
 111         ailerons.Green();
 112         if (ail < mid_point + deviation) {
 113             ailstate = middle;
 114         }
 115         else if (ail < mid_point - deviation) {
 116             FSM_lastMillis = currentMillis;
 117             ailstate = dxin;
 118         } ;
 119         break;
 120
 121     case dxin:
 122         // Transizione a dx
 123         dxLamp.Blink(200);
 124         if (currentMillis - pausa > FSM_lastMillis ) {
 125             ailstate = dx;
 126         }
 127         break;
 128
 129     case dx:
 130         // sx
 131         ailerons.Blue();
 132         if (ail > mid_point - deviation) {
 133             ailstate = middle;
 134         }
 135         else if (ail > mid_point + deviation) {
 136             FSM_lastMillis = currentMillis;
 137             ailstate = dxin;
 138         } ;
 139         break;
 140     }
 141 #ifdef DEBUG
 142     Serial.print("ailIn: ");
 143     Serial.print(ailIn);
 144     Serial.print("\tail: ");
 145     Serial.print(ail);
 146     Serial.print("\t ailstate:");
 147     Serial.println(ailstate);
 148 #endif
 149 }
 150 // Functions
 151 void chRise2() {
 152     attachInterrupt(0, chFall2, FALLING);
 153     chStart2 = micros();
 154 }
 155
 156 void chFall2() {
 157     attachInterrupt(0, chRise2, RISING);
 158     chValue2 = micros() - chStart2;
 159 }
 160 // Seconod iterrupt
 161 void chRise3() {
 162     attachInterrupt(1, chFall3, FALLING);
 163     chStart3 = micros();
 164 }
 165
 166 void chFall3() {
 167     attachInterrupt(1, chRise3, RISING);
 168     ail = micros() - chStart3;
 169 }