From: Andrea Manni Date: Mon, 13 Mar 2017 16:14:48 +0000 (+0100) Subject: Palla FSM motore X-Git-Url: http://git.piffa.net/web?p=aerei;a=commitdiff_plain;h=f768fc472387c5a06a6bbee13eadedf486b6be7c Palla FSM motore --- diff --git a/aerei/palla/ailerons_state_rgb/ailerons_state_rgb.ino b/aerei/palla/ailerons_state_rgb/ailerons_state_rgb.ino deleted file mode 100644 index 7af7018..0000000 --- a/aerei/palla/ailerons_state_rgb/ailerons_state_rgb.ino +++ /dev/null @@ -1,168 +0,0 @@ -/* Aereo Palla - -Pilotare un LED RGB in base al canale degli alettoni: -per versione con interrupts vedere esempio successivo. - -= 3 stati + 2 transizioni: -- piatto -- roll a sx -- roll a dx - -TODO: -* clean up magic numbers - -*/ - -#include -#define dEBUG // Cambiare in DEBUG per il debug - -// Variabili: -unsigned long currentMillis; // timestamp reference per millis per tutto il loop - -// Un LED RGB -RGBLed ailerons(9,10,7,255); // Common Cat - -// Transizione: Pwm -Lampeggiatore sxLamp(10); // Lampeggiatore -Lampeggiatore dxLamp(9); // Lampeggiatore - - -// Variabili per lettura canale servo -int ail ; // Valore per ailerons -int ailIn ; // Valore rilevato del 4 Ch della RX -unsigned long ailTimer ; // millis per ail - - -// 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 = 1000; // Pausa per la transizione durante gli stati 2, 4 della FSM - -// Variabili per interrupt 0 si PIN 2: Throttle -volatile unsigned int chValue2 = 1500; // Valore computato -volatile unsigned int chStart2 = 1500; // Inizio rilevamento - -// Variabili per interrupt 1 su PIN 3: Ailerons -volatile unsigned int ail = 1500; // Valore computato -volatile unsigned int mid_point = 1560; // Inizio rilevamento - - -// Variabili per autocalibrazione 1 -const byte chPin3 = 3; // PIN per la calibrazione -int mid_point3 = 1000; - -// 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 - -/////////////////////////////////////////////////////////// -void setup() { - -// #define DEBUG - -#ifdef DEBUG - Serial.begin(9600); -#endif - -// Funzione relativa a calibrazione: -mid_point = calibraTrim(ailPin) ; // + LED di servizio per monitor calibrazione -} - -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: - // dx - 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: - // sx - ailerons.Blue(); - if (ail > mid_point - deviation) { - ailstate = middle; - } - else if (ail > mid_point + deviation) { - FSM_lastMillis = currentMillis; - ailstate = dxin; - } ; - break; - } -#ifdef DEBUG - Serial.print("ailIn: "); - Serial.print(ailIn); - 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); - chValue2 = micros() - chStart2; -} -// Seconod iterrupt -void chRise3() { - attachInterrupt(1, chFall3, FALLING); - chStart3 = micros(); -} - -void chFall3() { - attachInterrupt(1, chRise3, RISING); - ail = micros() - chStart3; -} diff --git a/aerei/palla/ailerons_state_rgb_old/ailerons_state_rgb_old.ino b/aerei/palla/ailerons_state_rgb_old/ailerons_state_rgb_old.ino new file mode 100644 index 0000000..7af7018 --- /dev/null +++ b/aerei/palla/ailerons_state_rgb_old/ailerons_state_rgb_old.ino @@ -0,0 +1,168 @@ +/* Aereo Palla + +Pilotare un LED RGB in base al canale degli alettoni: +per versione con interrupts vedere esempio successivo. + += 3 stati + 2 transizioni: +- piatto +- roll a sx +- roll a dx + +TODO: +* clean up magic numbers + +*/ + +#include +#define dEBUG // Cambiare in DEBUG per il debug + +// Variabili: +unsigned long currentMillis; // timestamp reference per millis per tutto il loop + +// Un LED RGB +RGBLed ailerons(9,10,7,255); // Common Cat + +// Transizione: Pwm +Lampeggiatore sxLamp(10); // Lampeggiatore +Lampeggiatore dxLamp(9); // Lampeggiatore + + +// Variabili per lettura canale servo +int ail ; // Valore per ailerons +int ailIn ; // Valore rilevato del 4 Ch della RX +unsigned long ailTimer ; // millis per ail + + +// 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 = 1000; // Pausa per la transizione durante gli stati 2, 4 della FSM + +// Variabili per interrupt 0 si PIN 2: Throttle +volatile unsigned int chValue2 = 1500; // Valore computato +volatile unsigned int chStart2 = 1500; // Inizio rilevamento + +// Variabili per interrupt 1 su PIN 3: Ailerons +volatile unsigned int ail = 1500; // Valore computato +volatile unsigned int mid_point = 1560; // Inizio rilevamento + + +// Variabili per autocalibrazione 1 +const byte chPin3 = 3; // PIN per la calibrazione +int mid_point3 = 1000; + +// 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 + +/////////////////////////////////////////////////////////// +void setup() { + +// #define DEBUG + +#ifdef DEBUG + Serial.begin(9600); +#endif + +// Funzione relativa a calibrazione: +mid_point = calibraTrim(ailPin) ; // + LED di servizio per monitor calibrazione +} + +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: + // dx + 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: + // sx + ailerons.Blue(); + if (ail > mid_point - deviation) { + ailstate = middle; + } + else if (ail > mid_point + deviation) { + FSM_lastMillis = currentMillis; + ailstate = dxin; + } ; + break; + } +#ifdef DEBUG + Serial.print("ailIn: "); + Serial.print(ailIn); + 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); + chValue2 = micros() - chStart2; +} +// Seconod iterrupt +void chRise3() { + attachInterrupt(1, chFall3, FALLING); + chStart3 = micros(); +} + +void chFall3() { + attachInterrupt(1, chRise3, RISING); + ail = micros() - chStart3; +} diff --git a/aerei/palla/ailerons_state_thrFSM/ailerons_state_thrFSM.ino b/aerei/palla/ailerons_state_thrFSM/ailerons_state_thrFSM.ino new file mode 100644 index 0000000..1336198 --- /dev/null +++ b/aerei/palla/ailerons_state_thrFSM/ailerons_state_thrFSM.ino @@ -0,0 +1,206 @@ +/* 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 = 0; // 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 lettura canale alettoni +byte ailPin = 2; // Calibrazione + +// Variabili per autocalibrazione 0 +int mid_point2 = 1500; + + +// LED RGB alettoni +RGBLed ailerons(5,6,9); // Common Cat +// Transizione: Lampeggiatore +Lampeggiatore sxLamp(6); // Lampeggiatore +Lampeggiatore dxLamp(9); // Lampeggiatore + + + +// Vars Alettoni +int mid_point = 1500 ; // 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: +Pwm motore = 10; + +// 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 = 400; // 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 +mid_point = 1500 ; // + LED di servizio per monitor calibrazione +} + +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(pausa/2); + 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(pausa/2); + 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 + motore.lDown(1500); + } + else if (thr > 1900) { + // Throttle al massimo: LED laterali lampeggiano a caso, + // Sotto luminosita' a caso + caso = random(30, 250) ; + motore.lSet(caso); + delay(caso); // Blocking! + } + else { + // Throttle medio + thrBit = map(thr,1050, 1900, 0, 255); + 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); + ail = micros() - chStart2; +} + +// Seconod iterrupt +void chRise3() { + attachInterrupt(1, chFall3, FALLING); + chStart3 = micros(); +} + +void chFall3() { + attachInterrupt(1, chRise3, RISING); + thr = micros() - chStart3; +} diff --git a/aerei/palla/prototipo/prototipo.ino b/aerei/palla/prototipo/prototipo.ino index 061b081..8ed75dc 100644 --- a/aerei/palla/prototipo/prototipo.ino +++ b/aerei/palla/prototipo/prototipo.ino @@ -60,15 +60,15 @@ Serial.begin(9600); void loop() { sotto.Red(); - motore.set(0); + motore.Set(0); delay(500); sotto.Off(); sotto.Green(); - motore.set(150); + motore.Set(150); delay(500); sotto.Off(); sotto.Blue(); - motore.set(250); + motore.Set(250); delay(500); sotto.Off();