git.piffa.net Git - aerei/blob - esempi/ailerons_state_interrupt/ailerons_state_interrupt.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 RGB Alettoni
  13 Motore PWM
  14
  15 FSM per alettoni
  16 = 3 stati + 2 transizioni:
  17 - piatto
  18 - roll a sx
  19 - roll a dx
  20 - piatto -> sx
  21 - piatto -> dx
  22
  23
  24
  25 TODO:
  26 * clean up magic numbers
  27
  28 */
  29
  30 #include <common.h>
  31 #define dEBUG
  32
  33 // Variabili per interrupt 0 si PIN 2
  34 volatile unsigned int thr = 1500; // Valore computato
  35 volatile unsigned int chStart2 = 1500; // Inizio rilevamento
  36
  37 // Variabili per interrupt 1 su PIN 3
  38 volatile unsigned int ail = 1500; // Valore computato
  39 volatile unsigned int chStart3 = 1500; // Inizio rilevamento
  40
  41 // Variabili per autocalibrazione 0
  42 const byte chPin2 = 3; // PIN per la calibrazione
  43 int mid_point2 = 1500;
  44
  45
  46 // Variabili:
  47 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  48
  49 Pwm motore = 11;
  50
  51 // Un LED RGB
  52 RGBLed ailerons(11,10,9,255); // Common Cat
  53
  54 // Transizione: Pwm
  55 Lampeggiatore sxLamp(10); // Lampeggiatore
  56 Lampeggiatore dxLamp(9); // Lampeggiatore
  57
  58
  59 // Variabili per lettura canale servo
  60 byte ailPin = 3; // Calibrazione
  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 // FSM gestione alettoni
  69 enum  { // Stati della FMS
  70     middle,   // centrale
  71     sxin,     // transizione a sx
  72     sx,       // sx
  73     dxin,     // transizione a dx
  74     dx        // dx
  75 } ailstate  = middle;
  76
  77 // Vars FSM
  78 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  79 unsigned long pausa = 500;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  80
  81 ///////////////////////////////////////////////////////////
  82 void setup() {
  83
  84
  85 #ifdef DEBUG
  86    Serial.begin(9600);
  87 #endif
  88
  89     attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
  90     attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168
  91
  92
  93 // Funzione relativa a calibrazione:
  94 mid_point =  calibraTrim(ailPin) + 10 ; // + LED di servizio per monitor calibrazione
  95 //Serial.print(mid_point);
  96 //while(1) {
  97 //}
  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(200);
 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(200);
 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 // PWM Motore
 158 motore.lSet((thr - 980) / 4); // 980 = minimo
 159
 160 #ifdef DEBUG
 161 Serial.print((thr - 980) / 4);
 162     Serial.print("\tail: ");
 163     Serial.print(ail);
 164     Serial.print("\t ailstate:");
 165     Serial.println(ailstate);
 166 #endif
 167 }
 168 // Functions
 169 void chRise2() {
 170     attachInterrupt(0, chFall2, FALLING);
 171     chStart2 = micros();
 172 }
 173
 174 void chFall2() {
 175     attachInterrupt(0, chRise2, RISING);
 176     thr = micros() - chStart2;
 177 }
 178 // Seconod iterrupt
 179 void chRise3() {
 180     attachInterrupt(1, chFall3, FALLING);
 181     chStart3 = micros();
 182 }
 183
 184 void chFall3() {
 185     attachInterrupt(1, chRise3, RISING);
 186     ail = micros() - chStart3;
 187 }