Diff: main.cpp

Revision:

3:c63c0a23ea21

Parent:

2:69bfc537508f

Child:

4:ca797e7daaf4

--- a/main.cpp	Thu Nov 02 18:37:18 2017 +0000
+++ b/main.cpp	Fri Nov 03 11:09:30 2017 +0000
@@ -7,6 +7,8 @@
 #include "QEI.h"
 #include "math.h"
 #include "iostream"
+#include "BiQuad.h"
+#include "TextLCD.h"
 #include "MODSERIAL.h"
 
 //intialize all pins
@@ -18,8 +20,15 @@
 QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING);
 DigitalIn button1(D8); //button to move cw
 DigitalIn button2(D9); //button to move ccw
-DigitalIn button3(D2);
-DigitalIn button4(D3);
+//DigitalIn button3(D2);
+//DigitalIn button4(D3);
+AnalogIn bicepsEMG(A0);
+AnalogIn tricepsEMG(A1);
+AnalogIn carpiFlexEMG(A2);
+AnalogIn palmarisEMG(A3);
+DigitalOut bit1(D2);
+DigitalOut bit2(D14);
+DigitalOut bit3(D15);
 MODSERIAL pc(USBTX, USBRX);
 
 //initialize variables
@@ -30,9 +39,9 @@
 const double motor1KI=3.0; //Integral gain of motor1 PI control
 const double motor1KD=3.0; // Differential gain of motor1 PID control
 
-const double motor2KP=1.3; //Proportional gain of motor1 PI control
-const double motor2KI=0.5; //Integral gain of motor1 PI control
-const double motor2KD=1.0; // Differential gain of motor1 PID control
+const double motor2KP=1.3; //Proportional gain of motor2 PI control
+const double motor2KI=0.5; //Integral gain of motor2 PI control
+const double motor2KD=1.0; // Differential gain of motor2 PID control
 
 const double N=100; //LP filter coefficient
 const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder
@@ -44,10 +53,99 @@
 
 double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller
 double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller
-double desiredAngle2 =0; //desired position of motor1
+double desiredAngle2 =0; //desired position of motor2
 //initialize ticker for checking and correcting the angle
 Ticker myControllerTicker;
 
