From: Andrea Manni Date: Mon, 20 Feb 2017 11:30:24 +0000 (+0100) Subject: Alettoni con interrupt + motore X-Git-Url: http://git.piffa.net/web?p=aerei;a=commitdiff_plain;h=0a3053c551a39c852376431700ff459e2efe1b84 Alettoni con interrupt + motore --- diff --git a/esempi/ailerons_state_interrupt/ailerons_state_interrupt.ino b/esempi/ailerons_state_interrupt/ailerons_state_interrupt.ino new file mode 100644 index 0000000..58948e4 --- /dev/null +++ b/esempi/ailerons_state_interrupt/ailerons_state_interrupt.ino @@ -0,0 +1,187 @@ +/* Ailerons state machine + Serial.print(mid_point); + +Pilotare un LED RGB in base al canale degli alettoni: +Questo sketch usa 2 interrupts per thr e alettoni. + +INPUT: +PIN 2 : throttle +PIN 3 : alettoni + +OUTPUT: +RGB Alettoni +Motore PWM + +FSM per alettoni += 3 stati + 2 transizioni: +- piatto +- roll a sx +- roll a dx +- piatto -> sx +- piatto -> dx + + + +TODO: +* clean up magic numbers + +*/ + +#include +#define dEBUG + +// Variabili per interrupt 0 si PIN 2 +volatile unsigned int thr = 1500; // Valore computato +volatile unsigned int chStart2 = 1500; // Inizio rilevamento + +// Variabili per interrupt 1 su PIN 3 +volatile unsigned int ail = 1500; // Valore computato +volatile unsigned int chStart3 = 1500; // Inizio rilevamento + +// Variabili per autocalibrazione 0 +const byte chPin2 = 3; // PIN per la calibrazione +int mid_point2 = 1500; + + +// Variabili: +unsigned long currentMillis; // timestamp reference per millis per tutto il loop + +Pwm motore = 11; + +// Un LED RGB +RGBLed ailerons(11,10,9,255); // Common Cat + +// Transizione: Pwm +Lampeggiatore sxLamp(10); // Lampeggiatore +Lampeggiatore dxLamp(9); // Lampeggiatore + + +// Variabili per lettura canale servo +byte ailPin = 3; // Calibrazione + +// Vars Alettoni +int mid_point = 1560 ; // centro del segnale, trimmato nel setup +const int deviation = 50 ; // deviazione dal punto medio + //per entrare nello stato successivo dal centro + + +// FSM gestione alettoni +enum { // Stati della FMS + middle, // centrale + sxin, // transizione a sx + sx, // sx + dxin, // transizione a dx + dx // dx +} ailstate = middle; + +// Vars FSM +unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni +unsigned long pausa = 500; // Pausa per la transizione durante gli stati 2, 4 della FSM + +/////////////////////////////////////////////////////////// +void setup() { + + +#ifdef DEBUG + Serial.begin(9600); +#endif + + attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168 + attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168 + + +// Funzione relativa a calibrazione: +mid_point = calibraTrim(ailPin) + 10 ; // + LED di servizio per monitor calibrazione +//Serial.print(mid_point); +//while(1) { +//} +} + +void loop() { + currentMillis = millis(); // Timestamp per tutto il loop + + switch (ailstate) { + case middle: + ailerons.White(); + // Alettoni piatti + if (ail > mid_point + deviation + deviation /3) { + // extra margine per avere un po' di gioco + ailstate = sxin; + FSM_lastMillis = currentMillis; + } + else if (ail < mid_point - deviation - deviation / 3) { + ailstate = dxin; + FSM_lastMillis = currentMillis ; + } ; + break; + + case sxin: + // Transizione a sx + sxLamp.Blink(200); + if (currentMillis - pausa > FSM_lastMillis ) { + ailstate = sx; + } + break; + + case sx: + ailerons.Green(); + if (ail < mid_point + deviation) { + ailstate = middle; + } + else if (ail < mid_point - deviation) { + FSM_lastMillis = currentMillis; + ailstate = dxin; + } ; + break; + + case dxin: + // Transizione a dx + dxLamp.Blink(200); + if (currentMillis - pausa > FSM_lastMillis ) { + ailstate = dx; + } + break; + + case dx: + ailerons.Blue(); + if (ail > mid_point - deviation) { + ailstate = middle; + } + else if (ail > mid_point + deviation) { + FSM_lastMillis = currentMillis; + ailstate = dxin; + } ; + break; + } + +// PWM Motore +motore.lSet((thr - 980) / 4); // 980 = minimo + +#ifdef DEBUG +Serial.print((thr - 980) / 4); + Serial.print("\tail: "); + Serial.print(ail); + Serial.print("\t ailstate:"); + Serial.println(ailstate); +#endif +} +// Functions +void chRise2() { + attachInterrupt(0, chFall2, FALLING); + chStart2 = micros(); +} + +void chFall2() { + attachInterrupt(0, chRise2, RISING); + thr = micros() - chStart2; +} +// Seconod iterrupt +void chRise3() { + attachInterrupt(1, chFall3, FALLING); + chStart3 = micros(); +} + +void chFall3() { + attachInterrupt(1, chRise3, RISING); + ail = micros() - chStart3; +} diff --git a/esempi/ailerons_state_rgb/ailerons_state_rgb.ino b/esempi/ailerons_state_rgb/ailerons_state_rgb.ino index bde402b..f812513 100644 --- a/esempi/ailerons_state_rgb/ailerons_state_rgb.ino +++ b/esempi/ailerons_state_rgb/ailerons_state_rgb.ino @@ -1,6 +1,8 @@ /* Ailerons state machine Pilotare un LED RGB in base al canale degli alettoni: +Questo sketch usa la funzione pulseIn(), +per versione con interrupts vedere esempio successivo. = 3 stati + 2 transizioni: - piatto @@ -13,6 +15,7 @@ TODO: */ #include +#define dEBUG // Cambiare in DEBUG per il debug // Variabili: unsigned long currentMillis; // timestamp reference per millis per tutto il loop @@ -21,8 +24,8 @@ unsigned long currentMillis; // timestamp reference per millis per tutto il loop RGBLed ailerons(11,10,9,255); // Common Cat // Transizione: Pwm -Pwm sxLamp(10); // Lampeggiatore -Pwm dxLamp(9); // Lampeggiatore +Lampeggiatore sxLamp(10); // Lampeggiatore +Lampeggiatore dxLamp(9); // Lampeggiatore // Variabili per lettura canale servo @@ -96,7 +99,7 @@ void loop() { case sxin: // Transizione a sx - sxLamp.(200); + sxLamp.Blink(200); if (currentMillis - pausa > FSM_lastMillis ) { ailstate = sx; } @@ -116,7 +119,7 @@ void loop() { case dxin: // Transizione a dx - dxLamp.(200); + dxLamp.Blink(200); if (currentMillis - pausa > FSM_lastMillis ) { ailstate = dx; } diff --git a/esempi/ailerons_state_thrFSM/ailerons_state_thrFSM.ino b/esempi/ailerons_state_thrFSM/ailerons_state_thrFSM.ino new file mode 100644 index 0000000..743bcc1 --- /dev/null +++ b/esempi/ailerons_state_thrFSM/ailerons_state_thrFSM.ino @@ -0,0 +1,229 @@ +/* Ailerons state machine + Serial.print(mid_point); + +Pilotare un LED RGB in base al canale degli alettoni: +Questo sketch usa 2 interrupts per thr e alettoni. + +INPUT: +PIN 2 : throttle +PIN 3 : alettoni + +OUTPUT: +ailerons RGB Alettoni +motore Motore PWM +left, right 2 Lampeggiatori PWM laterali + PWM + +FSM per alettoni += 3 stati + 2 transizioni: +- piatto +- roll a sx +- roll a dx +- piatto -> sx +- piatto -> dx + + +Ciclo if per gestione di 3 stati del motore: +- idle +- middle +- max + + +TODO: +* Da testare! Mai provato. + +*/ + +#include +#define DEBUG + +// Variabili per interrupt 0 si PIN 2 +volatile unsigned int thr = 1500; // Valore computato +volatile unsigned int chStart2 = 1500; // Inizio rilevamento + +// Variabili per interrupt 1 su PIN 3 +volatile unsigned int ail = 1500; // Valore computato +volatile unsigned int chStart3 = 1500; // Inizio rilevamento + +// Variabili per autocalibrazione 0 +const byte chPin2 = 3; // PIN per la calibrazione +int mid_point2 = 1500; + +// LEDs: +Pwm motore = 11; + +// LED RGB alettoni +RGBLed ailerons(5,6,9,255); // Common Cat +// Transizione: Pwm +Lampeggiatore sxLamp(6); // Lampeggiatore +Lampeggiatore dxLamp(9); // Lampeggiatore + + +// Variabili per lettura canale servo +byte ailPin = 3; // Calibrazione + +// Vars Alettoni +int mid_point = 1560 ; // centro del segnale, trimmato nel setup +const int deviation = 50 ; // deviazione dal punto medio + //per entrare nello stato successivo dal centro + +// Led motore e altri: +Lampeggiatore left = 10; +Lampeggiatore right = 12; +Pwm sotto = 9; + +// Quando il Throttle e' in IDE facciamo un PWM anche sui laterali +Pwm lpwm = 10 ; +Pwm rpwm = 12; + +// Variabili: +unsigned long currentMillis; // timestamp reference per millis per tutto il loop +byte caso; // Random var +byte thrBit ; // Valore a 8bit per il throttle + + + + +// FSM gestione alettoni +enum { // Stati della FMS + middle, // centrale + sxin, // transizione a sx + sx, // sx + dxin, // transizione a dx + dx // dx +} ailstate = middle; + +// Vars FSM +unsigned long FSM_lastMillis = 0 ; // Timestamp per la FSM degli alettoni +unsigned long pausa = 500; // Pausa per la transizione durante gli stati 2, 4 della FSM + +/////////////////////////////////////////////////////////// +void setup() { + + +#ifdef DEBUG + Serial.begin(9600); +#endif + + attachInterrupt(0, chRise2, RISING); // PIN 2 su 328p / 168 + attachInterrupt(1, chRise3, RISING); // PIN 3 su 328p / 168 + + +// Funzione relativa a calibrazione: +mid_point = calibraTrim(ailPin) + 10 ; // + LED di servizio per monitor calibrazione +//Serial.print(mid_point); +//while(1) { +//} +} + +void loop() { + currentMillis = millis(); // Timestamp per tutto il loop + + switch (ailstate) { + case middle: + ailerons.White(); + // Alettoni piatti + if (ail > mid_point + deviation + deviation /3) { + // extra margine per avere un po' di gioco + ailstate = sxin; + FSM_lastMillis = currentMillis; + } + else if (ail < mid_point - deviation - deviation / 3) { + ailstate = dxin; + FSM_lastMillis = currentMillis ; + } ; + break; + + case sxin: + // Transizione a sx + sxLamp.Blink(200); + if (currentMillis - pausa > FSM_lastMillis ) { + ailstate = sx; + } + break; + + case sx: + ailerons.Green(); + if (ail < mid_point + deviation) { + ailstate = middle; + } + else if (ail < mid_point - deviation) { + FSM_lastMillis = currentMillis; + ailstate = dxin; + } ; + break; + + case dxin: + // Transizione a dx + dxLamp.Blink(200); + if (currentMillis - pausa > FSM_lastMillis ) { + ailstate = dx; + } + break; + + case dx: + ailerons.Blue(); + if (ail > mid_point - deviation) { + ailstate = middle; + } + else if (ail > mid_point + deviation) { + FSM_lastMillis = currentMillis; + ailstate = dxin; + } ; + break; + } + +// Gestione throttle + if (thr < 1050) { + // IDLE + rpwm.UD(2000); + lpwm.UD(2000); + motore.lDown(1500); + } + else if (thr > 1900) { + // Throttle al massimo: LED laterali lampeggiano a caso, + // Sotto luminosita' a caso + caso = random(30, 250) ; + right.Swap(); + left.Swap(); + motore.lSet(caso); + delay(caso); // Blocking! + } + else { + // Throttle medio + thrBit = map(thr,1050, 1900, 0, 255); + right.Blink(1220 - 4 * thrBit ); + left.Blink(1220 - 4 * thrBit ); + motore.lSet(thrBit); // Luminosita' proporzionale al throttle + } + + +#ifdef DEBUG +Serial.print((thr - 980) / 4); + Serial.print("\tail: "); + Serial.print(ail); + Serial.print("\t ailstate:"); + Serial.println(ailstate); +#endif +} + + +// Functions +void chRise2() { + attachInterrupt(0, chFall2, FALLING); + chStart2 = micros(); +} + +void chFall2() { + attachInterrupt(0, chRise2, RISING); + thr = micros() - chStart2; +} +// Seconod iterrupt +void chRise3() { + attachInterrupt(1, chFall3, FALLING); + chStart3 = micros(); +} + +void chFall3() { + attachInterrupt(1, chRise3, RISING); + ail = micros() - chStart3; +} diff --git a/esempi/base_th_3stati/base_th_3stati.ino b/esempi/base_th_3stati/base_th_3stati.ino index e865f0a..22ea4c2 100644 --- a/esempi/base_th_3stati/base_th_3stati.ino +++ b/esempi/base_th_3stati/base_th_3stati.ino @@ -11,12 +11,12 @@ // LED disponibili Lampeggiatore left = 10; -Lampeggiatore right = 11; +Lampeggiatore right = 12; Pwm sotto = 9; // Quando il Throttle e' in IDE facciamo un PWM anche sui laterali Pwm lpwm = 10 ; -Pwm rpwm = 11; +Pwm rpwm = 12; // Variabili const byte thrPin = 3; // PIN collegato al CH3