git.piffa.net Git - aerei/blob - aerei/daniele/fsm/fsm.ino

   1 /* Aereo di Daniele
   2
   3 FSM: il throttle e' a posto
   4 Prototipo: F8 Bearcat
   5
   6 Output:
   7    2 LED PWM ai lati con lampeggio alternato
   8    2 LED PWM alle estremita ali
   9
  10 Input:
  11     2 interrupts per th e alettone
  12     PIN 2:  alettone
  13     PIN 3:  throttle
  14
  15
  16     TODO
  17 * Vedere la calibrazione automatica
  18 * Min e max a 1000 - 2000 per alettone
  19
  20 TODO:
  21 Aggiungere FSM per alettone: lampeggi alternati
  22 in base a chValue2
  23 */
  24
  25 #include <common.h>
  26 # define dEBUG
  27
  28 // Instanziamo un LED fuori dal loop
  29 Lampeggiatore left = 6;
  30 Lampeggiatore right = 9;
  31 Lampeggiatore codasx = 5;
  32 Lampeggiatore codadx = 10;
  33
  34 Pwm pleft = 6;
  35 Pwm pright = 9;
  36 Pwm pcodasx = 5;
  37 Pwm pcodadx = 10;
  38
  39 // Variabili per interrupt 0 si PIN 2
  40 volatile unsigned int chValue2 = 1500; // Valore computato
  41 volatile unsigned int chStart2 = 1500; // Inizio rilevamento
  42
  43 // Variabili per interrupt 1 su PIN 3
  44 volatile unsigned int chValue3 = 1500; // Valore computato
  45 volatile unsigned int chStart3 = 1500; // Inizio rilevamento
  46
  47 // Variabili per autocalibrazione 0
  48 const byte chPin2 = 2; // PIN per la calibrazione alettone
  49 int mid_point2 = 1500;
  50
  51 // Variabili per autocalibrazione 1
  52 const byte chPin3 = 3; // PIN per la calibrazione
  53 int mid_point3 = 1000;
  54
  55 // Variabili
  56 int caso ;
  57 int thrBit;
  58
  59 void setup() {
  60   // I PINs vengono impostati dal constructor al momento
  61   // della dichiarazione dell'ogetto.
  62 right.Invert();
  63 codadx.Invert();
  64
  65   // HI -> LOW --> LOW -> HI
  66   // per avere 2 LED che lampeggiano alternativamente
  67     // Funzione relativa a calibrazione con pulsein:
  68     //mid_point2 =  calibraTrim(chPin2) ; // Calibrazione del TRIM attivo sul canale
  69     //mid_point3 =  calibraTrim(chPin3) ; // Calibrazione del TRIM attivo sul canale
  70     attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168
  71     attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168
  72 #ifdef DEBUG
  73 Serial.begin(9600);
  74 #endif
  75 }
  76
  77 void loop() {
  78 //codasx.Blink();
  79 //codadx.Blink();
  80
  81 // Gestione throttle
  82     if (chValue3 < 1050) {
  83         // IDLE
  84 //pleft.Up(1000);
  85 //pright.Up(1000);
  86 pcodasx.UD(2000);
  87 pcodadx.UD(2000);
  88 pleft.lUp(1000);
  89 pright.lDown(1000);
  90
  91
  92     }
  93     else if (chValue3 > 1900) {
  94         // Throttle al massimo: LED laterali lampeggiano a caso,
  95         // Sotto luminosita' a caso
  96         caso = random(30, 250) ;
  97 pleft.Set();
  98 pright.Set();
  99 pcodasx.Set();
 100 pcodadx.Set();
 101         delay(caso);
 102     }
 103     else {
 104         // Throttle medio
 105         thrBit = map(chValue3,1050, 1900, 0, 255);
 106         codasx.Blink(1220 - 4 * thrBit );
 107         codadx.Blink(1220 - 4 * thrBit );
 108 left.Blink(map(chValue2,1000,2000,200,800 ));
 109 right.Blink(map(chValue2,1000,2000,800,200 ));
 110     }
 111 #ifdef DEBUG
 112     Serial.print("PIN2: ");
 113     Serial.print(chValue2);
 114     Serial.print(" -base: ");
 115     Serial.print(mid_point2);
 116
 117     Serial.print(" |-| PIN3:");
 118     Serial.print(chValue3);
 119     Serial.print(" -base: ");
 120     Serial.println(mid_point3);
 121     delay(200);
 122 #endif
 123 }
 124 // Functions
 125 void chRise2() {
 126     attachInterrupt(0, chFall2, FALLING);
 127     chStart2 = micros();
 128 }
 129
 130 void chFall2() {
 131     attachInterrupt(0, chRise2, RISING);
 132     chValue2 = micros() - chStart2;
 133 }
 134 // Seconod iterrupt
 135 void chRise3() {
 136     attachInterrupt(1, chFall3, FALLING);
 137     chStart3 = micros();
 138 }
 139
 140 void chFall3() {
 141     attachInterrupt(1, chRise3, RISING);
 142     chValue3 = micros() - chStart3;
 143 }