Revision 3:0c31a4a5d1fe, committed 2019-10-18

Comitter:

Mortimerz

Date:

Fri Oct 18 12:06:15 2019 +0000

Parent:

2:5730195cf595

Commit message:

meerdere functies in state machine verwerkt;

Changed in this revision

diff -r 5730195cf595 -r 0c31a4a5d1fe FastPWM.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Fri Oct 18 12:06:15 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/FastPWM/#c0b2265cff9c

diff -r 5730195cf595 -r 0c31a4a5d1fe Opzet_Eli.cpp
--- a/Opzet_Eli.cpp	Mon Oct 14 12:14:18 2019 +0000
+++ b/Opzet_Eli.cpp	Fri Oct 18 12:06:15 2019 +0000
@@ -3,22 +3,263 @@
 #include "HIDScope.h"
 #include "QEI.h"
 #include "MODSERIAL.h"
+#include "biquadFilter.h" 
+#include "math.h" 
+#include "FastPWM.h"
 
 // Define objects
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+AnalogIn emg3(A2);
+AnalogIn emg4(A3);
 MODSERIAL pc(USBTX,USBRX);
 DigitalOut led_red(LED_RED);
 DigitalOut led_blue(LED_BLUE);
 DigitalOut led_green(LED_GREEN);
 InterruptIn button_Mbed(PTC6); //Button 1 Mbed
-InterruptIn button_1(PTB10); //Button 2 BRS
-InterruptIn button_2(PTB11); // Button 3 BRS
+InterruptIn button_1(); //Button 2 BRS
+InterruptIn button_2(); // Button 3 BRS
+
+FastPWM motor1(D6);
+DigitalOut motor1_dir(D7);
+FastPWM motor2(D5);
+DigitalOut motor2_dir(D4);
+
+
+//ticker setup
+Ticker states_machine;
+
+    
+//encoder setup
+QEI encoder_1(D13,D12,NC,32,QEI::X4_ENCODING);
+QEI encoder_2(D11,D10,NC,32,QEI::X4_ENCODING);
+
+//motor setup
+int motordir1 = 1;
+int motordir2 = 1;
+double Kp = 17.5;
+
+//Hidscope setup
+HIDScope    scope(4); 
+
+// EMG setup
+biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
+biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
+biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
+biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
+double emg_value_1;
+double signalpart1_1;
+double signalpart2_1;
+double signalpart3_1;
+double signalpart4_1;
+double signalfinal_1;
+double emgsignal_1;
+double onoffsignal_1;
+double maxcal_1=0;        
 
-// Functions 
+double emg_value_2;
+double signalpart1_2;
+double signalpart2_2;
+double signalpart3_2;
+double signalpart4_2;
+double signalfinal_2;
+double emgsignal_2;
+double onoffsignal_2;
+double maxcal_2=0; 
+
+double emg_value_3;
+double signalpart1_3;
+double signalpart2_3;
+double signalpart3_3;
+double signalpart4_3;
+double signalfinal_3;
+double emgsignal_3;
+double onoffsignal_3;
+double maxcal_3=0; 
+
+double emg_value_4;
+double signalpart1_4;
+double signalpart2_4;
+double signalpart3_4;
+double signalpart4_4;
+double signalfinal_4;
+double emgsignal_4;
+double onoffsignal_4;
+double maxcal_4=0; 
+
+double emgx;
+double emgy;
+double kemg = 0.1;
+
+//kinematics setup
+double xgoal;
+double ygoal;
+double theta1;
+double theta2;
+
+   
+// State machine
 enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
-
 states CurrentState = START;
 bool StateChanged = true;         // this is the initialization of the first state
 
