git.piffa.net Git - aerei/blob - aerei/daniele/fsm_switch_trans/fsm_switch_trans.ino

   1 /* Aereo di Daniele
   2
   3 FSM: il throttle e' a posto
   4 Prototipo: F8 Bearcat
   5
   6 Output:
   7    2 LED PWM ai lati con lampeggio alternato
   8    2 LED PWM alle estremita ali
   9
  10 Input:
  11     2 interrupts per th e alettone
  12     PIN 2:  alettone
  13     PIN 3:  throttle
  14
  15
  16     TODO
  17 * Vedere la calibrazione automatica
  18 * Min e max a 1000 - 2000 per alettone
  19
  20 TODO:
  21 Aggiungere FSM per alettone: lampeggi alternati
  22 in base a chValue2
  23 */
  24
  25 #include <common.h>
  26 # define DEBUG
  27
  28 // Instanziamo un LED fuori dal loop
  29 Lampeggiatore left = 6;
  30 Lampeggiatore right = 9;
  31 Lampeggiatore codasx = 5;
  32 Lampeggiatore codadx = 10;
  33
  34 Pwm pleft = 6;
  35 Pwm pright = 9;
  36 Pwm pcodasx = 5;
  37 Pwm pcodadx = 10;
  38
  39 // Variabili per interrupt 0 si PIN 2
  40 volatile unsigned int chValue2 = 1500; // Valore computato
  41 volatile unsigned int chStart2 = 1500; // Inizio rilevamento
  42
  43 // Variabili per interrupt 1 su PIN 3
  44 volatile unsigned int chValue3 = 1500; // Valore computato
  45 volatile unsigned int chStart3 = 1500; // Inizio rilevamento
  46
  47 // Variabili per autocalibrazione 0
  48 const byte chPin2 = 2; // PIN per la calibrazione alettone
  49 int mid_point2 = 1500;
  50
  51 // Variabili per autocalibrazione 1
  52 const byte chPin3 = 3; // PIN per la calibrazione
  53 int mid_point3 = 1000;
  54
  55 // Variabili
  56 int caso ;
  57 int thrBit;
  58 unsigned long pausa = 2000;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  59
  60 // Variabili
  61 const byte chPin = A1; // PIN su cui e' collegato il canale
  62 unsigned int chIn = 1500; // Valore catturato
  63 long unsigned chStamp = 0; // Timestamp per
  64 unsigned int freq = 400 ; // Ogni quanti millisecondi leggere il valore
  65 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  66 const int soglia = 1500; // soglia per scatto toggle a 2 posizioni
  67 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  68
  69
  70 // FSM gestione interruttore luci
  71 enum  { // Stati della FMS
  72     On,     // Acceso
  73     toOff,  // Trans On -> Off
  74     Off,    // Spento
  75     toOn    // Trans OFF -> On
  76 } toggle  = Off;
  77
  78 void setup() {
  79   // I PINs vengono impostati dal constructor al momento
  80   // della dichiarazione dell'ogetto.
  81 right.Invert();
  82 codadx.Invert();
  83
  84   // HI -> LOW --> LOW -> HI
  85   // per avere 2 LED che lampeggiano alternativamente
  86     // Funzione relativa a calibrazione con pulsein:
  87     mid_point2 =  calibraTrim(chPin2) ; // Calibrazione del TRIM attivo sul canale
  88     //mid_point3 =  calibraTrim(chPin3) ; // Calibrazione del TRIM attivo sul canale
  89     attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
  90     attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168
  91 #ifdef dEBUG
  92 Serial.begin(9600);
  93 #endif
  94 }
  95
  96 void loop() {
  97 currentMillis = millis(); // Timestamp per tutto il loop
  98
  99 //codasx.Blink();
 100 //codadx.Blink();
 101 // Lettura ailerons channel ogni 200ms
 102     if (currentMillis - chStamp >= freq) {
 103
 104         chIn = pulseIn(chPin, HIGH, 25000);
 105         if (chIn != 0 && chIn > 1000 && chIn <2000)  {
 106             // get only resonable values
 107             chStamp = currentMillis ;
 108         } ;
 109     };
 110
 111 switch (toggle) {
 112     case Off:
 113     // Spento
 114     pcodasx.Set(255) ;
 115     pcodadx.Set(255) ;
 116     pleft.Set(255) ;
 117     pright.Set(255) ;
 118         if (chIn > soglia) {
 119             FSM_lastMillis = currentMillis;
 120             toggle = toOn ;
 121         }
 122         break;
 123
 124 // Interruttore on / off
 125
 126     case On:
 127     // Acceso
 128 // Gestione throttle
 129     if (chValue3 < 1050) {
 130         // IDLE
 131 //pleft.Up(1000);
 132 //pright.Up(1000);
 133 pcodasx.UD(2000);
 134 pcodadx.UD(2000);
 135 pleft.lUp(1000);
 136 pright.lDown(1000);
 137
 138
 139     }
 140     else if (chValue3 > 1900) {
 141         // Throttle al massimo: LED laterali lampeggiano a caso,
 142         // Sotto luminosita' a caso
 143         caso = random(30, 250) ;
 144 pleft.Set(caso);
 145 pright.Set(caso);
 146 pcodasx.Set(caso);
 147 pcodadx.Set(caso);
 148         delay(caso);
 149     }
 150     else {
 151         // Throttle medio
 152         thrBit = map(chValue3,1050, 1900, 0, 255);
 153         codasx.Blink(1220 - 4 * thrBit );
 154         codadx.Blink(1220 - 4 * thrBit );
 155 left.Blink(map(chValue2,1000,2000,200,800 ));
 156 right.Blink(map(chValue2,1000,2000,800,200 ));
 157     }
 158 break;
 159
 160
 161     case toOn:
 162     // Trans off -> on
 163 pleft.lUp(pausa);
 164 pright.lUp(pausa);
 165 pcodasx.lUp(pausa);
 166 pcodadx.lUp(pausa);
 167
 168         if (chIn > soglia && currentMillis - pausa > FSM_lastMillis ) {
 169             toggle = On ;
 170         } else if  (chIn <= soglia) {
 171             toggle = Off ;
 172         }
 173         break;
 174
 175     case toOff:
 176     // Trans on -> off
 177 pleft.lDown(pausa);
 178 pright.lDown(pausa);
 179 pcodasx.lDown(pausa);
 180 pcodadx.lDown(pausa);
 181
 182         if (chIn <= soglia && currentMillis - pausa > FSM_lastMillis ) {
 183             toggle = Off ;
 184         } else if  (chIn > soglia) {
 185             toggle = On ;
 186         }
 187         break;
 188 #ifdef DEBUG
 189     Serial.print("PIN2: ");
 190     Serial.print(chValue2);
 191     Serial.print(" -base: ");
 192     Serial.print(mid_point2);
 193
 194     Serial.print(" |-| PIN3:");
 195     Serial.print(chValue3);
 196     Serial.print(" -base: ");
 197     Serial.print(mid_point3);
 198     Serial.print(" switch: ");
 199     Serial.println(chIn);
 200 #endif
 201 }
 202 }
 203
 204
 205
 206
 207
 208 // Functions
 209 void chRise2() {
 210     attachInterrupt(0, chFall2, FALLING);
 211     chStart2 = micros();
 212 }
 213
 214 void chFall2() {
 215     attachInterrupt(0, chRise2, RISING);
 216     chValue2 = micros() - chStart2;
 217 }
 218 // Seconod iterrupt
 219 void chRise3() {
 220     attachInterrupt(1, chFall3, FALLING);
 221     chStart3 = micros();
 222 }
 223
 224 void chFall3() {
 225     attachInterrupt(1, chRise3, RISING);
 226     chValue3 = micros() - chStart3;
 227 }