git.piffa.net Git - aerei/blob - aerei/antonino/bugatti_fsm/bugatti_fsm.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   } else if (thr < 245) {
 119     // Throttle medio
 120
 121     right.Blink(1120 - 4 * thr );
 122     left.Blink(1120 - 4 * thr );
 123     motore.lSet(thr);   // Luminosita' proporzionale al throttle
 124   } else {
 125     // Throttle al massimo: LED laterali lampeggiano a caso,
 126     // Sotto luminosita' a caso
 127
 128     caso = random(20, 240) ;
 129     right.Swap();
 130     left.Swap();
 131     motore.lSet(caso);
 132 // TODO: check this
 133         ailerons.Rand();
 134     delay(caso);
 135   }
 136
 137
 138
 139   //// Ailerons:
 140  switch (ailstate) {
 141     case middle:
 142         ailerons.White();
 143         // Alettoni piatti
 144         if (ail > mid_point + deviation + deviation /3) {
 145             // extra margine per avere un po' di gioco
 146             ailstate = sxin;
 147             ailerons.Off();
 148             FSM_lastMillis = currentMillis;
 149         }
 150         else if (ail < mid_point - deviation - deviation / 3) {
 151             ailstate = dxin;
 152             ailerons.Off();
 153             FSM_lastMillis = currentMillis ;
 154         } ;
 155         break;
 156
 157     case sxin:
 158         // Transizione a sx
 159         sxLamp.Blink(150);
 160         if (currentMillis - pausa > FSM_lastMillis ) {
 161             ailstate = sx;
 162         }
 163         break;
 164
 165     case sx:
 166         ailerons.Green();
 167         if (ail < mid_point + deviation) {
 168             ailstate = middle;
 169         }
 170         else if (ail < mid_point - deviation) {
 171             FSM_lastMillis = currentMillis;
 172             ailstate = dxin;
 173         } ;
 174         break;
 175
 176     case dxin:
 177         // Transizione a dx
 178         dxLamp.Blink(150);
 179         if (currentMillis - pausa > FSM_lastMillis ) {
 180             ailstate = dx;
 181         }
 182         break;
 183
 184     case dx:
 185         ailerons.Blue();
 186         if (ail > mid_point - deviation) {
 187             ailstate = middle;
 188         }
 189         else if (ail > mid_point + deviation) {
 190             FSM_lastMillis = currentMillis;
 191             ailstate = dxin;
 192         } ;
 193         break;
 194     }
 195
 196 #ifdef DEBUG
 197 Serial.print(thrIn);
 198     Serial.print("\tthr: ");
 199 Serial.print(thr);
 200     Serial.print("\tail: ");
 201     Serial.print(ail);
 202     Serial.print("\t ailstate:");
 203     Serial.println(ailstate);
 204 #endif
 205 }
 206 // ISRs
 207 void chRise2() {
 208     attachInterrupt(0, chFall2, FALLING);
 209     chStart2 = micros();
 210 }
 211
 212 void chFall2() {
 213     attachInterrupt(0, chRise2, RISING);
 214     ail = micros() - chStart2;
 215 }