]> git.piffa.net Git - aerei/blob - aerei/antonino/bugatti_fsm/bugatti_fsm.ino
5099f7f0635002a44bbc19e53a97e38a7ce595ca
[aerei] / 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 }