+// button functions
+
+void emgread(){
+     emg_value_1 = emg1.read();//read the emg value from the elektrodes
+        signalpart1_1 = notch.step(emg_value_1);//Highpass filter for removing offset and artifacts
+        signalpart2_1 = filterhigh1.step(signalpart1_1);//rectify the filtered signal
+        signalpart3_1 = abs(signalpart2_1);//low pass filter to envelope the emg
+        signalpart4_1 = filterlow1.step(signalpart3_1);//notch filter to remove 50Hz signal
+        emgsignal_1 = filterlow2.step(signalpart4_1);//2nd low pass filter to envelope the emg
+     emg_value_2 = emg2.read();//read the emg value from the elektrodes
+        signalpart1_2 = notch.step(emg_value_2);//Highpass filter for removing offset and artifacts
+        signalpart2_2 = filterhigh1.step(signalpart1_2);//rectify the filtered signal
+        signalpart3_2 = abs(signalpart2_2);//low pass filter to envelope the emg
+        signalpart4_2 = filterlow1.step(signalpart3_2);//notch filter to remove 50Hz signal
+        emgsignal_2 = filterlow2.step(signalpart4_2);//2nd low pass filter to envelope the emg
+    emg_value_3 = emg3.read();//read the emg value from the elektrodes
+        signalpart1_3 = notch.step(emg_value_3);//Highpass filter for removing offset and artifacts
+        signalpart2_3 = filterhigh1.step(signalpart1_3);//rectify the filtered signal
+        signalpart3_3 = abs(signalpart2_3);//low pass filter to envelope the emg
+        signalpart4_3 = filterlow1.step(signalpart3_3);//notch filter to remove 50Hz signal
+        emgsignal_3 = filterlow2.step(signalpart4_3);//2nd low pass filter to envelope the emg
+    emg_value_4 = emg4.read();//read the emg value from the elektrodes
+        signalpart1_4 = notch.step(emg_value_4);//Highpass filter for removing offset and artifacts
+        signalpart2_4 = filterhigh1.step(signalpart1_4);//rectify the filtered signal
+        signalpart3_4 = abs(signalpart2_4);//low pass filter to envelope the emg
+        signalpart4_4 = filterlow1.step(signalpart3_4);//notch filter to remove 50Hz signal
+        emgsignal_4 = filterlow2.step(signalpart4_4);//2nd low pass filter to envelope the emg
+}
+void emgcal(){
+            emgread();
+            double signalmeasure_1 = emgsignal_1;
+            if (signalmeasure_1 > maxcal_1){//determine what the highest reachable emg signal is
+                maxcal_1 = signalmeasure_1;}
+            double signalmeasure_2 = emgsignal_2;
+            if (signalmeasure_2 > maxcal_2){//determine what the highest reachable emg signal is
+                maxcal_2 = signalmeasure_2;}
+            double signalmeasure_3 = emgsignal_3;
+            if (signalmeasure_3 > maxcal_3){//determine what the highest reachable emg signal is
+                maxcal_3 = signalmeasure_3;}
+            double signalmeasure_4 = emgsignal_4;
+            if (signalmeasure_4 > maxcal_4){//determine what the highest reachable emg signal is
+                maxcal_4 = signalmeasure_4;}
+            scope.set(0,signalmeasure_1);//set emg signal to scope in channel 1
+            scope.set(1,signalmeasure_2);//set filtered signal to scope in channel 2
+            scope.set(2,signalmeasure_3);//set filtered signal to scope in channel 3
+            scope.set(3,signalmeasure_4);//set filtered signal to scope in channel 4
+        }
+    
+
+void emgshow(){
+        emgread();
+        onoffsignal_1=emgsignal_1/maxcal_1;// emg positive x
+        onoffsignal_2=emgsignal_2/maxcal_2;// emg negative x   
+        onoffsignal_3=emgsignal_3/maxcal_3;// emg positive y
+        onoffsignal_4=emgsignal_4/maxcal_4;// emg negative y
+        scope.set(0,onoffsignal_1);//set emg signal to scope in channel 1
+        scope.set(1,onoffsignal_2);//set filtered signal to scope in channel 2
+        scope.set(2,onoffsignal_3);//set filtered signal to scope in channel 3
+        scope.set(3,onoffsignal_4);//set filtered signal to scope in channel 4
+        emgx = onoffsignal_1- onoffsignal_2;
+        emgy = onoffsignal_3- onoffsignal_4;
+    scope.send();
+        }
+
+void forwardkin(){
+    double qref2 = encoder_2.getPulses();
+    double q2_correct = (qref2*2*3.14)/8400.0;
+    double qref1 = encoder_1.getPulses();
+    double q1_correct = (qref1*2*3.14)/8400.0;
+    double x = 0.3*cos(q1_correct)+0.3*cos(q1_correct+q2_correct);
+    double y = 0.3*sin(q1_correct)+0.3*cos(q1_correct+q2_correct);
+    emgshow();
+    xgoal = x + emgx* kemg;
+    ygoal = y + emgy* kemg;
+    }
+    
+void reversekin(){
+    forwardkin();
+    theta2= acos(((xgoal*xgoal)+(ygoal*ygoal)-(0.3*0.3)-(0.3*0.3))/(2*0.3*0.3));
+    theta1= atan(ygoal/xgoal)-atan((0.3*sin(theta2))/(0.3+0.3*cos(theta2)));
+    }
+    
+void motor_position()
+{
+    reversekin();
+    double pos_1 = encoder_1.getPulses();
+    double poscorrect_1 = (pos_1*3.14*2)/8400.0;
+    double error1 = theta1-poscorrect_1;
+    if (error1 >=0) motor1_dir=1;
+    else motor1_dir=0;
+    if (fabs(error1)>1) motor1 = 1;
+    else motor1 = fabs(error1);
+    
+    double pos_2 = encoder_2.getPulses();
+    double poscorrect_2 = (pos_2*3.14*2)/8400.0;
+    double error2 = theta2-poscorrect_2;
+    if (error2 >=0) motor2_dir=1;
+    else motor1_dir=0;
+    if (fabs(error2)>1) motor2 = 1;
+    else motor1 = fabs(error2);
+    }
+
+void kalmot(){
+    CurrentState = KAL_ME;
+    StateChanged = true;
+}
+           
+void kalemg(){
+    CurrentState = KAL_EMG;
+    StateChanged = true;
+    wait(0.2f);
+}
+void movestart(){
+    CurrentState = MOVE_START;
+    StateChanged = true;
+    wait(0.2f);
+    }
+    
+void readystart(){
+    CurrentState = READY_START;
+    StateChanged = true;
+    wait(0.2f);
+    }
+void demo(){
+    CurrentState = DEMO;
+    StateChanged = true;
+    wait(0.2f);
+    }
+void move(){
+    CurrentState = MOVE;
+    StateChanged = true;
+    wait(0.2f);
+    }
+void wait(){
+    CurrentState = WAIT;
+    StateChanged = true;
+    wait(0.2f);
+    }
+void off(){
+    CurrentState = OFF;
+    StateChanged = true;
+    wait(0.2f);
+    }
+    
+//led functions
+void flashred(){
+    led_red = !led_red;
+    wait(0.4f);
+    }
+void flashgreen(){
+    led_green = !led_green;
+    wait(0.4f);
+    }    
+void flashblue(){
+    led_blue = !led_blue;
+    wait(0.4f);
+    } 
 // Function START_TO_KAL_ME
 
 void StateMachine(void)
