Alettoni con interrupt + motore
authorAndrea Manni <andrea@piffa.net>
Mon, 20 Feb 2017 11:30:24 +0000 (12:30 +0100)
committerAndrea Manni <andrea@piffa.net>
Mon, 20 Feb 2017 11:30:24 +0000 (12:30 +0100)
esempi/ailerons_state_interrupt/ailerons_state_interrupt.ino [new file with mode: 0644]
esempi/ailerons_state_rgb/ailerons_state_rgb.ino
esempi/ailerons_state_thrFSM/ailerons_state_thrFSM.ino [new file with mode: 0644]
esempi/base_th_3stati/base_th_3stati.ino

diff --git a/esempi/ailerons_state_interrupt/ailerons_state_interrupt.ino b/esempi/ailerons_state_interrupt/ailerons_state_interrupt.ino
new file mode 100644 (file)
index 0000000..58948e4
--- /dev/null
@@ -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 <common.h>
+#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;
+}
index bde402b..f812513 100644 (file)
@@ -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 <common.h>
+#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 (file)
index 0000000..743bcc1
--- /dev/null
@@ -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 <common.h>
+#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;
+}
index e865f0a..22ea4c2 100644 (file)
 
 // 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