git.piffa.net Git - aerei/blob - aerei/antonino/bugatti_fsm2/bugatti_fsm2.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 = 1432 ; // 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, 870, 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             FSM_lastMillis = currentMillis;
 147         }
 148         else if (ail < mid_point - deviation - deviation / 3) {
 149             ailstate = dxin;
 150             ailerons.Off();
 151             FSM_lastMillis = currentMillis ;
 152         } ;
 153         break;
 154
 155     case sxin:
 156         // Transizione a sx
 157         sxLamp.Blink(150);
 158         if (currentMillis - pausa > FSM_lastMillis ) {
 159             ailstate = sx;
 160         }
 161         break;
 162 case sx:
 163         ailerons.Green();
 164         if (ail < mid_point + deviation) {
 165             ailstate = middle;
 166         }
 167         else if (ail < mid_point - deviation) {
 168             FSM_lastMillis = currentMillis;
 169             ailstate = dxin;
 170         } ;
 171         break;
 172
 173     case dxin:
 174         // Transizione a dx
 175         dxLamp.Blink(150);
 176         if (currentMillis - pausa > FSM_lastMillis ) {
 177             ailstate = dx;
 178         }
 179         break;
 180
 181     case dx:
 182         ailerons.Blue();
 183         if (ail > mid_point - deviation) {
 184             ailstate = middle;
 185         }
 186         else if (ail > mid_point + deviation) {
 187             FSM_lastMillis = currentMillis;
 188             ailstate = dxin;
 189         } ;
 190         break;
 191     }
 192
 193
 194 #ifdef DEBUG
 195 Serial.print(thrIn);
 196     Serial.print("\tthr: ");
 197 Serial.print(thr);
 198     Serial.print("\tail: ");
 199     Serial.print(ail);
 200     Serial.print("\t ailstate:");
 201     Serial.println(ailstate);
 202 #endif
 203 }
 204 // ISRs
 205 void chRise2() {
 206     attachInterrupt(0, chFall2, FALLING);
 207     chStart2 = micros();
 208 }
 209
 210 void chFall2() {
 211     attachInterrupt(0, chRise2, RISING);
 212     ail = micros() - chStart2;
 213 }