@@ -34,21 +275,16 @@
                 led_green = 1;
                 StateChanged = false;
             }
-            if (button_Mbed.mode (PullDown)== false; ) // State switches when button pressed
-            {
-                CurrentState = KAL_ME;
-                StateChanged = true;
-                wait(0.2f);
-                
-            }
+            button_Mbed.rise(&kalmot);  // State switches when button pressed
+                           
         break;      // end of state START
                 
         case KAL_ME:
             if (StateChanged)
             {
                 pc.printf("Calibration ME state, red ld flickers slow"); 
-                //FUNCTION Red led flickers slow
-                
+                flashred();
+                                
                 
                 // FUNCTION Move to mechanical stop, include v_motor, t_passed
                 // FUNCTION Reset encoders
@@ -56,28 +292,19 @@
                 StateChanged = false;
             }
             
-            if  (v_motor == 0 && t_passed > 2) // FUNCTION t_passed, included in v_motor?
-            {
-                CurrentState = KAL_EMG;
-                StateChanged = true;
-            }
+            button_Mbed.rise(&kalemg);
         break;      // end of state KAL_ME
         
         case KAL_EMG:
             if (StateChanged)
             {
-                // FUCNTION Red led flickers fast
-                
-                //FUNCTION Measure EMG_max, EMG variable meten, t_passed
-                
+                flashgreen();
+                emgcal();              
                 StateChanged = false;
             }
             
