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

   1 /* Ailerons state machine
   2             Serial.print(mid_point);
   3
   4 Pilotare un LED RGB in base al canale degli alettoni:
   5 Questo sketch usa 2 interrupts per thr e alettoni.
   6
   7 INPUT:
   8 PIN 2   : throttle
   9 PIN 3   : alettoni
  10
  11 OUTPUT:
  12 ailerons        RGB Alettoni
  13 motore          Motore PWM
  14 left, right 2 Lampeggiatori PWM laterali + PWM
  15
  16 FSM per alettoni
  17 = 3 stati + 2 transizioni:
  18 - piatto
  19 - roll a sx
  20 - roll a dx
  21 - piatto -> sx
  22 - piatto -> dx
  23
  24
  25 Ciclo if per gestione di 3 stati del motore:
  26 - idle
  27 - middle
  28 - max
  29
  30
  31 TODO:
  32 * Da testare! Mai provato.
  33
  34 */
  35
  36 #include <common.h>
  37 #define DEBUG
  38
  39 // Variabili per interrupt 0 si PIN 2
  40 volatile unsigned int thr = 1500; // Valore computato
  41 volatile unsigned int chStart2 = 0; // Inizio rilevamento
  42
  43 // Variabili per interrupt 1 su PIN 3
  44 volatile unsigned int ail = 1500; // Valore computato
  45 volatile unsigned int chStart3 = 1500; // Inizio rilevamento
  46 // Variabili per lettura canale alettoni
  47 byte ailPin = 2; // Calibrazione
  48
  49 // Variabili per autocalibrazione 0
  50 int mid_point2 = 1500;
  51
  52
  53 // LED RGB alettoni
  54 RGBLed ailerons(5,6,9); // Common Cat
  55 // Transizione: Lampeggiatore
  56 Lampeggiatore sxLamp(6); // Lampeggiatore
  57 Lampeggiatore dxLamp(9); // Lampeggiatore
  58
  59
  60
  61 // Vars Alettoni
  62 int mid_point = 1500 ; // centro del segnale, trimmato nel setup
  63 const int deviation = 50 ; // deviazione dal punto medio
  64         //per entrare nello stato successivo dal centro
  65
  66 // Led motore e altri:
  67 Pwm motore = 10;
  68
  69 // Variabili:
  70 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  71 byte caso; // Random var
  72 byte thrBit ; // Valore a 8bit per il throttle
  73
  74 // FSM gestione alettoni
  75 enum  { // Stati della FMS
  76     middle,   // centrale
  77     sxin,     // transizione a sx
  78     sx,       // sx
  79     dxin,     // transizione a dx
  80     dx        // dx
  81 } ailstate  = middle;
  82
  83 // Vars FSM
  84 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  85 unsigned long pausa = 400;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  86
  87 ///////////////////////////////////////////////////////////
  88 void setup() {
  89 #ifdef DEBUG
  90    Serial.begin(9600);
  91 #endif
  92     attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
  93     attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168
  94
  95 // Funzione relativa a calibrazione:
  96 //mid_point =  calibraTrim(ailPin) + 10 ; // + LED di servizio per monitor calibrazione
  97 mid_point =  1500 ; // + LED di servizio per monitor calibrazione
  98 }
  99
 100 void loop() {
 101     currentMillis = millis(); // Timestamp per tutto il loop
 102
 103     switch (ailstate) {
 104     case middle:
 105         ailerons.White();
 106         // Alettoni piatti
 107         if (ail > mid_point + deviation + deviation /3) {
 108             // extra margine per avere un po' di gioco
 109             ailstate = sxin;
 110             FSM_lastMillis = currentMillis;
 111         }
 112         else if (ail < mid_point - deviation - deviation / 3) {
 113             ailstate = dxin;
 114             FSM_lastMillis = currentMillis ;
 115         } ;
 116         break;
 117
 118     case sxin:
 119         // Transizione a sx
 120         sxLamp.Blink(pausa/2);
 121         if (currentMillis - pausa > FSM_lastMillis ) {
 122             ailstate = sx;
 123         }
 124         break;
 125
 126     case sx:
 127         ailerons.Green();
 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     case dxin:
 138         // Transizione a dx
 139         dxLamp.Blink(pausa/2);
 140         if (currentMillis - pausa > FSM_lastMillis ) {
 141             ailstate = dx;
 142         }
 143         break;
 144
 145     case dx:
 146         ailerons.Blue();
 147         if (ail > mid_point - deviation) {
 148             ailstate = middle;
 149         }
 150         else if (ail > mid_point + deviation) {
 151             FSM_lastMillis = currentMillis;
 152             ailstate = dxin;
 153         } ;
 154         break;
 155     }
 156
 157 // Gestione throttle
 158     if (thr < 1050) {
 159         // IDLE
 160         motore.lDown(1500);
 161     }
 162     else if (thr > 1900) {
 163         // Throttle al massimo: LED laterali lampeggiano a caso,
 164         // Sotto luminosita' a caso
 165         caso = random(30, 250) ;
 166         motore.lSet(caso);
 167         delay(caso); // Blocking!
 168     }
 169     else {
 170         // Throttle medio
 171         thrBit = map(thr,1050, 1900, 0, 255);
 172         motore.lSet(thrBit);   // Luminosita' proporzionale al throttle
 173     }
 174
 175
 176 #ifdef DEBUG
 177 Serial.print((thr - 980) / 4);
 178     Serial.print("\tail: ");
 179     Serial.print(ail);
 180     Serial.print("\t ailstate:");
 181     Serial.println(ailstate);
 182 #endif
 183 }
 184
 185
 186 // Functions
 187 void chRise2() {
 188     attachInterrupt(0, chFall2, FALLING);
 189     chStart2 = micros();
 190 }
 191
 192 void chFall2() {
 193     attachInterrupt(0, chRise2, RISING);
 194     ail = micros() - chStart2;
 195 }
 196
 197 // Seconod iterrupt
 198 void chRise3() {
 199     attachInterrupt(1, chFall3, FALLING);
 200     chStart3 = micros();
 201 }
 202
 203 void chFall3() {
 204     attachInterrupt(1, chRise3, RISING);
 205     thr = micros() - chStart3;
 206 }