git.piffa.net Git - aerei/blob - esempi/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 = 1500; // 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
  47 // Variabili per autocalibrazione 0
  48 const byte chPin2 = 3; // PIN per la calibrazione
  49 int mid_point2 = 1500;
  50
  51 // LEDs:
  52 Pwm motore = 11;
  53
  54 // LED RGB alettoni
  55 RGBLed ailerons(5,6,9,255); // Common Cat
  56 // Transizione: Pwm
  57 Lampeggiatore sxLamp(6); // Lampeggiatore
  58 Lampeggiatore dxLamp(9); // Lampeggiatore
  59
  60
  61 // Variabili per lettura canale servo
  62 byte ailPin = 3; // Calibrazione
  63
  64 // Vars Alettoni
  65 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  66 const int deviation = 50 ; // deviazione dal punto medio
  67         //per entrare nello stato successivo dal centro
  68
  69 // Led motore e altri:
  70 Lampeggiatore left = 10;
  71 Lampeggiatore right = 12;
  72 Pwm sotto = 9;
  73
  74 // Quando il Throttle e' in IDE facciamo un PWM anche sui laterali
  75 Pwm lpwm = 10 ;
  76 Pwm rpwm = 12;
  77
  78 // Variabili:
  79 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  80 byte caso; // Random var
  81 byte thrBit ; // Valore a 8bit per il throttle
  82
  83
  84
  85
  86 // FSM gestione alettoni
  87 enum  { // Stati della FMS
  88     middle,   // centrale
  89     sxin,     // transizione a sx
  90     sx,       // sx
  91     dxin,     // transizione a dx
  92     dx        // dx
  93 } ailstate  = middle;
  94
  95 // Vars FSM
  96 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  97 unsigned long pausa = 500;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  98
  99 ///////////////////////////////////////////////////////////
 100 void setup() {
 101
 102
 103 #ifdef DEBUG
 104    Serial.begin(9600);
 105 #endif
 106
 107     attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
 108     attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168
 109
 110
 111 // Funzione relativa a calibrazione:
 112 mid_point =  calibraTrim(ailPin) + 10 ; // + LED di servizio per monitor calibrazione
 113 //Serial.print(mid_point);
 114 //while(1) {
 115 //}
 116 }
 117
 118 void loop() {
 119     currentMillis = millis(); // Timestamp per tutto il loop
 120
 121     switch (ailstate) {
 122     case middle:
 123         ailerons.White();
 124         // Alettoni piatti
 125         if (ail > mid_point + deviation + deviation /3) {
 126             // extra margine per avere un po' di gioco
 127             ailstate = sxin;
 128             FSM_lastMillis = currentMillis;
 129         }
 130         else if (ail < mid_point - deviation - deviation / 3) {
 131             ailstate = dxin;
 132             FSM_lastMillis = currentMillis ;
 133         } ;
 134         break;
 135
 136     case sxin:
 137         // Transizione a sx
 138         sxLamp.Blink(200);
 139         if (currentMillis - pausa > FSM_lastMillis ) {
 140             ailstate = sx;
 141         }
 142         break;
 143
 144     case sx:
 145         ailerons.Green();
 146         if (ail < mid_point + deviation) {
 147             ailstate = middle;
 148         }
 149         else if (ail < mid_point - deviation) {
 150             FSM_lastMillis = currentMillis;
 151             ailstate = dxin;
 152         } ;
 153         break;
 154
 155     case dxin:
 156         // Transizione a dx
 157         dxLamp.Blink(200);
 158         if (currentMillis - pausa > FSM_lastMillis ) {
 159             ailstate = dx;
 160         }
 161         break;
 162
 163     case dx:
 164         ailerons.Blue();
 165         if (ail > mid_point - deviation) {
 166             ailstate = middle;
 167         }
 168         else if (ail > mid_point + deviation) {
 169             FSM_lastMillis = currentMillis;
 170             ailstate = dxin;
 171         } ;
 172         break;
 173     }
 174
 175 // Gestione throttle
 176     if (thr < 1050) {
 177         // IDLE
 178         rpwm.UD(2000);
 179         lpwm.UD(2000);
 180         motore.lDown(1500);
 181     }
 182     else if (thr > 1900) {
 183         // Throttle al massimo: LED laterali lampeggiano a caso,
 184         // Sotto luminosita' a caso
 185         caso = random(30, 250) ;
 186         right.Swap();
 187         left.Swap();
 188         motore.lSet(caso);
 189         delay(caso); // Blocking!
 190     }
 191     else {
 192         // Throttle medio
 193         thrBit = map(thr,1050, 1900, 0, 255);
 194         right.Blink(1220 - 4 * thrBit );
 195         left.Blink(1220 - 4 * thrBit );
 196         motore.lSet(thrBit);   // Luminosita' proporzionale al throttle
 197     }
 198
 199
 200 #ifdef DEBUG
 201 Serial.print((thr - 980) / 4);
 202     Serial.print("\tail: ");
 203     Serial.print(ail);
 204     Serial.print("\t ailstate:");
 205     Serial.println(ailstate);
 206 #endif
 207 }
 208
 209
 210 // Functions
 211 void chRise2() {
 212     attachInterrupt(0, chFall2, FALLING);
 213     chStart2 = micros();
 214 }
 215
 216 void chFall2() {
 217     attachInterrupt(0, chRise2, RISING);
 218     thr = micros() - chStart2;
 219 }
 220 // Seconod iterrupt
 221 void chRise3() {
 222     attachInterrupt(1, chFall3, FALLING);
 223     chStart3 = micros();
 224 }
 225
 226 void chFall3() {
 227     attachInterrupt(1, chRise3, RISING);
 228     ail = micros() - chStart3;
 229 }