-/* Zeta test
+/* Zeta
-Nota: rovedere i PWM per come calcolano le pause:
+Nota: rivedere i PWM per come calcolano le pause:
non possono schendere sotto a 1ms
*/
states_available state ;
// Due LED con lampeggio alternato:
-Lampeggiatore right = 3;
+Lampeggiatore right = 3; // Lampeggiatori superiori a lato fusoliera
Lampeggiatore left = 5;
-const byte rtail = 6;
+const byte rtail = 6; // Luci del motore posteriore
const byte ltail = 9;
-const byte thrPin = A3;
+const byte thrPin = A3;
byte pausa ;
byte thr ;
int thrIn ;
-Pwm rwhite = 3;
-Pwm lwhite = 5;
-Pwm rtp = 6 ;
+Pwm rwhite = 3; // PWM sui lampeggiatori
+Pwm lwhite = 5;
+Pwm rtp = 6 ; // PWM sulla coda
Pwm ltp = 9 ;
RGBLed rgb(7,8,4,255);
thrIn = pulseIn(thrPin, HIGH, 25000);
thr = constrain(map(thrIn, 983, 2000, 0, 255), 0, 255) ;
- // Inserire necro delay
delay(10); // Se si abilita il serial debug
// togliere il delay
rgb.Green();
rwhite.UD(2000); // Utilizza il coseno
- lwhite.UD(2000); // Bisognerebbe evitare di calcolarlo 4 volte uguale
+ lwhite.UD(2000); // Bisognerebbe evitare di calcolarli 4 volte uguale
ltp.shift = 500;
ltp.lUp(1000);
rtp.lUp(1000);
- roll a sx
- roll a dx
-Trasizioni per accentuare il cambio di assetto (ad esempio con dei blink
-e introdurre un po' di inerzia temporale per il cambio di stato
+NOTE: uso di goto all'interno dell FSM.
+
+TODO:
+
+* clean up magic numbers
*/
#include <common.h>
enum { // Stati della FMS
- middle, // centrale
- sxin, // transizione a sx
- sx, // sx
- dxin, // transizione a dx
- dx // dx
+ middle, // centrale
+ sxin, // transizione a sx
+ sx, // sx
+ dxin, // transizione a dx
+ dx // dx
} ailstate = middle;
// Un LED RGB
const byte ailPin = A4;
int ail ; // Valore a 8bit per ailerons
-int ailIn ; // Valore rilevato del 4 Ch della RX
+int ailIn ; // Valore rilevato del 4 Ch della RX
+unsigned long now; // timestamp reference for millis
+unsigned long pausa = 1000;
-void setup() {
-/* Bisognerebbe introdurre una calibrazione per compensare i TRIM
- ed eventualmente i dual rates.
-- attivarla se allo start un ale e' al massimo
-- fargli leggere i valori massimi
-- salvarli in eprom
-- per i dual rates: si potrebbe intercettare valori oltre al max
- e in base a questi traslare le soglie automaticamente
-
-Hint: leggere la soglia di rollio significativo in volo
- e inserirla nei riferimenti.
-*/
+int mid_point = 1500 ; // centro del segnale, trimmato nel setup
+const int deviation = 50 ; // deviazione per entrare nello stato succiessivo
- Serial.begin(9600);
+void setup() {
+ /* Bisognerebbe introdurre una calibrazione per compensare i TRIM
+ ed eventualmente i dual rates.
+
+ - attivarla se allo start un ale e' al massimo
+ - fargli leggere i valori massimi
+ - salvarli in eprom
+ - per i dual rates: si potrebbe intercettare valori oltre al max
+ e in base a questi traslare le soglie automaticamente
+
+ Hint: leggere la soglia di rollio significativo in volo
+ e inserirla nei riferimenti.
+ */
+
+ Serial.begin(9600);
+
+/* START Calibrazione TRIM canale:
+ Lettura di 10 smaple
+ calcolo del valore medio esclusi gli 0
+ */
+ byte a = 0;
+ Serial.println(">> Calibrazione: ");
+ while (a < 10) {
+ ailIn = pulseIn(ailPin, HIGH, 25000);
+ if (ailIn != 0 ) {
+ ail = ail + ailIn ;
+ a++ ;
+ Serial.print(a);
+ Serial.print(": ");
+ Serial.println(ail);
+ digitalWrite(13, !digitalRead(13));
+ delay(10);
+ }
+ }
+ mid_point = ail / 10 ;
+ Serial.print(">> Fine Calibrazione: ");
+ Serial.print(mid_point);
+ Serial.println("--");
+// END calibrazione
+
+ // Funzione relativa a calibrazione:
+//mid_point = calibraTrim(ailPin) ; // Pin a cui e' collegato il canale
+//mid_point = calibraTrim(aliPin,11) ; // + LED di servizio per calibrazione
}
void loop() {
- // Lettura Aileron channel: FAKE con un potenziometro
- //ailIn = analogRead(3);
- //ail = constrain(aliIn / 4 , 0, 255) ;
-
- // Lettura ailerons channel
- ailIn = pulseIn(ailPin, HIGH, 25000);
- if (ailIn != 0) {ail = constrain(ailIn, 1000, 2000); } ;
- // con un altra ricevente, fare una calibrazione nel caso.
- // Middle = 1512
-
-
- switch (ailstate) {
- case middle:
- // Alettoni piatti
- if (ail > 1600) { ailstate = sxin; } ;
- if (ail < 1400) { ailstate = dxin; } ;
- ailerons.White();
-
- break;
-
- case sxin:
- // Transizione a sx
- ailerons.Off();
- delay(1000);
- ailstate = sx;
- break;
-
- case sx:
- // dx
- ailerons.Green();
- if (ail < 1600) { ailstate = middle; } ;
- if (ail < 1400) { ailstate = dxin; } ;
- break;
-
- case dxin:
- // Transizione a dx
- ailerons.Off();
- delay(1000);
- ailstate = dx;
- break;
-
- case dx:
- // sx
- ailerons.Blue();
- if (ail > 1400) { ailstate = middle; } ;
- if (ail > 1600) { ailstate = dxin; } ;
- break;
- }
-
- Serial.print("ailIn: ");
- Serial.print(ailIn);
- Serial.print("\tail: ");
- Serial.print(ail);
- Serial.print("\t ailstate:");
- Serial.println(ailstate);
- // delay(200);
- }
+// Lettura Aileron channel: FAKE con un potenziometro 10K
+//ailIn = analogRead(3);
+//ail = constrain(aliIn * 2 , 0, 2000) ;
+
+// Lettura ailerons channel
+ ailIn = pulseIn(ailPin, HIGH, 25000);
+ if (ailIn != 0) {
+ ail = constrain(ailIn, 1000, 2000);
+ } ;
+ // con un altra ricevente, fare una calibrazione nel caso.
+ // Middle = 1512
+
+
+ switch (ailstate) {
+ case middle:
+ // Alettoni piatti
+ if (ail > mid_point + deviation + deviation /3) {
+ ailstate = sxin;
+ now = millis() ;
+ goto sxin ;
+ }
+ else if (ail < mid_point - deviation - deviation / 3) {
+ ailstate = dxin;
+ now = millis() ;
+ goto dxin ;
+ } ;
+ ailerons.White();
+
+ break;
+
+ case sxin:
+sxin:
+ // Transizione a sx
+ ailerons.Off();
+ if (millis() - pausa > now ) {
+ ailstate = sx;
+ }
+ break;
+
+ case sx:
+ // dx
+ ailerons.Green();
+ if (ail < mid_point + deviation) {
+ ailstate = middle;
+ }
+ else if (ail < mid_point - deviation) {
+ ailstate = dxin;
+ } ;
+ break;
+
+ case dxin:
+ // Transizione a dx
+dxin:
+ ailerons.Off();
+ if (millis() - pausa > now ) {
+ ailstate = dx;
+ }
+ break;
+
+ case dx:
+ // sx
+ ailerons.Blue();
+ if (ail > mid_point - deviation) {
+ ailstate = middle;
+ }
+ else if (ail > mid_point + deviation) {
+ ailstate = dxin;
+ } ;
+ break;
+ }
+
+ Serial.print("ailIn: ");
+ Serial.print(ailIn);
+ Serial.print("\tail: ");
+ Serial.print(ail);
+ Serial.print("\t ailstate:");
+ Serial.println(ailstate);
+ // delay(200);
+}
projects / aerei / commitdiff