git.piffa.net Git - aerei/blob - aerei/antonino/bugatti_fsm_prot/bugatti_fsm_prot.ino

   1 /* Bugatti di Antonino
   2
   3 Outputs:
   4    2 LED / Strisce laterali che lampeggiano alternativamente
   5    1 LED in PWM per il motore
   6    1 Striscia RGB sotto per tutta la lunghezza delle ali
   7
   8 Inputs:
   9    Lettura del canale Throttle (3) con la funzione Pulsein
  10    Lettura alettoni con interrupt 0 (PIN2)
  11
  12 */
  13
  14 #include <common.h>
  15 #define DEBUG
  16
  17 // LED disponibili
  18 Lampeggiatore left = 7;
  19 Lampeggiatore right = 8;
  20 Pwm motore = 3;
  21
  22 // RGB
  23 RGBLed ailerons(11,10,9,255); // Common Cat
  24     // Transizione: lampeggiatori sui PIN RGB
  25     Lampeggiatore sxLamp(10); // Lampeggiatore
  26     Lampeggiatore dxLamp(9); // Lampeggiatore
  27 //Pwm rsotto = 6;
  28 //Pwm gsotto = 5;
  29 //Pwm bsotto = 3;
  30
  31
  32 // Var thr
  33 //////////////// !!!! cambiare thrIn
  34 const byte thrPin = A5; // PIN collegato al CH3
  35 byte thr ;  // Throttle a 8bit
  36 int thrIn ; // Valore del th in ingresso dal servo
  37
  38 // Variabili per interrupt 0 su PIN 2
  39 volatile unsigned int ail = 1500; // Valore computato
  40 volatile unsigned int chStart2 = 1500; // Inizio rilevamento
  41
  42 // Variabili per autocalibrazione 0
  43 const byte chPin2 = 2; // PIN per la calibrazione
  44 int mid_point2 = 1500;
  45
  46 // Vars Alettoni
  47 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  48 const int deviation = 50 ; // deviazione dal punto medio
  49         //per entrare nello stato successivo dal centro
  50
  51
  52 // FSM gestione alettoni
  53 enum  { // Stati della FMS
  54     middle,   // centrale
  55     sxin,     // transizione a sx
  56     sx,       // sx
  57     dxin,     // transizione a dx
  58     dx        // dx
  59 } ailstate  = middle;
  60
  61 // Vars FSM
  62 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  63 unsigned long pausa = 600;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  64
  65 // Variabili comuni:
  66 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  67 byte caso ; // Valore random
  68
  69
  70 void setup() {
  71   // I PINs vengono impostati dal constructor al momento
  72   // della dichiarazione dell'ogetto.
  73   pinMode(thrPin,INPUT);
  74   right.Invert() ;  // Opzionale: inverte l'ordine del lampeggio da
  75
  76   attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
  77
  78   randomSeed(analogRead(0));
  79
  80   // Test iniziale dei LED per verifica contatti:
  81   left.High();
  82   right.High();
  83   ailerons.White();
  84   motore.Set(255);
  85   delay(4000);
  86
  87
  88 #ifdef DEBUG
  89    Serial.begin(9600);
  90 #endif
  91 }
  92
  93 void loop() {
  94 currentMillis = millis(); // Timestamp per tutto il loop
  95
  96 // Lettura CH3
  97   thrIn = pulseIn(thrPin, HIGH, 25000);
  98   // Hint: thrIn andrebbe calibrato son un Serial.write
  99   if (thrIn != 0) {
 100 thr = map(thrIn, 960, 2000, 0, 255);
 101 };
 102
 103 // Gestione throttle
 104   if (thr >= 0 && thr < 15) {
 105     // IDLE
 106
 107     right.Blink();
 108     left.Blink();
 109     motore.UD(2000);
 110   } else if (thr < 245) {
 111     // Throttle medio
 112
 113     right.Blink(1120 - 4 * thr );
 114     left.Blink(1120 - 4 * thr );
 115     motore.lSet(thr);   // Luminosita' proporzionale al throttle
 116   } else {
 117     // Throttle al massimo: LED laterali lampeggiano a caso,
 118     // Sotto luminosita' a caso
 119
 120     caso = random(20, 240) ;
 121     right.Swap();
 122     left.Swap();
 123     motore.lSet(caso);
 124     delay(caso);
 125   }
 126
 127
 128
 129   //// Ailerons:
 130  switch (ailstate) {
 131     case middle:
 132         ailerons.White();
 133         // Alettoni piatti
 134         if (ail > mid_point + deviation + deviation /3) {
 135             // extra margine per avere un po' di gioco
 136             ailstate = sxin;
 137             ailerons.Off();
 138             FSM_lastMillis = currentMillis;
 139         }
 140         else if (ail < mid_point - deviation - deviation / 3) {
 141             ailstate = dxin;
 142             ailerons.Off();
 143             FSM_lastMillis = currentMillis ;
 144         } ;
 145         break;
 146
 147     case sxin:
 148         // Transizione a sx
 149         sxLamp.Blink(150);
 150         if (currentMillis - pausa > FSM_lastMillis ) {
 151             ailstate = sx;
 152         }
 153         break;
 154
 155     case sx:
 156         ailerons.Green();
 157         if (ail < mid_point + deviation) {
 158             ailstate = middle;
 159         }
 160         else if (ail < mid_point - deviation) {
 161             FSM_lastMillis = currentMillis;
 162             ailstate = dxin;
 163         } ;
 164         break;
 165
 166     case dxin:
 167         // Transizione a dx
 168         dxLamp.Blink(150);
 169         if (currentMillis - pausa > FSM_lastMillis ) {
 170             ailstate = dx;
 171         }
 172         break;
 173
 174     case dx:
 175         ailerons.Blue();
 176         if (ail > mid_point - deviation) {
 177             ailstate = middle;
 178         }
 179         else if (ail > mid_point + deviation) {
 180             FSM_lastMillis = currentMillis;
 181             ailstate = dxin;
 182         } ;
 183         break;
 184     }
 185
 186 #ifdef DEBUG
 187 Serial.print(thrIn);
 188     Serial.print("\tthr: ");
 189 Serial.print(thr);
 190     Serial.print("\tail: ");
 191     Serial.print(ail);
 192     Serial.print("\t ailstate:");
 193     Serial.println(ailstate);
 194 #endif
 195 }
 196 // ISRs
 197 void chRise2() {
 198     attachInterrupt(0, chFall2, FALLING);
 199     chStart2 = micros();
 200 }
 201
 202 void chFall2() {
 203     attachInterrupt(0, chRise2, RISING);
 204     ail = micros() - chStart2;
 205 }