-            if (EMG < 0.1*EMG_max && t_passed > 2)
-            {
-                CurrentState = MOVE_START;
-                StateChanged = true;
-            }
+            button_Mbed.rise(&readystart);
+            
         break;      // end of state KAL_EMG
         
         case MOVE_START:
@@ -92,11 +319,8 @@
                 StateChanged = false; 
             }
             
-            if (current_position == start_position && t_passed > 2) // FUNCTIO t_passed
-            {
-                CurrentState = READY_START;
-                StateChanged = true;
-            } 
+            button_Mbed.rise(&readystart);
+            
         break;      // end of state MOVE_START
         
         case READY_START:
@@ -109,18 +333,9 @@
                 StateChanged = false;
             }
             
-            if (button_1.read() == false)   // Button 1
-            {
-                CurrentState = DEMO;
-                StateChanged = true;
-                wait(0.2f);
-            }
-            else if (button_2.read() == false || EMG > 0.2*EMG_max) // Button 2 or 20% EMG_max
-            {
-                CurrentState = MOVE;
-                StateChanged = true;
-                wait(0.2f);
-            }                
+            button_Mbed.rise(&demo);
+            //button_1.rise(&move);
+                           
         break;      // end of state READY_START
         
         case DEMO:
@@ -133,11 +348,7 @@
                 StateChanged = false; 
             }
             
-            if (current_position == end_position && t_passed > 2) 
-            {
-                CurrentState = MOVE_START;
-                StateChanged = true;
-            }
+            button_Mbed.rise(&movestart);
         break;      // end of state DEMO
         
         case MOVE:
@@ -152,19 +363,9 @@
                 StateChanged = false; 
             }
             
-            if (button_Mbed.read() == false)
-            {
-                CurrentState = OFF;
-                StateChanged = true;
-                wait(0.2f);
-            }
+            button_Mbed.rise(&off);
+            button1.rise(&wait);
             
-            else if (button_1.read() == false || EMG < 0.2*EMG_max) 
-            {
-                CurrentState = WAIT;
-                StateChanged = true;
-                wait(0.2f);
-            }
             break;      // end of state MOVE
         
         case WAIT:
@@ -174,24 +375,10 @@
                 led_blue = 0; 
                 led_green = 1
                 
-            if (button_Mbed.read() == false) // For five second, too hard
-            {
-                CurrentState = OFF;
-                StateChanged = true;
-                wait(0.2f);
-            )
-            else if (button_1.read() == false) 
-            {
-                CurrentState = MOVE_START;
-                StateChanged = true;
-                wait(0.2f);
-            }
-            else if (button_2.read() == false || EMG > 0.2*EMG_max) 
-            {
-                CurrentState = MOVE;
-                StateChanged = true;
-                wait(0.2f);
-            }
+            button_Mbed.rise(&off);
+            button1.rise(&movestart);
+            button2.rise(&move);
+           
             
         break;      // end of state WAIT
         
@@ -210,11 +397,11 @@
 
 int main(void)      // wat hier in moet snap ik nog niet 
 {
+        states_machine.attach(&StateMachine, 0.002)
         // hier moeten dingen komen
         while (true)
         {
             CheckForCommandFromTerminal();
-            StateMachine();
         }
 } 
         

diff -r 5730195cf595 -r 0c31a4a5d1fe biquadFilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Fri Oct 18 12:06:15 2019 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/biquadFilter/#e3bf917ae0a3