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