Diff: main.cpp.orig

Revision:

19:1fd39a2afc30

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Thu Oct 24 09:25:27 2019 +0000
@@ -0,0 +1,259 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
+#include <math.h>
+#include "Servo.h"
+
+// Definieer objecten
+Serial pc(USBTX, USBRX);
+
+PwmOut motor1(D6);  // Misschien moeten we hiervoor DigitalOut gebruiken, moet 
+PwmOut motor2(D5);  // samen kunnen gaan met de servo motor
+
+DigitalOut motor1_dir(D7);
+DigitalOut motor2_dir(D4);
+  
+DigitalIn Power_button_pressed(D1); // Geen InterruptIn gebruiken!
+DigitalIn Emergency_button_pressed(D2);  
+
+AnalogIn EMG_biceps_right_raw (A0);
+AnalogIn EMG_biceps_left_raw (A1);
+AnalogIn EMG_calf_raw (A2);
+
+Ticker loop_ticker;
+Ticker HIDScope_ticker;
+Ticker emgSampleTicker;
+
+HIDScope scope(3);
+
+BiQuadChain bqc; // Let op !!! Deze coëfficiënten moeten nog worden veranderd
+BiQuad bq1(0.0030, 0.0059, 0.0030, -1.8404,0.8522); //voor low-pass
+BiQuad bq2(0.9737, -1.9474, 0.9737, -1.9467, 0.9481); //voor high-pass
+BiQuad bq3(0.9912, -1.9823,0.9912, -1.9822, 0.9824); //lage piek eruit-> voor coëfficienten, zie matlab
+
+bool calib = false; // MOGELIJK GAAT HET HIER FOUT
+int i_calib = 0;
+
+void emgSampleFilter() // Deze functie wordt aangeroepen dmv een ticker. Het sampled 
+                       // hierdoor het EMG signaal en het haalt er een filter overheen
+{   
+    float filtered_EMG_biceps_right=bqc.step(EMG_biceps_right_raw.read());
+    float filtered_EMG_biceps_left=bqc.step(EMG_biceps_left_raw.read());
+    float filtered_EMG_calf=bqc.step(EMG_calf_raw.read());
+    
+    float filtered_EMG_biceps_right_total=filtered_EMG_biceps_right;
+    float filtered_EMG_biceps_left_total=filtered_EMG_biceps_left;
+    float filtered_EMG_calf_total=filtered_EMG_calf;
+    
+    if (calib)
+        {   
+            if (i_calib < 500)
+                {
+                    filtered_EMG_biceps_right_total=filtered_EMG_biceps_right+filtered_EMG_biceps_right_total;
+                    filtered_EMG_biceps_left_total=filtered_EMG_biceps_left+filtered_EMG_biceps_left_total;
+                    filtered_EMG_calf_total=filtered_EMG_calf+filtered_EMG_calf_total;
+                    i_calib++;
+                }
+            if (i_calib >= 500)
+                {   
+                    mean_EMG_biceps_right=filtered_EMG_biceps_right_total/500;
+                    mean_EMG_biceps_left=filtered_EMG_biceps_left_total/500;
+                    mean_EMG_calf=filtered_EMG_calf_total/500;
+                    calib = false; 
+                }
+         }
+}
+
+void sendHIDScope() // Deze functie geeft de gefilterde EMG-signalen weer in de HIDScope
+                    // Wordt eveneens gerund dmv een ticker
+{
+    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    scope.set(0, filtered_EMG_biceps_right() ); // Werkt dit zo? Of nog met .read?
+    scope.set(1, filtered_EMG_biceps_left() );
+    scope.set(2, filtered_EMG_calf() );
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    *  Ensure that enough channels are available (HIDScope scope( 2 ))
+    *  Finally, send all channels to the PC at once */
+    scope.send();
+    // Eventueel nog een ledje laten branden
+}
+
+// Emergency    
+void emergency()
+    {
+        loop_ticker.detach(); 
+        motor1.write(0);
+        motor2.write(0);
+        pc.printf("Ik ga exploderen!!!\r\n");
+        // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
+        // opnieuw worden opgestart. Mogelijk kan dit door de ticker te 
+        // detachen
+    }    
+
+// Motoren uitzetten    
+void motors_off()
+    {
+        motor1.write(0);
+        motor2.write(0);
+        pc.printf("Motoren uit functie\r\n");
+    }  
+    
+// Motoren aanzetten
+void motors_on()
+    {
+        motor1.write(0.9);
+        motor1_dir.write(1);
+        motor2.write(0.1);
+        motor1_dir.write(1);
+        pc.printf("Motoren aan functie\r\n");
+    }
+        
+// EMG kalibreren        
+void  emg_calibration()   
+     {
+        // Gedurende bijv. 5 seconden EMG meten, wanneer de spieren maximaal
+        // worden aangespannen -> maximaal potentiaal verkrijgen. Een fractie 
+        // hiervan kan als drempel worden gebruikt voor beweging
+        
+        // *Tijd instellen* 
+        // Iets met DOUBLE_MAX? https://docs.microsoft.com/en-us/cpp/c-language/cpp-integer-limits?view=vs-2019
+        
+        // Ledje van kleur laten veranderen
+        
+        // MOGELIJK NIET MEER NODIG???
+       
+     }
+
+// Finite state machine programming (calibration servo motor?)
+enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode};
+
+states currentState = Motors_off; 
+bool stateChanged = true; // Make sure the initialization of first state is executed
+
+void ProcessStateMachine(void) 
+{   
+    switch (currentState) 
+    { 
+        case Motors_off:
+        
+            if (stateChanged) 
+            {
+                motors_off(); // functie waarbij motoren uitgaan
+                stateChanged = false;
+                pc.printf("Motors off state\r\n");
+            }          
+            if  (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
+            {      
+                motors_on();
+                currentState = Calib_motor;
+                stateChanged = true;
+                pc.printf("Moving to Calib_motor state\r\n");
+            }
+            if  (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
+            { 
+                emergency();
+            } 
+            break;
+            
+        case Calib_motor:
+            
+            if (stateChanged)
+            {       
+                // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
+                currentState = Calib_EMG;
+                stateChanged = true;
+                pc.printf("Moving to Calib_EMG state\r\n");
+            }    
+            if  (Emergency_button_pressed.read() == false) 
+            { 
+                emergency();
+            } 
+            break;
+            
+         case Calib_EMG:
+            
+            motors_off();
+            if (stateChanged)
+            {
+                // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
+                calib = true;
+                emgSampleFilter() // Gaat dit nu goed? -> moet sws worden toegevoegd bij relevante onderdelen?
+                if (i_calib >= 500) // of wait(10);?
+                {
+                    currentState = Homing;
+                    stateChanged = true;
+                    pc.printf("Moving to Homing state\r\n");
+                }
+            if  (Emergency_button_pressed.read() == false) 
+            { 
+                emergency();
+            } 
+            break; 
+            
+         case Homing:
+            
+            motors_on();
+            if (stateChanged)
+            {   
+                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
+                // (inclusief de end-effector) in de juiste home positie wordt gezet
+                currentState = Operation_mode;
+                stateChanged = true;
+                pc.printf("Moving to operation mode \r\n");
+            }
+            if  (Emergency_button_pressed.read() == false) 
+            { 
+                emergency();
+            } 
+            break; 
+            
+         case Operation_mode: // Overgaan tot emergency wanneer referentie niet 
+                              // overeenkomt met werkelijkheid
+        
+            if (stateChanged)
+             
+                // Hier moet een functie worden aangeroepen die ervoor zorgt dat 
+                // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, 
+                // zodat de robotarm kan bewegen   
+                
+                if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
+                {
+                    motors_off();
+                    currentState = Motors_off;
+                    stateChanged = true;
+                    pc.printf("Terug naar de state Motors_off\r\n");
+                }
+                if  (Emergency_button_pressed.read() == false) 
+                { 
+                    emergency();
+                } 
+                // wait(5);
+                else 
+                { 
+                    currentState = Homing; 
+                    stateChanged = true; 
+                    pc.printf("Terug naar de state Homing\r\n");
+                }
+                break;
+                     
+        default:
+            // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
+            motors_off();
+            pc.printf("Unknown or uninplemented state reached!\r\n");
+            
+    }
+}
+
+int main(void)
+    {
+        pc.printf("Opstarten\r\n");
+        bqc.add(&bq1).add(&bq2).add(&bq3);
+        emgSampleTicker.attach(&emgSampleFilter, 0.01f);
+        HIDScope_ticker.attach(&sendHIDScope, 0.01f);
+        loop_ticker.attach(&ProcessStateMachine, 5.0f);  
+        while(true) 
+        { /* do nothing */}
+    }
\ No newline at end of file