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 Note:
  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 // LEDS
  46 Pwm motore = 7;
  47
  48 //RGB
  49 RGBLed ailerons(11,10,9,255); // Common Cat
  50     // Transizione: lampeggiatori sui PIN RGB
  51     Lampeggiatore sxLamp(10); // Lampeggiatore
  52     Lampeggiatore dxLamp(9); // Lampeggiatore
  53
  54
  55 // Variabili per lettura canale servo
  56 byte ailPin = 3; // Calibrazione
  57
  58 // Vars Alettoni
  59 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  60 const int deviation = 50 ; // deviazione dal punto medio
  61         //per entrare nello stato successivo dal centro
  62
  63
  64 // FSM gestione alettoni
  65 enum  { // Stati della FMS
  66     middle,   // centrale
  67     sxin,     // transizione a sx
  68     sx,       // sx
  69     dxin,     // transizione a dx
  70     dx        // dx
  71 } ailstate  = middle;
  72
  73 // Vars FSM
  74 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  75 unsigned long pausa = 600;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  76
  77 // Variabili comuni:
  78 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  79
  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             ailerons.Off();
 111             FSM_lastMillis = currentMillis;
 112         }
 113         else if (ail < mid_point - deviation - deviation / 3) {
 114             ailstate = dxin;
 115             ailerons.Off();
 116             FSM_lastMillis = currentMillis ;
 117         } ;
 118         break;
 119
 120     case sxin:
 121         // Transizione a sx
 122         sxLamp.Blink(150);
 123         if (currentMillis - pausa > FSM_lastMillis ) {
 124             ailstate = sx;
 125         }
 126         break;
 127
 128     case sx:
 129         ailerons.Green();
 130         if (ail < mid_point + deviation) {
 131             ailstate = middle;
 132         }
 133         else if (ail < mid_point - deviation) {
 134             FSM_lastMillis = currentMillis;
 135             ailstate = dxin;
 136         } ;
 137         break;
 138
 139     case dxin:
 140         // Transizione a dx
 141         dxLamp.Blink(150);
 142         if (currentMillis - pausa > FSM_lastMillis ) {
 143             ailstate = dx;
 144         }
 145         break;
 146
 147     case dx:
 148         ailerons.Blue();
 149         if (ail > mid_point - deviation) {
 150             ailstate = middle;
 151         }
 152         else if (ail > mid_point + deviation) {
 153             FSM_lastMillis = currentMillis;
 154             ailstate = dxin;
 155         } ;
 156         break;
 157     }
 158
 159 // PWM Motore
 160 motore.lSet((thr - 980) / 4); // 980 = minimo
 161
 162 #ifdef DEBUG
 163 Serial.print((thr - 980) / 4);
 164     Serial.print("\tail: ");
 165     Serial.print(ail);
 166     Serial.print("\t ailstate:");
 167     Serial.println(ailstate);
 168 #endif
 169 }
 170 // ISRs
 171 void chRise2() {
 172     attachInterrupt(0, chFall2, FALLING);
 173     chStart2 = micros();
 174 }
 175
 176 void chFall2() {
 177     attachInterrupt(0, chRise2, RISING);
 178     thr = micros() - chStart2;
 179 }
 180 // Seconod iterrupt
 181 void chRise3() {
 182     attachInterrupt(1, chFall3, FALLING);
 183     chStart3 = micros();
 184 }
 185
 186 void chFall3() {
 187     attachInterrupt(1, chRise3, RISING);
 188     ail = micros() - chStart3;
 189 }