Revision 0:0fd6a09e85d9, committed 2018-11-02

Comitter:

Duif

Date:

Fri Nov 02 08:23:22 2018 +0000

Commit message:

Demo met EMG, deze zou het moeten doen

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad4th_order.lib	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Mirjam/code/BiQuad4th_order/#caa20b96f300

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Biquad.lib	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Gerth/code/Biquad/#3a80d1f8f30a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/FastPWM/#c0b2265cff9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/FilterDesign.cpp	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,62 @@
+#include "FilterDesign.h"
+#include "BiQuad.h"
+#include "BiQuad4.h"
+
+// Notch filter op 50 Hz
+double nb0 = 0.999103206817809;
+double nb1 = -1.994263409725146;
+double nb2 = 0.999103206817809;
+double na1 = -1.994263409725146;
+double na2 = 0.998206413635618;
+
+// 4th order Butterworth High pass 10 Hz
+double hpb0 = 0.922946103200875;
+double hpb1 = -3.691784412803501;
+double hpb2 = 5.537676619205252; 
+double hpb3 = -3.691784412803501;
+double hpb4 = 0.922946103200875;
+double hpa1 = -3.839672788481732;
+double hpa2 = 5.531745865737864;
+double hpa3 = -3.543889487580057;
+double hpa4 = 0.851829509414351;
+
+/*
+// 4th order Butterworth Low pass 6 Hz
+double lpb0 = 0.000000109473538449645 ;
+double lpb1 = 0.000000437894153798579 ;
+double lpb2 = 0.000000656841230697869; 
+double lpb3 = 0.000000437894153798579;
+double lpb4 = 0.000000109473538449645;
+double lpa1 = -3.903798995738811;
+double lpa2 = 5.715997307717368;
+double lpa3 = -3.720469814151233;
+double lpa4 = 0.908273253749291;
+*/
+
+//4th order Butterworth low pass 9 Hz
+double lpb0 = 0.00000054134117189603 ;
+double lpb1 = 0.00000216536468758410  ;
+double lpb2 = 0.00000324804703137616; 
+double lpb3 = 0.00000216536468758410;
+double lpb4 = 0.00000054134117189603;
+double lpa1 = -3.855703428197204;
+double lpa2 = 5.577429961461492;
+double lpa3 = -3.587322565783154;
+double lpa4 = 0.865604693977616;
+
+double gain = 10.00000;
+
+BiQuad notch50(nb0, nb1, nb2, na1, na2);
+BiQuad4 highpass(hpb0, hpb1, hpb2, hpb3, hpb4, hpa1, hpa2, hpa3, hpa4);
+BiQuad4 lowpass(lpb0, lpb1, lpb2, lpb3, lpb4, lpa1, lpa2, lpa3, lpa4);
+
+double FilterDesign(double u)
+{   
+    double y_n = notch50.step(u);
+    double y_hp = highpass.step(y_n);
+    double y_abs = abs(y_hp);
+    double y_lp = lowpass.step(y_abs);
+    double y_gain = y_lp*gain;
+    
+    return y_gain;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/FilterDesign.h	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,3 @@
+#include "mbed.h"
+ 
+double FilterDesign(double u);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter2/FilterDesign2.cpp	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,62 @@
+#include "FilterDesign2.h"
+#include "BiQuad.h"
+#include "BiQuad4.h"
+
+// Notch filter op 50 Hz
+double nb0_2 = 0.999103206817809;
+double nb1_2 = -1.994263409725146;
+double nb2_2 = 0.999103206817809;
+double na1_2 = -1.994263409725146;
+double na2_2 = 0.998206413635618;
+
+// 4th order Butterworth High pass 10 Hz
+double hpb0_2 = 0.922946103200875;
+double hpb1_2 = -3.691784412803501;
+double hpb2_2 = 5.537676619205252; 
+double hpb3_2 = -3.691784412803501;
+double hpb4_2 = 0.922946103200875;
+double hpa1_2 = -3.839672788481732;
+double hpa2_2 = 5.531745865737864;
+double hpa3_2 = -3.543889487580057;
+double hpa4_2 = 0.851829509414351;
+
+/*
+// 4th order Butterworth Low pass 6 Hz
+double lpb0 = 0.000000109473538449645 ;
+double lpb1 = 0.000000437894153798579 ;
+double lpb2 = 0.000000656841230697869; 
+double lpb3 = 0.000000437894153798579;
+double lpb4 = 0.000000109473538449645;
+double lpa1 = -3.903798995738811;
+double lpa2 = 5.715997307717368;
+double lpa3 = -3.720469814151233;
+double lpa4 = 0.908273253749291;
+*/
+
+//4th order Butterworth low pass 9 Hz
+double lpb0_2 = 0.00000054134117189603 ;
+double lpb1_2 = 0.00000216536468758410  ;
+double lpb2_2 = 0.00000324804703137616; 
+double lpb3_2 = 0.00000216536468758410;
+double lpb4_2 = 0.00000054134117189603;
+double lpa1_2 = -3.855703428197204;
+double lpa2_2 = 5.577429961461492;
+double lpa3_2 = -3.587322565783154;
+double lpa4_2 = 0.865604693977616;
+
+double gain_2 = 10.00000;
+
+BiQuad notch50_2(nb0_2, nb1_2, nb2_2, na1_2, na2_2);
+BiQuad4 highpass_2(hpb0_2, hpb1_2, hpb2_2, hpb3_2, hpb4_2, hpa1_2, hpa2_2, hpa3_2, hpa4_2);
+BiQuad4 lowpass_2(lpb0_2, lpb1_2, lpb2_2, lpb3_2, lpb4_2, lpa1_2, lpa2_2, lpa3_2, lpa4_2);
+
+double FilterDesign2(double u)
+{   
+    double y_n_2 = notch50_2.step(u);
+    double y_hp_2 = highpass_2.step(y_n_2);
+    double y_abs_2 = abs(y_hp_2);
+    double y_lp_2 = lowpass_2.step(y_abs_2);
+    double y_gain_2 = y_lp_2*gain_2;
+    
+    return y_gain_2;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter2/FilterDesign2.h	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,3 @@
+#include "mbed.h"
+ 
+double FilterDesign2(double u);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/MODSERIAL/#da0788f0bd77

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/tomlankhorst/code/biquadFilter/#26861979d305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,281 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "math.h"
+#include "BiQuad.h"
+#include "BiQuad4.h"
+#include "FilterDesign.h"
+#include "FilterDesign2.h"
+
+// --------------------------------------------------
+// ----------------- SET UP -------------------------
+QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
+QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
+DigitalOut directionM1(D4);
+DigitalOut directionM2(D7);
+FastPWM motor1_pwm(D5);
+FastPWM motor2_pwm(D6);
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut ledr(LED_RED);
+DigitalOut ledg(LED_GREEN);
+DigitalOut ledb(LED_BLUE);
+DigitalIn directionx(SW3); //x direction switch button
+DigitalIn directiony(SW2); //y direction switch button
+DigitalIn buttonx(D2); //x EMG replacement 
+DigitalIn buttony(D3); //y EMG replacement
+AnalogIn emg1_raw(A0);
+AnalogIn emg2_raw(A1);
+
+// Tickers
+Ticker Demo;
+
+// ---------------------------------------------------
+// ----------------- GLOBAL VARIABLES ----------------
+volatile int counts1; 
+volatile int counts2;
+volatile double theta1;
+volatile double theta2;
+const double  pi = 3.14159265359;
+volatile double  error1;
+volatile double  error2;
+//volatile double  error1_final=10.0;
+//volatile double  error2_final=10.0;
+//volatile double q1_diff_final;
+//volatile double q2_diff_final;
+//double  point1x = 200.0;
+//double  point1y = 200.0;
+//double  point2x = 200.0;
+//double  point2y = 100.0;
+//double  point3x = 350.0;
+//double  point3y = 0.0;
+//double  point4x = 200.0;
+//double  point4y = 0.0;
+//volatile int track = 1;
+const double x0 = 80.0; //zero x position after homing
+const double y0 = 141.0; //zero y position after homing
+volatile double  setpointx = x0;
+volatile double  setpointy = y0;
+volatile double  U1;
+volatile double  U2;
+volatile double xfinal;
+volatile double yfinal;
+volatile int sx;//value of the button and store as switch
+volatile int sy;//value of the button and store as switch
+double dirx = 1.0; //determine the direction of the setpoint placement
+double diry = 1.0; //determine the direction of the setpoint placement
+int need_to_move_1; // Does the robot needs to move in the first direction?
+int need_to_move_2; // Does the robot needs to move in the second direction?
+double EMG_calibrated_max_1 = 2.0000;  // Maximum value of the first EMG signal found in the calibration state.
+double EMG_calibrated_max_2 = 2.0000;  // Maximum value of the second EMG signal found in the calibration state.
+
+float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG
+
+double emg1_filtered = 0.00;
+double emg2_filtered = 0.00;
+
+const double v=0.1; //moving speed of setpoint 
+
+
+// Inverse Kinematics
+volatile double q1_diff;
+volatile double q2_diff;
+const double sq = 2.0; //to square numbers
+const double L1 = 250.0; //length of the first link
+const double L3 = 350.0; //length of the second link
+
+// Reference angles of the starting position
+double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
+double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
+double q2_0_enc = (pi-q2_0) - q1_0;
+
+// --------------------------------------------------------------------
+// -------------- Get EMG values --------------------------------------
+// --------------------------------------------------------------------
+
+// Sample: samples the EMG and sends it to HIDScope
+void sample()
+{
+    emg1_filtered = FilterDesign(emg1_raw.read());
+    emg2_filtered = FilterDesign2(emg2_raw.read());
+    
+    //scope.set(0, emg1_raw.read());      // Raw EMG 1 send to scope 0
+    //scope.set(1, emg1_filtered);        // Filtered EMG 1 send to scope 1
+    //scope.set(2, emg2_raw.read());      // Raw EMG 2 send to scope 2
+    //scope.set(3, emg2_filtered);        // Filtered EMG 2 send to scope 3
+    //scope.send();                       // Send the data to the computer
+}
+
+
+// --------------------------------------------------------------------
+// ---------------Read out encoders------------------------------------
+// --------------------------------------------------------------------    
+double  counts2angle1()
+{
+    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
+    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    return theta1;
+}
+
+double counts2angle2()
+{
+    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
+    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    return theta2;
+}
+
+// -------------------------------------------------------------------------
+// -------------- Determine Setpoints --------------------------------------
+// -------------------------------------------------------------------------
+
+void determinedemoset(){
+    setpointx = setpointx + dirx*need_to_move_1*v;
+    setpointy = setpointy + diry*need_to_move_2*v;
+    }
+
+//function to change the moving direction of the setpoint
+void ChangeDirectionX(){
+    dirx = -1*dirx;
+    }
+    
+void ChangeDirectionY(){
+    diry = -1*diry;
+    }    
+
+
+
+// -----------------------------------------------------------------    
+// --------------------------- PI controllers ----------------------
+// -----------------------------------------------------------------
+double PI_controller1(double error1)
+{
+    static double error_integral1 = 0;
+    
+    // Proportional part
+    double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral1 = error_integral1 + error1 * Ts1;  
+    double u_i1 = Ki1 * error_integral1;
+    
+    // Sum 
+    U1 = u_p1 + u_i1;
+    
+    // Return
+    return U1;      
+}
+double PI_controller2(double error2)
+{
+    static double error_integral2 = 0;
+    
+    // Proportional part
+    double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral2 = error_integral2 + error2 * Ts2;  
+    double u_i2 = Ki2 * error_integral2;
+    
+    // Sum +
+    U2 = u_p2 + u_i2;
+    
+    // Return
+    return U2;      
+} 
+
+// ------------------------------------------------------------
+// ------------ Inverse Kinematics ----------------------------
+// ------------------------------------------------------------
+double makeAngleq1(double x, double y){
+    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+    q1_diff = -(2.0*(q1-q1_0));                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return q1_diff;
+}
+
+double makeAngleq2(double x, double y){
+    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
+    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+    double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+    q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return q2_diff;
+}
+
+
+// -----------------------------------------------
+// ------------ RUN MOTORS -----------------------
+// -----------------------------------------------
+void motoraansturing()
+{
+    sample();
+    determinedemoset();
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+    //q1_diff_final = makeAngleq1(xfinal, yfinal);
+    //q2_diff_final = makeAngleq2(xfinal, yfinal);
+
+    theta2 = counts2angle2();           
+    error2 = q2_diff - theta2;
+    theta1 = counts2angle1();  
+    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+    
+    //errors die de setpoints bepalen
+    //error1_final = q1_diff_final - theta1;
+    //error2_final = q2_diff_final - theta2;
+
+    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PI_controller2(error2);   
+    
+    motor1_pwm.write(fabs(U1));         // Motor aansturen
+    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
+    motor2_pwm.write(fabs(U2));
+    directionM2 = U2 > 0.0f;     
+}
+
+
+void rundemo()
+{
+    motoraansturing();
+}
+
+
+int main()
+{
+    pc.baud(115200);
+    motor1_pwm.period_us(60);                // Period is 60 microseconde
+    motor2_pwm.period_us(60);
+    Demo.attach(&rundemo, 0.005f); 
+    
+    InterruptIn directionx(SW3);
+    directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
+    InterruptIn directiony(SW2);
+    directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
+    
+    pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
+    
+    while (true) {
+        //sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button
+        //sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button
+        if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
+            need_to_move_1 = 1; // The robot does have to move
+            }
+        else {
+            need_to_move_1 = 0; // If the robot does not have to move
+            }
+
+        if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
+            need_to_move_2 = 1;
+            }
+        else {
+            need_to_move_2 = 0;
+            }
+        pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f, movex: %i, movey: %i\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2,need_to_move_1,need_to_move_2);
+        
+        wait(0.5f);
+   
+    }
+}    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 02 08:23:22 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file