Diff: main.cpp

Revision:

33:97e69c32a768

Parent:

32:a779b1131977

Child:

34:6e74f6629a0e

diff -r a779b1131977 -r 97e69c32a768 main.cpp
--- a/main.cpp	Thu Nov 02 13:10:28 2017 +0000
+++ b/main.cpp	Fri Nov 03 00:25:57 2017 +0000
@@ -2,7 +2,7 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 #include "BiQuad.h"
-//#include "QEI.h"
+#include "QEI.h"
 
 //Define Objects
     AnalogIn    emg1_in( A0 ); // Read Out The Signal
@@ -16,21 +16,20 @@
     PwmOut motor1pwm( D5);
     PwmOut motor2pwm( D6 );
     DigitalOut motor2direction( D7 );
-    //QEI Encoder1(D10, D11, NC, 32); // Encoder
-    //QEI Encoder2(D12, D13, NC, 32); 
+    QEI Encoder1(D10, D11, NC, 64); // Encoder
+    QEI Encoder2(D12, D13, NC, 64); 
 
     Ticker      main_timer;
     Ticker      max_read1;
     Ticker      max_read3;
     Ticker      Motorcontrol;
-    HIDScope    scope( 6 );
+    HIDScope    scope( 4 );
     DigitalOut  red(LED_RED);
     DigitalOut  blue(LED_BLUE);
     DigitalOut  green(LED_GREEN);
-    MODSERIAL   pc(USBTX, USBRX);
+    Serial      pc(USBTX, USBRX);
 
-
-// EMG Variables
+// EMG Variables & Constants
     //Right Biceps
     double emg1;
     double emg1highfilter;
@@ -63,7 +62,7 @@
     double emg4lowfilter;
     double max4;
     double maxpart4;
-// Moving Average Floats
+    // Moving Average Floats
     // Right Biceps Movavg
     float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12, 
      RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24, 
@@ -102,10 +101,48 @@
      RL85, RL86, RL87, MOVAVG_RL;    
     float AV = 0.02; //multiplication value for adding each movavg value
     // This value also adds a very slight gain to every value
+
+// RKI Variables & Constants
+    float setpoint1;
+    float setpoint2;
+    float pi = 3.14159265359;
+    float a = 22.25;             //originally 22.25, makes for xinitial=10.766874   // length of arm 1
+    float b = 26.5;             //originally 16.48   makes for yinitial=15.733      // length of arm 2
+    float alpha_old = 2.35;     //INITIAL ANGLES IN RADIANS
+    float beta_old = 0.785;
+    float delta1;
+    float delta2;
+    float x = 10.77;
+    float y = 15.73;
+    float diffmotor_a;
+    float diffmotor_b;
+
+// PID Variables & Constants
+    double Kp = 0.5;// you can set these constants however you like depending on trial & error
+    double Ki = 0.1;
+    double Kd = 0.1;
+
+    float last_error1 = 0;
+    float new_error1 = 0;
+    float change_error1 = 0;
+    float total_error1 = 0;
+    float pid_term1 = 0;
+    float pid_term_scaled1 = 0;
     
-// Motor Variables
-    float MV1 = 0;
-    float MV2 = 0;
+    float last_error2 = 0;
+    float new_error2 = 0;
+    float change_error2 = 0;
+    float total_error2 = 0;
+    float pid_term2 = 0;
+    float pid_term_scaled2 = 0;
+    
+// Motor Variables & Constants
+    //float MV1 = 0;
+    //float MV2 = 0;
+    double count1 = 0;
+    double count2 = 0;
+    double angle1 = 2.35;
+    double angle2 = 0.785;
    
 // BiQuad Filter Settings
     // Right Biceps
@@ -126,9 +163,32 @@
     BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
     //
 
+// RKI Calculations
+//
+//
+
+float inversekinematics1(float x, float y){
+    // cosine law:
+    float c = sqrtf(x*x + y*y);                             // here comes the relevant math, see my inverse kinematics explaination using cosine law
+    float alpha1 = acosf((a*a + c*c - b*b)/(2*a*c));
+    
+    // remaining parts of alpha:
+    float alpha2 = atan2f(y,x);
+    float alpha = alpha1 + alpha2;
+    return alpha;
+}
+
+float inversekinematics2(float x, float y){
+    // cosine law:
+    float c = sqrtf(x*x + y*y);                             // here comes the relevant math, see my inverse kinematics explaination using cosine law
+    float beta = acosf((a*a + b*b - c*c)/(2*a*b));
+    return beta;
+}
+
 // Finding max values for correct motor switch if the button is pressed
