Revision 43:a6fbcada47aa, committed 2017-11-03

Comitter:

ralphg_92

Date:

Fri Nov 03 08:38:14 2017 +0000

Parent:

42:3aa03b5cefb0

Commit message:

revisiting old memories

Changed in this revision

--- a/PID.lib	Fri Nov 03 03:52:30 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19

--- a/main.cpp	Fri Nov 03 03:52:30 2017 +0000
+++ b/main.cpp	Fri Nov 03 08:38:14 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,21 @@
     PwmOut motor1pwm( D5);
     PwmOut motor2pwm( D6 );
     DigitalOut motor2direction( D7 );
-    QEI Encoder1(D10, D11, NC, 64); // Encoder
-    QEI Encoder2(D12, D13, NC, 64); 
+    //QEI Encoder1(D10, D11, NC, 32); // Encoder
+    //QEI Encoder2(D12, D13, NC, 32); 
 
     Ticker      main_timer;
     Ticker      max_read1;
     Ticker      max_read3;
     Ticker      Motorcontrol;
-    Ticker      tencoder;
-    HIDScope    scope( 4 );
+    HIDScope    scope( 6 );
     DigitalOut  red(LED_RED);
     DigitalOut  blue(LED_BLUE);
     DigitalOut  green(LED_GREEN);
-    Serial      pc(USBTX, USBRX);
+    MODSERIAL   pc(USBTX, USBRX);
 
-// EMG Variables & Constants
+
+// EMG Variables
     //Right Biceps
     double emg1;
     double emg1highfilter;
@@ -63,7 +63,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,48 +102,10 @@
      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
-    double setpoint1 = 2.35;
-    double setpoint2 = 0.785;
-    double x = 10.77;
-    double y = 15.73;
-    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
-    double alpha_old = 2.35;     //INITIAL ANGLES IN RADIANS
-    double beta_old = 0.785;
-    float delta1;
-    float delta2;
-    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;
     
-    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;
+// Motor Variables
+    float MV1 = 0;
+    float MV2 = 0;
    
 // BiQuad Filter Settings
     // Right Biceps
@@ -164,32 +126,9 @@
     BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
     //
 
-// RKI Calculations
-//
-//
+// Finding max values for correct motor switch if the button is pressed
+// calibration of both biceps
 
-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;
@@ -279,6 +218,7 @@
 }
 
 // calibration of both lower arms
