Diff: main.cpp

Revision:

40:c6dffb676350

Parent:

39:f9042483b921

Child:

41:8e8141f355af

--- a/main.cpp	Wed Oct 30 16:33:01 2019 +0000
+++ b/main.cpp	Wed Oct 30 19:37:41 2019 +0000
@@ -6,13 +6,15 @@
 #include "MODSERIAL.h"      // Serial connection to PC
 #include "BiQuad.h"         // Biquad filter management
 #include <vector>           // Array management
+#include "FastPWM.h"        // PWM
+#include "QEI.h"            // Encoder
 
 /*
 ------------------------------ DEFINE MBED CONNECTIONS ------------------------------
 */
 
 // PC connections
-HIDScope        scope( 4 );
+HIDScope        scope( 6 );
 MODSERIAL pc(USBTX, USBRX);
 
 // Buttons
@@ -27,9 +29,21 @@
 DigitalOut      led_b(LED_BLUE);
 
 // Analog EMG inputs
-AnalogIn emg1_in (A1); // Right biceps -> x axis
-AnalogIn emg2_in (A2); // Left biceps  -> y axis
-AnalogIn emg3_in (A3); // Third muscle -> TBD
+AnalogIn emg1_in (A0); // Right biceps -> x axis
+AnalogIn emg2_in (A1); // Left biceps  -> y axis
+AnalogIn emg3_in (A2); // Third muscle -> TBD
+
+// Encoder inputs
+DigitalIn encA1(D9);
+DigitalIn encB1(D8);
+DigitalIn encA2(D13);
+DigitalIn encB2(D12);
+
+// Motor outputs
+DigitalOut motor2Direction(D4);
+FastPWM motor2Power(D5);
+DigitalOut motor1Direction(D7);
+FastPWM motor1Power(D6);
 
 /*
 ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------
@@ -60,6 +74,13 @@
 bool emg_MVC_cal_done = false;
 bool emg_rest_cal_done = false;
 
+// Motor Substate variables
+enum Motor_States {motor_wait, motor_encoderset, motor_finish, motor_movement, failuremode};
+Motor_States motor_curr_state = motor_wait;
+bool motor_state_changed = true;
+
+bool motor_calibration_done = false;
+
 // Button press interrupts (to prevent bounce)
 bool button1_pressed = false;
 bool button2_pressed = false;
@@ -146,6 +167,282 @@
 }
 
 /*
+------------------------------ MOTOR CONTROL ------------------------------
+*/
+// Initialize encoder
+QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);     //Encoding motor 1
+QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);   //Encoding motor 2
+
+volatile int counts1; // Encoder counts
+volatile int counts2;
+volatile int counts1af;
+volatile int counts2af;
+int counts1offset;
+int counts2offset;
+volatile int countsPrev1 = 0;
+volatile int countsPrev2 = 0;
+volatile int deltaCounts1;
+volatile int deltaCounts2;
+
+// PWM period
+const float PWM_period = 1/(18*10e3);
+
+// Degree to radian convertor
+const float deg2rad = 0.0174532;                        //Conversion factor degree to rad
+const float rad2deg = 57.29578;                         //Conversion factor rad to degree
+
+// Motor angles in starting position
+float motor1angle=(-140.0-10.0)*deg2rad*5.5;    //Measured angle motor 1
+float motor2angle=(-10.0)*deg2rad*2.75;         //Measured angle motor 2
+
+const float factorin = 6.23185/64; // Convert encoder counts to angle in rad
+const float gearratio = 131.25; // Gear ratio of gearbox
+
+// GLobal variales
+float motor1offset;                             //Offset bij calibratie
+float motor2offset;
+float omega1;                                   //velocity rad/s motor 1
+float omega2;                                   //Velocity rad/s motor2
+bool motordir1;
+bool motordir2;
+float motor1ref=(-140.0-10.0)*deg2rad*5.5;
+float motor2ref=(-10.0)*deg2rad*2.75;
+double controlsignal1;
+double controlsignal2;
+double pi2= 6.283185;
+float motor1error;                              //motor 1 error
+float motor2error;
+float Kp=0.27;
+float Ki=0.35;
+float Kd=0.1;
+float u_p1;
+float u_p2;
+float u_i1;
+float u_i2;
+
+//Windup control
+float ux1;
+float ux2;
+float up1;                                      //Proportional contribution motor 1
+float up2;                                      //Proportional contribution motor 2
+float ek1;
+float ek2;
+float ei1= 0.0;                                 //Error integral motor 1
+float ei2=0.0;                                  //Error integral motor 2
+float Ka= 1.0;                                  //Integral windup gain
+
+//RKI
+float Vx=0.0;                                   //Desired linear velocity x direction
+float Vy=0.0;                                   //Desired linear velocity y direction
+float q1=-10.0f*deg2rad;                          //Angle of first joint [rad]
+float q2=-140.0f*deg2rad;                       //Angle of second joint [rad]
+float q1dot;                                    //Velocity of first joint [rad/s]
+float q2dot;                                    //Velocity of second joint [rad/s]
+float l1=26.0;                                  //Distance base-link [cm]
+float l2=62.0;                                  //Distance link-endpoint [cm]
+float xe;                                       //Endpoint x position [cm]
+float ye;                                       //Endpoint y position [cm]
+
+/*
+------------------------------ MOTOR FUNCTIONS ------------------------------
+*/
+void PID_controller()
+{
+
+    static float error_integral1=0;
+    static float e_prev1=motor1error;
+
+    //Proportional part:
+    u_p1=Kp*motor1error;
+
+    //Integral part
+    error_integral1=error_integral1+ei1*Ts;
+    u_i1=Ki*error_integral1;
+
+    //Derivative part
+    float error_derivative1=(motor1error-e_prev1)/Ts;
+    float u_d1=Kd*error_derivative1;
+    e_prev1=motor1error;
+
+    // Sum and limit
+    up1= u_p1+u_i1+u_d1;
+    if (up1>1) {
+        controlsignal1=1;
+    } else if (up1<-1) {
+        controlsignal1=-1;
+    } else {
+        controlsignal1=up1;
+    }
+
+    // To prevent windup
+    ux1= up1-controlsignal1;
+    ek1= Ka*ux1;
+    ei1= motor1error-ek1;
+
+// Motor 2
+
+    static float error_integral2=0;
+    static float e_prev2=motor2error;
+
+    //Proportional part:
+    u_p2=Kp*motor2error;
+
+    //Integral part
+    error_integral2=error_integral2+ei2*Ts;
+    u_i2=Ki*error_integral2;
+
+    //Derivative part
+    float error_derivative2=(motor2error-e_prev2)/Ts;
+    float u_d2=Kd*error_derivative2;
+    e_prev2=motor2error;
+
+    // Sum and limit
+    up2= u_p2+u_i2+u_d2;
+    if (up2>1.0f) {
+        controlsignal2=1.0f;
+    } else if (up2<-1) {
+        controlsignal2=-1.0f;
+    } else {
+        controlsignal2=up2;
+    }
+
+    // To prevent windup
+    ux2= up2-controlsignal2;
+    ek2= Ka*ux2;
+    ei2= motor2error-ek2;
+}
+
+void RKI()
+{
+    q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
+    q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
+    q1=q1+q1dot*Ts;
+    q2=q2+q2dot*Ts;
+
+    xe=l1*cos(q1)+l2*cos(q1+q2);
+    ye=l1*sin(q1)+l2*sin(q1+q2);
+
+
+
+    if (q1<-5.0f) {
+        q1=-5.0;
+    } else if (q1>65.0f*deg2rad) {
+        q1=65.0f*deg2rad;
+    } else {
+        q1=q1;
+    }
+
+    if (q2>-50.0*deg2rad) {
+        q2=-50.0*deg2rad;
+    } else if (q2<-138.0*deg2rad) {
+        q2=-138.0*deg2rad;
+    } else {
+        q2=q2;
+    }
+
+    motor1ref=5.5f*q1+5.5f*q2;
+    motor2ref=2.75f*q1;
+}
+
+void motorControl()
+{
+    counts1= counts1af-counts1offset;
+    motor1angle = (counts1 * factorin / gearratio)-((140.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
+    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    motor1error=motor1ref-motor1angle;
+    if (controlsignal1<0) {
+        motordir1= 0;
+    } else {
+        motordir1= 1;
+    }
+
+    motor1Power.write(abs(controlsignal1));
+    motor1Direction= motordir1;
+
+    counts2= counts2af - counts2offset;
+    motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1
+    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    motor2error=motor2ref-motor2angle;
+    if (controlsignal2<0) {
+        motordir2= 0;
+    } else {
+        motordir2= 1;
+    }
+    if (motor_calibration_done == true) {
+        motor2Power.write(abs(controlsignal2));
+    }
+    motor2Direction= motordir2;
+}
+
+/*
+------------------------------ MOTOR SUBSTATE FUNCTIONS ------------------------------
+*/
+
+void do_motor_wait()
+{
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+
+    PID_controller();
+    motorControl();
+
+    // State transition guard
+    if ( button2_pressed ) {
+        button2_pressed = false;
+        motor_curr_state = motor_encoderset;           //Beginnen met calibratie
+        motor_state_changed = true;
+        // More functions
+    }
+
+}
+
+void do_motor_Encoder_Set()
+{
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+    motor1Power.write(0.0f);
+    motor2Power.write(0.0f);
+    counts1offset = counts1af ;
+    counts2offset = counts2af;
+
+    // State transition guard
+    if ( button2_pressed ) {
+        button2_pressed = false;
+        motor_calibration_done = true;
+        motor_curr_state = motor_finish;
+        motor_state_changed = true;
+    }
+}
+
+void do_motor_finish()
+{
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+    }
+    
+    // Do stuff until end condition is true
+    PID_controller();
+    motorControl();
+    RKI();
+
+    // State transition guard
+    if ( button2_pressed ) {
+        button2_pressed = false;
+        motor_cal_done = true;
+        motor_curr_state = motor_wait;
+        motor_state_changed = true;
+    }
+
+}
+
+/*
 ------------------------------ EMG GLOBAL VARIABLES & CONSTANTS ------------------------------
 */
 
@@ -343,7 +640,7 @@
     // Entry function
     if ( emg_state_changed == true ) {
         emg_state_changed = false; // Disable entry functions
-        
+
         // Compute scale factors for all EMG signals
         double margin_percentage = 5; // Set up % margin for rest
         emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC
@@ -360,7 +657,7 @@
 
         emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
     }
-    
+
     // This state only runs its entry functions ONCE and then exits the EMG substate machine
 
     // State transition guard
@@ -393,6 +690,25 @@
 }
 
 /*
+------------------------------ MOTOR SUBSTATE MACHINE ------------------------------
+*/
+
+void motor_state_machine()
+{
+    switch(motor_curr_state) {
+        case motor_wait:
+            do_motor_wait();
+            break;
+        case motor_encoderset:
+            do_motor_Encoder_Set();
+            break;
+        case motor_finish:
+            do_motor_finish();
+            break;
+    }
+}
+
+/*
 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
 */
 /* ALL STATES HAVE THE FOLLOWING FORM:
@@ -426,6 +742,10 @@
     }
 
     // Do stuff until end condition is met
+    motor1Power.write(0.0f);
+    motor2Power.write(0.0f);
+    Vx=0.0f;
+    Vy=0.0f;
 
     // State transition guard
     if ( false ) { // Never move to other state
@@ -479,6 +799,7 @@
         button2_pressed = false;
         global_curr_state = global_motor_cal;
         global_state_changed = true;
+        
     } else if ( emg_cal_done && motor_cal_done ) { // OPERATION MODE
         global_curr_state = global_operation;
         global_state_changed = true;
@@ -512,7 +833,7 @@
     }
 
     // Do stuff until end condition is met
-    doStuff();
+    motor_state_machine();
 
     // State transition guard
     if ( emg_cal_done == true ) { // OPERATION MODE
@@ -533,7 +854,7 @@
     // Entry function
     if ( global_state_changed == true ) {
         global_state_changed = false;
-        
+
         emg_sampleNow = true; // Enable signal sampling in sampleSignals()
         emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
     }
@@ -559,6 +880,7 @@
     emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
     emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
     emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
+    Vx = emg1_out;
 
     // Idem for emg2
     if ( emg2_norm < emg2_th ) {
@@ -569,6 +891,7 @@
         emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
     }
     emg2_out = emg2_out * emg2_dir; // Set direction of EMG output
+    Vy = emg2_out;
 
     // Idem for emg3
     if ( emg3_norm < emg3_th ) {
@@ -578,6 +901,19 @@
     } else {
         emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
     }
+    
+    /*
+    // For testing Vx and Vy
+    static float t=0;
+    Vy=10.0f*sin(1.0f*t);
+    Vx=0.0f;
+    t+=Ts;
+    */
+    
+    // Move motors
+    PID_controller();
+    motorControl();
+    RKI();
 
     // Set HIDScope outputs
     scope.set(0, emg1 );
@@ -658,6 +994,14 @@
             // emg3_cal_size = emg1_cal.size(); // Used for debugging
         }
     }
+
+    counts1af = encoder1.getPulses();
+    deltaCounts1 = counts1af - countsPrev1;
+    countsPrev1 = counts1af;
+
+    counts2af = encoder2.getPulses();
+    deltaCounts2 = counts2af - countsPrev2;
+    countsPrev2 = counts2af;
 }
 
 /*
@@ -701,13 +1045,20 @@
     switch2.fall( &switch2Press );
     switch3.fall( &switch3Press );
     
+    // ---------- Attach PWM ----------
+    motor1Power.period(PWM_period);
+    motor2Power.period(PWM_period);
+
     // ---------- Turn OFF LEDs ----------
     led_b = 1;
     led_g = 1;
     led_r = 1;
 
     while(true) {
-        pc.printf("Global state: %i EMG substate: %i\r\n", global_curr_state, emg_curr_state);
+        pc.printf("Global state: %i EMG substate: %i Motor substate: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state);
+
+        pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
+        pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);
     }
 }
\ No newline at end of file