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);
  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 = A5; // 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 // Variabili per autocalibrazione 0
  49 const byte chPin2 = 2; // PIN per la calibrazione
  50 int mid_point2 = 1500;
  51
  52 // Vars Alettoni
  53 int mid_point = 1560 ; // centro del segnale, trimmato nel setup
  54 const int deviation = 50 ; // deviazione dal punto medio
  55 //per entrare nello stato successivo dal centro
  56
  57
  58 // FSM gestione alettoni
  59 enum  { // Stati della FMS
  60     middle,   // centrale
  61     sxin,     // transizione a sx
  62     sx,       // sx
  63     dxin,     // transizione a dx
  64     dx        // dx
  65 } ailstate  = middle;
  66
  67 // Vars FSM
  68 unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni
  69 unsigned long pausa = 600;  // Pausa per la transizione durante gli stati 2, 4 della FSM
  70
  71 // Variabili comuni:
  72 unsigned long currentMillis; // timestamp reference per millis per tutto il loop
  73 byte caso ; // Valore random
  74
  75
  76 void setup() {
  77     // I PINs vengono impostati dal constructor al momento
  78     // della dichiarazione dell'ogetto.
  79     pinMode(thrPin,INPUT);
  80     right.Invert() ;  // Opzionale: inverte l'ordine del lampeggio da
  81
  82     attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
  83
  84     randomSeed(analogRead(0));
  85
  86     // Test iniziale dei LED per verifica contatti:
  87     left.High();
  88     right.High();
  89     ailerons.White();
  90     motore.Set(255);
  91     delay(4000);
  92
  93
  94     mid_point =  calibraTrim(chPin2) + 8 ; // + LED di servizio per monitor calibrazione
  95 #ifdef DEBUG
  96     Serial.begin(9600);
  97 #endif
  98 }
  99
 100 void loop() {
 101     currentMillis = millis(); // Timestamp per tutto il loop
 102
 103 // Lettura CH3
 104     thrIn = pulseIn(thrPin, HIGH, 25000);
 105     // Hint: thrIn andrebbe calibrato son un Serial.write
 106     if (thrIn != 0) {
 107         thr = map(thrIn, 960, 2000, 0, 255);
 108     };
 109
 110
 111 // Gestione throttle
 112     if (thr >= 0 && thr < 15) {
 113         // IDLE
 114
 115         right.Blink();
 116         left.Blink();
 117         motore.UD(2000);
 118         // RGB
 119         rsotto.lDown(3000);
 120         gsotto.lUp(1000);
 121         bsotto.lup(2000);
 122
 123
 124     } else if (thr < 245) {
 125         // Throttle medio
 126
 127         right.Blink(1120 - 4 * thr );
 128         left.Blink(1120 - 4 * thr );
 129         motore.lSet(thr);   // Luminosita' proporzionale al throttle
 130
 131         //// Ailerons:
 132         switch (ailstate) {
 133         case middle:
 134         //    ailerons.White();
 135         rsotto.UD(2000);
 136         gsotto.UD(2000);
 137         bsotto.UD(2000);
 138             // Alettoni piatti
 139             if (ail > mid_point + deviation + deviation /3) {
 140                 // extra margine per avere un po' di gioco
 141                 ailstate = sxin;
 142                 ailerons.Off();
 143                 FSM_lastMillis = currentMillis;
 144             }
 145             else if (ail < mid_point - deviation - deviation / 3) {
 146                 ailstate = dxin;
 147                 ailerons.Off();
 148                 FSM_lastMillis = currentMillis ;
 149             } ;
 150             break;
 151
 152         case sxin:
 153             // Transizione a sx
 154             sxLamp.Blink(150);
 155             if (currentMillis - pausa > FSM_lastMillis ) {
 156                 ailstate = sx;
 157             }
 158             break;
 159
 160         case sx:
 161             ailerons.Green();
 162             if (ail < mid_point + deviation) {
 163                 ailstate = middle;
 164             }
 165             else if (ail < mid_point - deviation) {
 166                 FSM_lastMillis = currentMillis;
 167                 ailstate = dxin;
 168             } ;
 169             break;
 170
 171         case dxin:
 172             // Transizione a dx
 173             dxLamp.Blink(150);
 174             if (currentMillis - pausa > FSM_lastMillis ) {
 175                 ailstate = dx;
 176             }
 177             break;
 178
 179         case dx:
 180             ailerons.Blue();
 181             if (ail > mid_point - deviation) {
 182                 ailstate = middle;
 183             }
 184             else if (ail > mid_point + deviation) {
 185                 FSM_lastMillis = currentMillis;
 186                 ailstate = dxin;
 187             } ;
 188             break;
 189         };
 190
 191     } else {
 192         // Throttle al massimo: LED laterali lampeggiano a caso,
 193         // Sotto luminosita' a caso
 194
 195         caso = random(20, 240) ;
 196         right.Swap();
 197         left.Swap();
 198         motore.lSet(caso);
 199 // TODO: check this
 200         ailerons.Rand();
 201         delay(caso);
 202     }
 203
 204
 205
 206
 207 #ifdef DEBUG
 208     Serial.print(thrIn);
 209     Serial.print("\tthr: ");
 210     Serial.print(thr);
 211     Serial.print("\tail: ");
 212     Serial.print(ail);
 213     Serial.print("\t ailstate:");
 214     Serial.println(ailstate);
 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 }