+//
+//
 // calibration of both biceps
-
 void get_max1(){
     if (max_reader12==0){
             green = !green;
@@ -213,12 +273,11 @@
             wait(0.2f);
             green = 1;
     }
-    maxpart1 = 0.14*max1; // set cut off voltage at 13% of max for right biceps
+    maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
     maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
 }
 
 // calibration of both lower arms
-
 void get_max3(){
     if (max_reader34==0){
             green = 1;
@@ -307,7 +366,9 @@
     maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
 }
 
-// Filtering & Scope
+// EMG Filtering & Scope
+//
+//
 void filter() {
     // Right Biceps
     emg1 = emg1_in.read();
@@ -446,78 +507,100 @@
  // Compare measurement to the calibrated value to decide actions
  // more options could be added if the callibration is well defined enough
     // This part checks for right biceps contractions only
-    if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+/*    if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
+    wait(0.15f);*/
+        if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
             red = 1;
             blue = 1;
             green = 0;
-            MV1 = 0.5;
-            MV2 = 0;
-    }            
-    // This part checks for left biceps contractions only  
-    else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+            //MV1 = 0.5;
+            //MV2 = 0;
+            x = x + 0.5;
+            float alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
+            float beta = inversekinematics2(x,y);
+            setpoint1 = alpha;
+            setpoint2 = beta;
+        }            
+        // This part checks for left biceps contractions only  
+        else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
             red = 0;
             blue = 1;
             green = 1;
-            MV1 = -0.5;
-            MV2 = 0;
-    }
-    // This part checks for left lower arm contractions only  
-    else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
+            //MV1 = -0.5;
+            //MV2 = 0;
+            x = x - 0.5;
+            float alpha = inversekinematics1(x,y);
+            float beta = inversekinematics2(x,y);
+            setpoint1 = alpha;
+            setpoint2 = beta;
+        }
+        // This part checks for left lower arm contractions only  
+        else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
             red = 1;
             blue = 0;
             green = 1;
-            MV1 = 0;
-            MV2 = 0.5;
-    }
-    // This part checks for right lower arm contractions only
-    else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
+            //MV1 = 0;
+            //MV2 = 0.5;
+            y = y + 0.5;
+            float alpha = inversekinematics1(x,y);
+            float beta = inversekinematics2(x,y);
+            setpoint1 = alpha;
+            setpoint2 = beta;
+        }
+        // This part checks for right lower arm contractions only
+        else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
             red = 0;
             blue = 1;
             green = 0;
-            MV1 = 0;
-            MV2 = -0.5;
-    }           
-    // This part checks for both lower arm contractions only 
-    else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
+            //MV1 = 0;
+            //MV2 = -0.5;
+            y = y - 0.5;
+            float alpha = inversekinematics1(x,y);
+            float beta = inversekinematics2(x,y);
+            setpoint1 = alpha;
+            setpoint2 = beta;
+        }           
+/*        // This part checks for both lower arm contractions only 
+        else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
             red = 0;
             blue = 0;
             green = 0;
-            MV1 = -0.5;
-            MV2 = -0.5;
-    }
-    // This part checks for both biceps contractions only 
-    else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+            //MV1 = -0.5;
+            //MV2 = -0.5;
+        }
+        // This part checks for both biceps contractions only 
+        else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
             red = 0;
             blue = 0;
             green = 0;
-            MV1 = 0.5;
-            MV2 = 0.5;
-    }
-    // This part checks for right lower arm & left biceps contractions only
-    else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
+            //MV1 = 0.5;
+            //MV2 = 0.5;
+        }
+        // This part checks for right lower arm & left biceps contractions only
+        else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
             red = 0;
             blue = 0;
             green = 0;
-            MV1 = 0.5;
-            MV2 = -0.5;
-    }  
-    // This part checks for left lower arm & right biceps contractions only
-    else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
+            //MV1 = 0.5;
+            //MV2 = -0.5;
+        }  
+        // This part checks for left lower arm & right biceps contractions only
+        else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
             red = 0;
             blue = 0;
             green = 0;
-            MV1 = -0.5;
-            MV2 = 0.5;
-    }                                 
-    else {
-        red = 1; // Shut down all led colors if no movement is registered
-        blue = 1;
-        green = 1;
-        MV1 = 0;
-        MV2 = 0;
-        //pc.printf( "No contraction registered\n");
-    }
-
+            //MV1 = -0.5;
+            //MV2 = 0.5;
+        }*/
+    //}                                 
+        else {
+            red = 1; // Shut down all led colors if no movement is registered
+            blue = 1;
+            green = 1;
+            //MV1 = 0;
+            //MV2 = 0;
+        }
+    
     // 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, MOVAVG_RB ); // plot Right biceps voltage 
     scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage 
