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 per versione con interrupts vedere esempio successivo.
   5
   6 = 3 stati + 2 transizioni:
   7 - piatto
   8 - roll a sx
   9 - roll a dx
  10
  11 TODO:
  12 * clean up magic numbers
  13
  14 */
  15
  16 #include <common.h>
  17 #define dEBUG // Cambiare in DEBUG per il debug
  18
  19 // Variabili:
  20 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  21
  22 // Un LED RGB
  23 RGBLed ailerons(9,10,7,255); // Common Cat
  24
  25 // Transizione: Pwm
  26 Lampeggiatore sxLamp(10); // Lampeggiatore
  27 Lampeggiatore dxLamp(9); // Lampeggiatore
  28
  29
  30 // Variabili per lettura canale servo
  31 int ail ; // Valore per ailerons
  32 int ailIn ; // Valore rilevato del 4 Ch della RX
  33 unsigned long ailTimer ; // millis per ail
  34
  35
  36 // FSM gestione alettoni
  37 enum  { // Stati della FMS
  38     middle,   // centrale
  39     sxin,     // transizione a sx
  40     sx,       // sx
  41     dxin,     // transizione a dx
  42     dx        // dx
  43 } ailstate  = middle;
  44
  45 // Vars FSM
  46 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  47 unsigned long pausa = 1000;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  48
  49 // Variabili per interrupt 0 si PIN 2: Throttle
  50 volatile unsigned int chValue2 = 1500; // Valore computato
  51 volatile unsigned int chStart2 = 1500; // Inizio rilevamento
  52
  53 // Variabili per interrupt 1 su PIN 3: Ailerons
  54 volatile unsigned int ail = 1500; // Valore computato
  55 volatile unsigned int mid_point = 1560; // Inizio rilevamento
  56
  57
  58 // Variabili per autocalibrazione 1
  59 const byte chPin3 = 3; // PIN per la calibrazione
  60 int mid_point3 = 1000;
  61
  62 // Vars Alettoni
  63 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  64 const int deviation = 50 ; // deviazione dal punto medio
  65         //per entrare nello stato successivo dal centro
  66
  67 ///////////////////////////////////////////////////////////
  68 void setup() {
  69
  70 //   #define DEBUG
  71
  72 #ifdef DEBUG
  73    Serial.begin(9600);
  74 #endif
  75
  76 // Funzione relativa a calibrazione:
  77 mid_point =  calibraTrim(ailPin) ; // + LED di servizio per monitor calibrazione
  78 }
  79
  80 void loop() {
  81     currentMillis = millis(); // Timestamp per tutto il loop
  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 }
 149 // Functions
 150 void chRise2() {
 151     attachInterrupt(0, chFall2, FALLING);
 152     chStart2 = micros();
 153 }
 154
 155 void chFall2() {
 156     attachInterrupt(0, chRise2, RISING);
 157     chValue2 = micros() - chStart2;
 158 }
 159 // Seconod iterrupt
 160 void chRise3() {
 161     attachInterrupt(1, chFall3, FALLING);
 162     chStart3 = micros();
 163 }
 164
 165 void chFall3() {
 166     attachInterrupt(1, chRise3, RISING);
 167     ail = micros() - chStart3;
 168 }