Diff: main.cpp

Revision:

0:0fd6a09e85d9

--- /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