+enum States {off,motorCalib, bicepsCalib, tricepsCalib, carpiCalib, palmarisCalib, demo, EMGControl};
+States state=off;
+bool stateChange=false;
+Timer flex;
+
+double tickerF =0.002;
+double bicepsMOVAG [20];
+double tricepsMOVAG [20];
+double carpiFlexMOVAG [20];
+double palmarisMOVAG [20];
+int MOVAGLength=20;
+
+//Notch Filter 50Hz Q of 0.7
+
+double bicepsNotcha0 = 0.7063988100714527;
+double bicepsNotcha1 = -1.1429772843080923;
+double bicepsNotcha2 = 0.7063988100714527;
+double bicepsNotchb1 = -1.1429772843080923;
+double bicepsNotchb2 = 0.41279762014290533;
+
+double tricepsNotcha0 = 0.7063988100714527;
+double tricepsNotcha1 = -1.1429772843080923;
+double tricepsNotcha2 = 0.7063988100714527;
+double tricepsNotchb1 = -1.1429772843080923;
+double tricepsNotchb2 = 0.41279762014290533;
+
+double carpiFlexNotcha0 = 0.7063988100714527;
+double carpiFlexNotcha1 = -1.1429772843080923;
+double carpiFlexNotcha2 = 0.7063988100714527;
+double carpiFlexNotchb1 = -1.1429772843080923;
+double carpiFlexNotchb2 = 0.41279762014290533;
+
+double palmarisNotcha0 = 0.7063988100714527;
+double palmarisNotcha1 = -1.1429772843080923;
+double palmarisNotcha2 = 0.7063988100714527;
+double palmarisNotchb1 = -1.1429772843080923;
+double palmarisNotchb2 = 0.41279762014290533;
+
+//Highpass filters 20Hz cutoff Q of 0.7
+double bicepsHigha0 = 0.8370879899975344;
+double bicepsHigha1 = -1.6741759799950688;
+double bicepsHigha2 = 0.8370879899975344;
+double bicepsHighb1 = -1.6474576182593796;
+double bicepsHighb2 = 0.7008943417307579;
+
+double tricepsHigha0 = 0.8370879899975344;
+double tricepsHigha1 = -1.6741759799950688;
+double tricepsHigha2 = 0.8370879899975344;
+double tricepsHighb1 = -1.6474576182593796;
+double tricepsHighb2 = 0.7008943417307579;
+
+double carpiFlexHigha0 = 0.8370879899975344;
+double carpiFlexHigha1 = -1.6741759799950688;
+double carpiFlexHigha2 = 0.8370879899975344;
+double carpiFlexHighb1 = -1.6474576182593796;
+double carpiFlexHighb2 = 0.7008943417307579;
+
+double palmarisHigha0 = 0.8370879899975344;
+double palmarisHigha1 = -1.6741759799950688;
+double palmarisHigha2 = 0.8370879899975344;
+double palmarisHighb1 = -1.6474576182593796;
+double palmarisHighb2 = 0.7008943417307579;
+
+BiQuad bicepsNotch (bicepsNotcha0,bicepsNotcha1,bicepsNotcha2,bicepsNotchb1,bicepsNotchb2);
+BiQuad tricepsNotch (tricepsNotcha0,tricepsNotcha1,tricepsNotcha2,tricepsNotchb1,tricepsNotchb2);
+BiQuad carpiFlexNotch (carpiFlexNotcha0,carpiFlexNotcha1,carpiFlexNotcha2,carpiFlexNotchb1,carpiFlexNotchb2);
+BiQuad palmarisNotch (palmarisNotcha0,palmarisNotcha1,palmarisNotcha2, palmarisNotchb1,palmarisNotchb2);
+
+BiQuad bicepsHigh (bicepsHigha0,bicepsHigha1,bicepsHigha2,bicepsHighb1,bicepsHighb2);
+BiQuad tricepsHigh (tricepsHigha0,tricepsHigha1,tricepsHigha2,tricepsHighb1,tricepsHighb2);
+BiQuad carpiFlexHigh (carpiFlexHigha0,carpiFlexHigha1,carpiFlexHigha2,carpiFlexHighb1,carpiFlexHighb2);
+BiQuad palmarisHigh (palmarisHigha0,palmarisHigha1,palmarisHigha2, palmarisHighb1,palmarisHighb2);
+
+double bicepsAvg;
+double tricepsAvg;
+double carpiFlexAvg;
+double palmarisAvg;
+
+double bicepsMax=0;
+double tricepsMax=0;
+double carpiFlexMax=0;
+double palmarisMax=0;
+
+double bicepsMulti=0;
+double tricepsMulti=0;
+double carpiFlexMulti=0;
+double palmarisMulti=0;
+Ticker filterTicker;
+
 const float l1 = 460; //Length of the arm from base to joint 1 (arm1)   ENTER MANUALLY [mm]
 const float l2 = 450; //length of the arm from joint 1 to the end-effector.(arm2)   ENTER MANUALLY [mm]
 float x_des = l1+l2; //(initial)desired x location of the end-effector (ee)
@@ -72,6 +170,158 @@
     return u;
 }
 