+
 void get_max3(){
     if (max_reader34==0){
             green = 1;
@@ -367,10 +307,8 @@
     maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
 }
 
-// EMG Filtering & Scope
-//
-//
-void filter(/*double setpoint1, double setpoint2*/) {
+// Filtering & Scope
+void filter() {
     // Right Biceps
     emg1 = emg1_in.read();
     emg1highfilter = filterhigh1.step(emg1);
@@ -508,117 +446,81 @@
  // 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){
-    wait(0.15f);*/
-        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){
+    
+    if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
             red = 1;
             blue = 1;
             green = 0;
-            //MV1 = 0.5;
-            //MV2 = 0;
-            x = x + 0.1;
-            if (x > 16) {
-            x = 16;
-            }
-            double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
-            double beta = inversekinematics2(x,y);
-            setpoint1 = alpha;
-            setpoint2 = beta;
-            wait(0.1f);
-            
-        }            
-        // This part checks for left biceps contractions only  
-        else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+            MV1 = 0.08;
+            MV2 = 0;
+    }            
+    // 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;
-            x = x - 0.1;
-            if (x < 10.77){
-            x = 10.87;
-            }
-            double alpha = inversekinematics1(x,y);
-            double beta = inversekinematics2(x,y);
-            setpoint1 = alpha;
-            setpoint2 = beta;
-            wait(0.1f);
-        }
-        // 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.08;
+            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){
             red = 1;
             blue = 0;
             green = 1;
-            //MV1 = 0;
-            //MV2 = 0.5;
-            y = y + 0.1;
-            if (y > 19.5) {
-            y = 19.5;
-            }
-            double alpha = inversekinematics1(x,y);
-            double beta = inversekinematics2(x,y);
-            setpoint1 = alpha;
-            setpoint2 = beta;
-            wait(0.1f);
-        }
-        // 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.08;
+    }
+    // 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;
-            y = y - 0.1;
-            if (y < 15.73) {
-            y = 15.83;
-            }
-            double alpha = inversekinematics1(x,y);
-            double beta = inversekinematics2(x,y);
-            setpoint1 = alpha;
-            setpoint2 = beta;
-            wait(0.1f);
-        }           
-/*        // 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.08;
+    }           
+    // 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.08;
+            MV2 = -0.08;
+    }
+    // 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 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.08;
+            MV2 = 0.08;
+    }
+    // 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.08;
+            MV2 = -0.08;
+    }  
+    // 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;
-        }
-    
+            MV1 = -0.08;
+            MV2 = 0.08;
+    }
+    }                                 
+    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");
+    }
+
     // 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 
@@ -628,141 +530,42 @@
     
 }
 
-void encoders(){
-        count1 = Encoder1.getPulses();
-        count2 = Encoder2.getPulses();
-}
-
-/*
-
-// PID calculations
-//
-//
-void PIDcalculation1() {
-//PID calculation for motor 1
-    filter();
-    count1 = Encoder1.getPulses();
-    angle1 += (0.000748 * count1);
-    new_error1 = setpoint1 - angle1;
-    
-    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;
-}
-void PIDcalculation2() {    
-//PID calculation for motor 2
-    filter();
-    count2 = Encoder2.getPulses();
-    angle2 += (0.000748 * count2);
-    new_error2 = setpoint2 - angle2;
-    
-    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;
+// 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_term_scaled2 = abs(pid_term2);
-    
-    last_error2 = new_error2;
-}
- */ 
-// Motor control
-//
-//
-// check to see if motor1 needs to be activated
-void SetMotor1() {
-    //PIDcalculation1();
-    //filter();
-    while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
-        encoders();
-        float count1;
-        float count2;
-        angle1 += (0.0981 * count1);
-        angle2 += (0.0981 * count2);
-        if (angle1<setpoint1 && angle2<setpoint2) {
-        motor1direction = 1; // counterclockwise rotation
-        motor2direction = 1;
-        }
-        else if (angle1>setpoint1 && angle2<setpoint2) {
-        motor1direction = 0; // clockwise rotation
-        motor2direction = 1;
-        }
-        else if (angle1<setpoint1 && angle2>setpoint2) {
-        motor1direction = 1;
-        motor2direction = 0;
-        }
-        else if (angle1>setpoint1 && angle2>setpoint2) {
-        motor1direction = 0;
-        motor2direction = 0;
-        }
-        if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
-        motor1pwm = 0.2;
-        motor2pwm = 0.2;
-        }
-        else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
-        motor1pwm = 0.2;
-        motor2pwm = 0;
-        }
-        else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
-        motor1pwm = 0;
-        motor2pwm = 0.2;
-        }
-        else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
-        motor1pwm = 0;
-        motor2pwm = 0;
-        }
+    else {
+    motor1direction = 0; // counterclockwise rotation 
     }
 }
-/*
-// check if motor1 needs to be activated
-void SetMotor2() {
-    filter();
-    //PIDcalculation2();
-    while (angle2<setpoint2 || angle2>setpoint2) {
-        count2 = Encoder2.getPulses();
-        angle2 += (0.0981 * count2);
-        if (angle2<setpoint2){
-        motor1direction = 0; // counterclockwise rotation
-        }
-        else  if (angle2>setpoint2){
-        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) {
+    motor2direction = 1; // clockwise rotation
+    }
+    else {
+    motor2direction = 0; // counterclockwise rotation
     }
 }
 
-void MeasureAndControl(void) {
-    SetMotor1();
-    //SetMotor2();
+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);
 }
-*/
+
     
 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);
-    tencoder.attach(&encoders, 0.001);
-    Motorcontrol.attach(&SetMotor1,0.1);
-    //PIDtimer.attach(&PIDcalculation1, 0.005);
-    //PIDtimer.attach(&PIDcalculation2, 0.005);
+    Motorcontrol.attach(&MeasureAndControl,0.5);
     while(1) {}
 }
\ No newline at end of file