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