+void sendState(States s)
+{
+    if(s==motorCalib) {
+        bit1=1;
+        bit2=0;
+        bit3=0;
+    } else if(s==bicepsCalib) {
+        bit1=0;
+        bit2=1;
+        bit3=0;
+    } else if(s==tricepsCalib) {
+        bit1=1;
+        bit2=1;
+        bit3=0;
+    } else if(s==carpiCalib) {
+        bit1=0;
+        bit2=0;
+        bit3=1;
+    } else if(s==palmarisCalib) {
+        bit1=1;
+        bit2=0;
+        bit3=1;
+    } else if(s==demo) {
+        bit1=0;
+        bit2=1;
+        bit3=1;
+    } else if(s==EMGControl) {
+        bit1=1;
+        bit2=1;
+        bit3=1;
+    }
+}
+
+void filterBiceps()
+{
+    double bicepsSignal = bicepsEMG.read();
+    double bicepsFiltered = bicepsSignal;
+
+    //Filter out 50Hz from all signals
+    bicepsFiltered = bicepsNotch.step(bicepsFiltered);
+
+    //Highpass filtering
+    bicepsFiltered = bicepsHigh.step(bicepsFiltered);
+    //Rectification
+    bicepsFiltered = fabs(bicepsFiltered);
+
+    //caculate moving average
+    for(int i=0; i<MOVAGLength-1; i++) {
+        bicepsMOVAG[i]=bicepsMOVAG[i+1];
+    }
+
+    bicepsMOVAG[MOVAGLength-1]=bicepsFiltered;
+
+    double bicepsSum;
+
+    for(int i=0; i<MOVAGLength; i++) {
+        bicepsSum+= bicepsMOVAG[i];
+    }
+
+    bicepsAvg= bicepsSum/MOVAGLength;
+}
+void filterTriceps()
+{
+    double tricepsSignal = tricepsEMG.read();
+    double tricepsFiltered = tricepsSignal;
+
+    //Filter out 50Hz from all signals
+    tricepsFiltered = tricepsNotch.step(tricepsFiltered);
+
+    //Highpass filtering
+    tricepsFiltered = tricepsHigh.step(tricepsFiltered);
+
+    //Rectification
+    tricepsFiltered = fabs(tricepsFiltered);
+
+    //caculate moving average
+    for(int i=0; i<MOVAGLength-1; i++) {
+        tricepsMOVAG[i]=tricepsMOVAG[i+1];
+    }
+
+    tricepsMOVAG[MOVAGLength-1]=tricepsFiltered;
+
+    double tricepsSum;
+
+    for(int i=0; i<MOVAGLength; i++) {
+        tricepsSum+= tricepsMOVAG[i];
+    }
+    tricepsAvg= tricepsSum/MOVAGLength;
+}
+
+void filterCarpiFlex()
+{
+    double carpiFlexSignal = carpiFlexEMG.read();
+    double carpiFlexFiltered = carpiFlexSignal;
+
+    //Filter out 50Hz from all signals
+    carpiFlexFiltered = carpiFlexNotch.step(carpiFlexFiltered);
+
+    //Highpass filtering
+    carpiFlexFiltered = carpiFlexHigh.step(carpiFlexFiltered);
+
+    //Rectification
+    carpiFlexFiltered = fabs(carpiFlexFiltered);
+
+    //caculate moving average
+    for(int i=0; i<MOVAGLength-1; i++) {
+        carpiFlexMOVAG[i]=carpiFlexMOVAG[i+1];
+    }
+
+    carpiFlexMOVAG[MOVAGLength-1]=carpiFlexFiltered;
+
+    double carpiFlexSum;
+    for(int i=0; i<MOVAGLength; i++) {
+        carpiFlexSum+= carpiFlexMOVAG[i];
+    }
+
+    carpiFlexAvg= carpiFlexSum/MOVAGLength;
+}
+void filterPalmaris()
+{
+    double palmarisSignal = palmarisEMG.read();
+    double palmarisFiltered = palmarisSignal;
+
+    //Filter out 50Hz from all signals
+    palmarisFiltered = palmarisNotch.step(palmarisFiltered);
+
+    //Highpass filtering
+    palmarisFiltered = palmarisHigh.step(palmarisFiltered);
+
+    //Rectification
+    palmarisFiltered = fabs(palmarisFiltered);
+
+    //caculate moving average
+    for(int i=0; i<MOVAGLength-1; i++) {
+        palmarisMOVAG[i]=palmarisMOVAG[i+1];
+    }
+    palmarisMOVAG[MOVAGLength-1]=palmarisFiltered;
+
+    double palmarisSum;
+    for(int i=0; i<MOVAGLength; i++) {
+        palmarisSum+= palmarisMOVAG[i];
+    }
+    palmarisAvg= palmarisSum/MOVAGLength;
+}
+void filter()
+{
+    filterBiceps();
+    filterTriceps();
+    filterCarpiFlex();
+    filterPalmaris();
+}
+
 //Code for motor angles as a function of the length and positions of the arms, for a double revolutional joint arm in 2D plane
 void motorAngle()
 {
@@ -81,40 +331,57 @@
 //Works for all 4 quadrants of the (unit) circle
     xe = x_des;
     ye = y_des;
+    
+   //while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)){ //when attempted to go to a point out of reach
+//       if (y_des == 0){    //make sure you do not divide by 0 if ye == 0
+//           xe = x_des - 1;
+//       } else {
+//           xe = x_des - (x_des/y_des)/10; //go to a smaller xe point on the same line
+//       }
+//       if (x_des == 0){    //make sure you do not divide by 0 if xe == 0
+//           ye = y_des - 1;
+//       } else {
+//           ye = y_des - (y_des/x_des)/10; //go to a smaller ye point on the same line
+//       }
+//       x_des = xe;
+//       y_des = ye;
+//   }
 
-    while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
-        if (xe > 0) {            //right hand plane
-            if (ye > 0) {        //positive x & y            QUADRANT 1
-                xe = x_des - (x_des/y_des);   //go to a smaller xe point on the same line
-                ye = y_des - (y_des/x_des);   //go to a smaller ye point on the same line
-            } else if (ye < 0) { //positive x, negative y    QUADRANT 2
-                xe = x_des - (x_des/y_des);   //smaller xe
-                ye = y_des + (y_des/x_des);   //greater ye
-            } else if (ye == 0) { //positive x, y == 0
-                xe = x_des - 1;                  //stay on the x-axis but at a smaller value (within reach)
-            }
-        } else if (xe < 0) {     //left hand plane
-            if (ye > 0) {        //negative x, positive y    QUADRANT 4
-                xe = x_des + (x_des/y_des);   //greater xe
-                ye = y_des - (y_des/x_des);   //smaller ye
-            } else if (ye < 0) { //negative x & y            QUADRANT 3
-                xe = x_des + (x_des/y_des);   //greater xe
-                ye = y_des + (y_des/x_des);   //greater ye
-            } else if (ye ==0) { //negative x, y == 0
-                xe = x_des + 1;                  //stay on the x-axis but at a greater value (within reach)
-            }
 
-        } else if (xe == 0) {    //on the y-axis
-            if (ye > 0) {        //x == 0, positive y
-                ye = y_des - 1;                  //stay on the y-axis but at a smaller value (within reach)
-            } else if (ye < 0) { //x == 0, negative y
-                ye = y_des + 1;                  //stay on the y-axis but at a greater value (within reach)
-                //x == 0 & y == 0 is a state that is unable to be reached, no need to cover that case
-            }
-            x_des = xe;
-            y_des = ye;
-        }
-    }
+   // while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
+//        if (xe > 0) {            //right hand plane
+//            if (ye > 0) {        //positive x & y            QUADRANT 1
+//                xe = x_des - (x_des/y_des);   //go to a smaller xe point on the same line
+//                //(x_des/y_des) can be multiplied or divided to make bigger or smaller steps back if you're trying to exceed max. reach
+//                ye = y_des - (y_des/x_des);   //go to a smaller ye point on the same line
+//            } else if (ye < 0) { //positive x, negative y    QUADRANT 2
+//                xe = x_des - (x_des/y_des);   //smaller xe
+//                ye = y_des + (y_des/x_des);   //greater ye
+//            } else if (ye == 0) { //positive x, y == 0
+//                xe = x_des - 1;                  //stay on the x-axis but at a smaller value (within reach)
+//            }
+//        } else if (xe < 0) {     //left hand plane
+//            if (ye > 0) {        //negative x, positive y    QUADRANT 4
+//                xe = x_des + (x_des/y_des);   //greater xe
+//                ye = y_des - (y_des/x_des);   //smaller ye
+//            } else if (ye < 0) { //negative x & y            QUADRANT 3
+//                xe = x_des + (x_des/y_des);   //greater xe
+//                ye = y_des + (y_des/x_des);   //greater ye
+//            } else if (ye ==0) { //negative x, y == 0
+//                xe = x_des + 1;                  //stay on the x-axis but at a greater value (within reach)
+//            }
+//
+//        } else if (xe == 0) {    //on the y-axis
+//            if (ye > 0) {        //x == 0, positive y
+//                ye = y_des - 1;                  //stay on the y-axis but at a smaller value (within reach)
+//            } else if (ye < 0) { //x == 0, negative y
+//                ye = y_des + 1;                  //stay on the y-axis but at a greater value (within reach)
+//                //x == 0 & y == 0 is a state that is unable to be reached, no need to cover that case
+//            }
+//            x_des = xe;
+//            y_des = ye;
+//        }
+//    }
 
 
 //from here on is the code for setting the angles for the motors
