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 left.Blink(map(chValue2,1000,2000,200,800 );
  79 right.Blink(map(chValue2,1000,2000,800,200 );
  80 //codasx.Blink();
  81 //codadx.Blink();
  82
  83 // Gestione throttle
  84     if (chValue3 < 1050) {
  85         // IDLE
  86 //pleft.Up(1000);
  87 //pright.Up(1000);
  88 pcodasx.UD(2000);
  89 pcodadx.UD(2000);
  90
  91     }
  92     else if (chValue3 > 1900) {
  93         // Throttle al massimo: LED laterali lampeggiano a caso,
  94         // Sotto luminosita' a caso
  95         caso = random(30, 250) ;
  96 codasx.Swap();
  97 codadx.Swap();
  98         delay(caso);
  99     }
 100     else {
 101         // Throttle medio
 102         thrBit = map(chValue3,1050, 1900, 0, 255);
 103         codasx.Blink(1220 - 4 * thrBit );
 104         codadx.Blink(1220 - 4 * thrBit );
 105                 //left.Blink(chValue2 - 300);
 106                 //right.Blink(chValue2 - 300);
 107     }
 108 #ifdef DEBUG
 109     Serial.print("PIN2: ");
 110     Serial.print(chValue2);
 111     Serial.print(" -base: ");
 112     Serial.print(mid_point2);
 113
 114     Serial.print(" |-| PIN3:");
 115     Serial.print(chValue3);
 116     Serial.print(" -base: ");
 117     Serial.println(mid_point3);
 118     delay(200);
 119 #endif
 120 }
 121 // Functions
 122 void chRise2() {
 123     attachInterrupt(0, chFall2, FALLING);
 124     chStart2 = micros();
 125 }
 126
 127 void chFall2() {
 128     attachInterrupt(0, chRise2, RISING);
 129     chValue2 = micros() - chStart2;
 130 }
 131 // Seconod iterrupt
 132 void chRise3() {
 133     attachInterrupt(1, chFall3, FALLING);
 134     chStart3 = micros();
 135 }
 136
 137 void chFall3() {
 138     attachInterrupt(1, chRise3, RISING);
 139     chValue3 = micros() - chStart3;
 140 }