@@ -527,39 +610,95 @@
     
 }
 
-// check MV1 to see if motor1 needs to be activated
-void SetMotor1(float MV1) {
-    motor1pwm = fabs(MV1); // motor speed 
-    if (MV1 >=0) {
-    motor1direction = 1; // clockwise rotation 
+// PID calculations
+//
+//
+void PIDcalculation() {
+//PID calculation for motor 1
+    count1 = Encoder1.getPulses();
+    angle1 += (0.0981 * count1);
+    new_error1 = setpoint1 - alpha_old;
+    
+    change_error1 = new_error1 - last_error1;
+    total_error1 += new_error1;
+    pid_term1 = (Kp * new_error1) + (Ki * total_error1) + (Kd * change_error1);
+    if (pid_term1<-255) {
+    pid_term1 = -255;
+    }
+    if (pid_term1>255) {
+    pid_term1 = 255;
+    }
+    pid_term_scaled1 = abs(pid_term1);
+    
+    last_error1 = new_error1;
+    
+//PID calculation for motor 2
+    count2 = Encoder2.getPulses();
+    angle2 += (0.0981 * count2);
+    new_error2 = setpoint2 - beta_old;
+    
+    change_error2 = new_error2 - last_error2;
+    total_error2 += new_error2;
+    pid_term2 = (Kp * new_error2) + (Ki * total_error2) + (Kd * change_error2);
+    if (pid_term2<-255) {
+    pid_term2 = -255;
+    }
+    if (pid_term2>255) {
+    pid_term2 = 255;
+    }
+    pid_term_scaled2 = abs(pid_term2);
+    
+    last_error2 = new_error2;
+}
+   
+// Motor control
+//
+//
+// check to see if motor1 needs to be activated
+void SetMotor1(float angle1, float setpoint1) {
+    PIDcalculation();
+    if (angle1<setpoint1){
+    motor1direction = 0; // counterclockwise rotation
     }
     else {
-    motor1direction = 0; // counterclockwise rotation 
+    motor1direction = 1; // clockwise rotation
+    }
+    if (angle2<setpoint2 || angle2>setpoint2) {
+    motor2pwm = 0.5;
+    }
+    else if (angle2 == setpoint2){
+    motor2pwm = 0;
     }
 }
-// check MV2 if motor1 needs to be activated
-void SetMotor2(float MV2) {
-    motor2pwm = fabs(MV2); // motor speed 
-    if (MV2 >=0) {
+// check if motor1 needs to be activated
+void SetMotor2(float angle2, float setpoint2) {
+    PIDcalculation();
+    if (angle2<setpoint2){
+    motor2direction = 0; // counterclockwise rotation
+    }
+    else {
     motor2direction = 1; // clockwise rotation
     }
-    else {
-    motor2direction = 0; // counterclockwise rotation
+    if (angle2<setpoint2 || angle2>setpoint2) {
+    motor2pwm = 0.5;
+    }
+    else if (angle2 == setpoint2){
+    motor2pwm = 0;
     }
 }
 
-void MeasureAndControl(void)
-{
+void MeasureAndControl(void) {
+    
     // This function measures the potmeter position, extracts a
     // reference velocity from it, and controls the motor with 
     // a simple FeedForward controller. Call this from a Ticker.
-    SetMotor1(MV1);
-    SetMotor2(MV2);
+    SetMotor1(angle1,setpoint1);
+    SetMotor2(angle2,setpoint2);
 }
 
     
 int main(){   
-
+    pc.baud(115200);
     main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
     max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
     max_read3.attach(&get_max3, 2);