@@ -178,28 +445,154 @@
     pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
     pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n \r", q1, desiredAngle1, q2, desiredAngle2);
     myControllerTicker.attach(&motorButtonController, controllerTickerTime);
-    while(1) {
+    bit1=0;
+    bit2=0;
+    bit3=0;
+    wait(5);
+    for(int i=0; i<MOVAGLength; i++) {
+        bicepsMOVAG[i]=0;
+        tricepsMOVAG[i]=0;
+        carpiFlexMOVAG[i]=0;
+        palmarisMOVAG[i]=0;
+    }
+    filterTicker.attach(filter,tickerF);
+
+    //start up
+    state=bicepsCalib;
+    stateChange=true;
+
+    while(true) {
         if(!button1) {
-            y_des += positionIncrement;
+            x_des -= positionIncrement;
             motorAngle();
-            wait(0.3f);
+            wait(0.4f);
         }
 
         if(!button2) {
-            y_des -= positionIncrement;
+            x_des += positionIncrement;
             motorAngle();
-            wait(0.3f);
+            wait(0.4f);
         }
-
-        if(button3) {
+        if(bicepsAvg*bicepsMulti > 0.9){
+            x_des -= positionIncrement;
+            motorAngle();
+            wait(0.5f);    
+        }
+        if(tricepsAvg*tricepsMulti > 0.9){
             x_des += positionIncrement;
             motorAngle();
-            wait(0.3f);
+            wait(0.5f);
         }
-        if(button4) {
-            x_des -= positionIncrement;
-            motorAngle();
-            wait(0.3f);
+//
+//        if(button3) {
+//            x_des += positionIncrement;
+//            motorAngle();
+//            wait(0.3f);
+//        }
+//        if(button4) {
+//            x_des -= positionIncrement;
+//            motorAngle();
+//            wait(0.3f);
+//        }
+        if(stateChange) {
+            switch(state) {
+                case motorCalib  :
+                    stateChange=false;
+                    sendState(motorCalib);
+                    break;
+                case bicepsCalib  :
+                    stateChange=false;
+                    sendState(bicepsCalib);
+                    wait(5);
+                    flex.reset();
+                    flex.start();
+                    while(flex.read()<2) {
+                        if(bicepsAvg>bicepsMax) {
+                            bicepsMax=bicepsAvg;
+                        }
+                    }
+                    bit1 = 0;
+                    bit2 = 0;
+                    bit3 = 0;
+                    wait(5);
+                    state=tricepsCalib;
+                    stateChange=true;
+                    break;
+                case tricepsCalib  :
+                    stateChange=false;
+                    sendState(tricepsCalib);
+                    wait(5);
+                    flex.reset();
+                    flex.start();
+                    while(flex.read()<2) {
+                        if(tricepsAvg>tricepsMax) {
+                            tricepsMax=tricepsAvg;
+                        }
+                    }
+                    bit1 = 0;
+                    bit2 = 0;
+                    bit3 = 0;
+                    wait(5);
+                    state=carpiCalib;
+                    stateChange=true;
+                    break;
+                case carpiCalib  :
+                    stateChange=false;
+                    sendState(carpiCalib);
+                    wait(5);
+                    flex.reset();
+                    flex.start();
+                    while(flex.read()<2) {
+                        if(carpiFlexAvg>carpiFlexMax) {
+                            carpiFlexMax=carpiFlexAvg;
+                        }
+                    }
+                    bit1 = 0;
+                    bit2 = 0;
+                    bit3 = 0;
+                    wait(5);
+                    state=palmarisCalib;
+                    stateChange=true;
+                    break;
+                case palmarisCalib  :
+                    stateChange=false;
+                    sendState(palmarisCalib);
+                    wait(5);
+                    flex.reset();
+                    flex.start();
+                    while(flex.read()<2) {
+                        if(palmarisAvg>palmarisMax) {
+                            palmarisMax=palmarisAvg;
+                        }
+                    }
+                    bit1 = 0;
+                    bit2 = 0;
+                    bit3 = 0;
+                    wait(5);
+                    state=EMGControl;
+                    stateChange=true;
+                    break;
+                case demo  :
+                    stateChange=false;
+                    sendState(demo);
+                    break;
+                case EMGControl  :
+                    stateChange=false;
+                    sendState(EMGControl);
+                    if(bicepsMax != 0){
+                    bicepsMulti=1/bicepsMax;
+                    }
+                    if(tricepsMax != 0){
+                    tricepsMulti=1/tricepsMax;
+                    }
+                    if(carpiFlexMax != 0){
+                    carpiFlexMulti=1/carpiFlexMax;
+                    }
+                    if(palmarisMax != 0){
+                    palmarisMulti=1/palmarisMax;
+                    }
+                    break;
+            }
         }
     }
 }
\ No newline at end of file