Thijs de Kleijn
mbed motor control with emg
Dependencies: Encoder HIDScope MODSERIAL QEI TextLCD biquadFilter mbed
Fork of
2MotorPID
by 
Adam Bako
main.cpp@3:c63c0a23ea21, 2017-11-03 (annotated)
- Committer:
- Frimzenner
- Date:
- Fri Nov 03 11:09:30 2017 +0000
- Revision:
- 3:c63c0a23ea21
- Parent:
- 2:69bfc537508f
- Child:
- 4:ca797e7daaf4
including emg signal processing;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| bako | 0:46cf63cba59a | 1 | /* |
| bako | 0:46cf63cba59a | 2 | * Parts of the code copied from PES lecture slides |
| bako | 0:46cf63cba59a | 3 | */ |
| bako | 0:46cf63cba59a | 4 | |
| bako | 0:46cf63cba59a | 5 | // include all necessary libraries |
| bako | 0:46cf63cba59a | 6 | #include "mbed.h" |
| bako | 0:46cf63cba59a | 7 | #include "QEI.h" |
| Frimzenner | 1:864a5f8bb886 | 8 | #include "math.h" |
| Frimzenner | 2:69bfc537508f | 9 | #include "iostream" |
| Frimzenner | 3:c63c0a23ea21 | 10 | #include "BiQuad.h" |
| Frimzenner | 3:c63c0a23ea21 | 11 | #include "TextLCD.h" |
| Frimzenner | 2:69bfc537508f | 12 | #include "MODSERIAL.h" |
| bako | 0:46cf63cba59a | 13 | |
| bako | 0:46cf63cba59a | 14 | //intialize all pins |
| bako | 0:46cf63cba59a | 15 | PwmOut motor1(D5); |
| bako | 0:46cf63cba59a | 16 | PwmOut motor2(D6); |
| bako | 0:46cf63cba59a | 17 | DigitalOut motor1Dir(D4); // direction of motor 1 (1 is ccw 0 is cw (looking at the shaft from the front)) |
| bako | 0:46cf63cba59a | 18 | DigitalOut motor2Dir(D7); // direction of motor 2 (1 is ccw 0 is cw (looking at the shaft from the front)) |
| bako | 0:46cf63cba59a | 19 | QEI motor1Encoder (D10,D11, NC, 624,QEI::X4_ENCODING); |
| bako | 0:46cf63cba59a | 20 | QEI motor2Encoder (D12,D13, NC, 624,QEI::X4_ENCODING); |
| bako | 0:46cf63cba59a | 21 | DigitalIn button1(D8); //button to move cw |
| bako | 0:46cf63cba59a | 22 | DigitalIn button2(D9); //button to move ccw |
| Frimzenner | 3:c63c0a23ea21 | 23 | //DigitalIn button3(D2); |
| Frimzenner | 3:c63c0a23ea21 | 24 | //DigitalIn button4(D3); |
| Frimzenner | 3:c63c0a23ea21 | 25 | AnalogIn bicepsEMG(A0); |
| Frimzenner | 3:c63c0a23ea21 | 26 | AnalogIn tricepsEMG(A1); |
| Frimzenner | 3:c63c0a23ea21 | 27 | AnalogIn carpiFlexEMG(A2); |
| Frimzenner | 3:c63c0a23ea21 | 28 | AnalogIn palmarisEMG(A3); |
| Frimzenner | 3:c63c0a23ea21 | 29 | DigitalOut bit1(D2); |
| Frimzenner | 3:c63c0a23ea21 | 30 | DigitalOut bit2(D14); |
| Frimzenner | 3:c63c0a23ea21 | 31 | DigitalOut bit3(D15); |
| Frimzenner | 2:69bfc537508f | 32 | MODSERIAL pc(USBTX, USBRX); |
| bako | 0:46cf63cba59a | 33 | |
| bako | 0:46cf63cba59a | 34 | //initialize variables |
| Frimzenner | 1:864a5f8bb886 | 35 | const float pi = 3.14159265358979323846; //value for pi |
| Frimzenner | 2:69bfc537508f | 36 | double positionIncrement = 20; // increment of angle when button pressed (1 is a whole rotation (360 degrees)) |
| bako | 0:46cf63cba59a | 37 | |
| Frimzenner | 2:69bfc537508f | 38 | const double motor1KP=7.0; //Proportional gain of motor1 PI control |
| Frimzenner | 2:69bfc537508f | 39 | const double motor1KI=3.0; //Integral gain of motor1 PI control |
| Frimzenner | 2:69bfc537508f | 40 | const double motor1KD=3.0; // Differential gain of motor1 PID control |
| bako | 0:46cf63cba59a | 41 | |
| Frimzenner | 3:c63c0a23ea21 | 42 | const double motor2KP=1.3; //Proportional gain of motor2 PI control |
| Frimzenner | 3:c63c0a23ea21 | 43 | const double motor2KI=0.5; //Integral gain of motor2 PI control |
| Frimzenner | 3:c63c0a23ea21 | 44 | const double motor2KD=1.0; // Differential gain of motor2 PID control |
| bako | 0:46cf63cba59a | 45 | |
| bako | 0:46cf63cba59a | 46 | const double N=100; //LP filter coefficient |
| bako | 0:46cf63cba59a | 47 | const double encoderToMotor= 0.000119047619047619; //proportion of the rotation of the motor to the rotation of the encoder |
| bako | 0:46cf63cba59a | 48 | const double controllerTickerTime=0.01; //ticker frequency |
| bako | 0:46cf63cba59a | 49 | |
| bako | 0:46cf63cba59a | 50 | double motor1ErrorInt=0; //error of motor1 for the integrating part of PI controller |
| bako | 0:46cf63cba59a | 51 | double motor1ErrorDif=0; //error of motor1 for the integrating part of PI controller |
| Frimzenner | 1:864a5f8bb886 | 52 | double desiredAngle1 =0; //desired position of motor1 |
| bako | 0:46cf63cba59a | 53 | |
| bako | 0:46cf63cba59a | 54 | double motor2ErrorInt=0; //error of motor1 for the integrating part of PI controller |
| bako | 0:46cf63cba59a | 55 | double motor2ErrorDif=0; //error of motor1 for the integrating part of PI controller |
| Frimzenner | 3:c63c0a23ea21 | 56 | double desiredAngle2 =0; //desired position of motor2 |
| bako | 0:46cf63cba59a | 57 | //initialize ticker for checking and correcting the angle |
| bako | 0:46cf63cba59a | 58 | Ticker myControllerTicker; |
| bako | 0:46cf63cba59a | 59 | |
| Frimzenner | 3:c63c0a23ea21 | 60 | enum States {off,motorCalib, bicepsCalib, tricepsCalib, carpiCalib, palmarisCalib, demo, EMGControl}; |
| Frimzenner | 3:c63c0a23ea21 | 61 | States state=off; |
| Frimzenner | 3:c63c0a23ea21 | 62 | bool stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 63 | Timer flex; |
| Frimzenner | 3:c63c0a23ea21 | 64 | |
| Frimzenner | 3:c63c0a23ea21 | 65 | double tickerF =0.002; |
| Frimzenner | 3:c63c0a23ea21 | 66 | double bicepsMOVAG [20]; |
| Frimzenner | 3:c63c0a23ea21 | 67 | double tricepsMOVAG [20]; |
| Frimzenner | 3:c63c0a23ea21 | 68 | double carpiFlexMOVAG [20]; |
| Frimzenner | 3:c63c0a23ea21 | 69 | double palmarisMOVAG [20]; |
| Frimzenner | 3:c63c0a23ea21 | 70 | int MOVAGLength=20; |
| Frimzenner | 3:c63c0a23ea21 | 71 | |
| Frimzenner | 3:c63c0a23ea21 | 72 | //Notch Filter 50Hz Q of 0.7 |
| Frimzenner | 3:c63c0a23ea21 | 73 | |
| Frimzenner | 3:c63c0a23ea21 | 74 | double bicepsNotcha0 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 75 | double bicepsNotcha1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 76 | double bicepsNotcha2 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 77 | double bicepsNotchb1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 78 | double bicepsNotchb2 = 0.41279762014290533; |
| Frimzenner | 3:c63c0a23ea21 | 79 | |
| Frimzenner | 3:c63c0a23ea21 | 80 | double tricepsNotcha0 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 81 | double tricepsNotcha1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 82 | double tricepsNotcha2 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 83 | double tricepsNotchb1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 84 | double tricepsNotchb2 = 0.41279762014290533; |
| Frimzenner | 3:c63c0a23ea21 | 85 | |
| Frimzenner | 3:c63c0a23ea21 | 86 | double carpiFlexNotcha0 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 87 | double carpiFlexNotcha1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 88 | double carpiFlexNotcha2 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 89 | double carpiFlexNotchb1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 90 | double carpiFlexNotchb2 = 0.41279762014290533; |
| Frimzenner | 3:c63c0a23ea21 | 91 | |
| Frimzenner | 3:c63c0a23ea21 | 92 | double palmarisNotcha0 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 93 | double palmarisNotcha1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 94 | double palmarisNotcha2 = 0.7063988100714527; |
| Frimzenner | 3:c63c0a23ea21 | 95 | double palmarisNotchb1 = -1.1429772843080923; |
| Frimzenner | 3:c63c0a23ea21 | 96 | double palmarisNotchb2 = 0.41279762014290533; |
| Frimzenner | 3:c63c0a23ea21 | 97 | |
| Frimzenner | 3:c63c0a23ea21 | 98 | //Highpass filters 20Hz cutoff Q of 0.7 |
| Frimzenner | 3:c63c0a23ea21 | 99 | double bicepsHigha0 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 100 | double bicepsHigha1 = -1.6741759799950688; |
| Frimzenner | 3:c63c0a23ea21 | 101 | double bicepsHigha2 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 102 | double bicepsHighb1 = -1.6474576182593796; |
| Frimzenner | 3:c63c0a23ea21 | 103 | double bicepsHighb2 = 0.7008943417307579; |
| Frimzenner | 3:c63c0a23ea21 | 104 | |
| Frimzenner | 3:c63c0a23ea21 | 105 | double tricepsHigha0 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 106 | double tricepsHigha1 = -1.6741759799950688; |
| Frimzenner | 3:c63c0a23ea21 | 107 | double tricepsHigha2 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 108 | double tricepsHighb1 = -1.6474576182593796; |
| Frimzenner | 3:c63c0a23ea21 | 109 | double tricepsHighb2 = 0.7008943417307579; |
| Frimzenner | 3:c63c0a23ea21 | 110 | |
| Frimzenner | 3:c63c0a23ea21 | 111 | double carpiFlexHigha0 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 112 | double carpiFlexHigha1 = -1.6741759799950688; |
| Frimzenner | 3:c63c0a23ea21 | 113 | double carpiFlexHigha2 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 114 | double carpiFlexHighb1 = -1.6474576182593796; |
| Frimzenner | 3:c63c0a23ea21 | 115 | double carpiFlexHighb2 = 0.7008943417307579; |
| Frimzenner | 3:c63c0a23ea21 | 116 | |
| Frimzenner | 3:c63c0a23ea21 | 117 | double palmarisHigha0 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 118 | double palmarisHigha1 = -1.6741759799950688; |
| Frimzenner | 3:c63c0a23ea21 | 119 | double palmarisHigha2 = 0.8370879899975344; |
| Frimzenner | 3:c63c0a23ea21 | 120 | double palmarisHighb1 = -1.6474576182593796; |
| Frimzenner | 3:c63c0a23ea21 | 121 | double palmarisHighb2 = 0.7008943417307579; |
| Frimzenner | 3:c63c0a23ea21 | 122 | |
| Frimzenner | 3:c63c0a23ea21 | 123 | BiQuad bicepsNotch (bicepsNotcha0,bicepsNotcha1,bicepsNotcha2,bicepsNotchb1,bicepsNotchb2); |
| Frimzenner | 3:c63c0a23ea21 | 124 | BiQuad tricepsNotch (tricepsNotcha0,tricepsNotcha1,tricepsNotcha2,tricepsNotchb1,tricepsNotchb2); |
| Frimzenner | 3:c63c0a23ea21 | 125 | BiQuad carpiFlexNotch (carpiFlexNotcha0,carpiFlexNotcha1,carpiFlexNotcha2,carpiFlexNotchb1,carpiFlexNotchb2); |
| Frimzenner | 3:c63c0a23ea21 | 126 | BiQuad palmarisNotch (palmarisNotcha0,palmarisNotcha1,palmarisNotcha2, palmarisNotchb1,palmarisNotchb2); |
| Frimzenner | 3:c63c0a23ea21 | 127 | |
| Frimzenner | 3:c63c0a23ea21 | 128 | BiQuad bicepsHigh (bicepsHigha0,bicepsHigha1,bicepsHigha2,bicepsHighb1,bicepsHighb2); |
| Frimzenner | 3:c63c0a23ea21 | 129 | BiQuad tricepsHigh (tricepsHigha0,tricepsHigha1,tricepsHigha2,tricepsHighb1,tricepsHighb2); |
| Frimzenner | 3:c63c0a23ea21 | 130 | BiQuad carpiFlexHigh (carpiFlexHigha0,carpiFlexHigha1,carpiFlexHigha2,carpiFlexHighb1,carpiFlexHighb2); |
| Frimzenner | 3:c63c0a23ea21 | 131 | BiQuad palmarisHigh (palmarisHigha0,palmarisHigha1,palmarisHigha2, palmarisHighb1,palmarisHighb2); |
| Frimzenner | 3:c63c0a23ea21 | 132 | |
| Frimzenner | 3:c63c0a23ea21 | 133 | double bicepsAvg; |
| Frimzenner | 3:c63c0a23ea21 | 134 | double tricepsAvg; |
| Frimzenner | 3:c63c0a23ea21 | 135 | double carpiFlexAvg; |
| Frimzenner | 3:c63c0a23ea21 | 136 | double palmarisAvg; |
| Frimzenner | 3:c63c0a23ea21 | 137 | |
| Frimzenner | 3:c63c0a23ea21 | 138 | double bicepsMax=0; |
| Frimzenner | 3:c63c0a23ea21 | 139 | double tricepsMax=0; |
| Frimzenner | 3:c63c0a23ea21 | 140 | double carpiFlexMax=0; |
| Frimzenner | 3:c63c0a23ea21 | 141 | double palmarisMax=0; |
| Frimzenner | 3:c63c0a23ea21 | 142 | |
| Frimzenner | 3:c63c0a23ea21 | 143 | double bicepsMulti=0; |
| Frimzenner | 3:c63c0a23ea21 | 144 | double tricepsMulti=0; |
| Frimzenner | 3:c63c0a23ea21 | 145 | double carpiFlexMulti=0; |
| Frimzenner | 3:c63c0a23ea21 | 146 | double palmarisMulti=0; |
| Frimzenner | 3:c63c0a23ea21 | 147 | Ticker filterTicker; |
| Frimzenner | 3:c63c0a23ea21 | 148 | |
| Frimzenner | 1:864a5f8bb886 | 149 | const float l1 = 460; //Length of the arm from base to joint 1 (arm1) ENTER MANUALLY [mm] |
| Frimzenner | 1:864a5f8bb886 | 150 | const float l2 = 450; //length of the arm from joint 1 to the end-effector.(arm2) ENTER MANUALLY [mm] |
| Frimzenner | 2:69bfc537508f | 151 | float x_des = l1+l2; //(initial)desired x location of the end-effector (ee) |
| Frimzenner | 2:69bfc537508f | 152 | float y_des = 0; //(initial) desired y location of the end-effector (ee) |
| Frimzenner | 1:864a5f8bb886 | 153 | float xe, ye, D, phi, q2, beta, alpha, q1; //other variables used in calculating the angle for the motors |
| Frimzenner | 2:69bfc537508f | 154 | float radDeg, rad2rot; |
| Frimzenner | 1:864a5f8bb886 | 155 | //q1 is the angle for the base motor, q2 is the angle for the elbow motor, both in [rad] |
| bako | 0:46cf63cba59a | 156 | |
| Frimzenner | 2:69bfc537508f | 157 | |
| Frimzenner | 2:69bfc537508f | 158 | double PIDController(double error, const double Kp, const double Ki, const double Kd, double Ts, const double N,double &intError, double &DifError) |
| Frimzenner | 2:69bfc537508f | 159 | { |
| bako | 0:46cf63cba59a | 160 | const double a1 = -4/(N*Ts+2); |
| bako | 0:46cf63cba59a | 161 | const double a2 = -(N*Ts-2)/(N*Ts+2); |
| bako | 0:46cf63cba59a | 162 | const double b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4); |
| bako | 0:46cf63cba59a | 163 | const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2); |
| bako | 0:46cf63cba59a | 164 | const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4); |
| bako | 0:46cf63cba59a | 165 | |
| bako | 0:46cf63cba59a | 166 | double v = error - a1*intError - a2*DifError; |
| bako | 0:46cf63cba59a | 167 | double u = b0*v + b1*intError + b2*DifError; |
| Frimzenner | 1:864a5f8bb886 | 168 | DifError = intError; |
| Frimzenner | 1:864a5f8bb886 | 169 | intError = v; |
| bako | 0:46cf63cba59a | 170 | return u; |
| bako | 0:46cf63cba59a | 171 | } |
| bako | 0:46cf63cba59a | 172 | |
| Frimzenner | 3:c63c0a23ea21 | 173 | void sendState(States s) |
| Frimzenner | 3:c63c0a23ea21 | 174 | { |
| Frimzenner | 3:c63c0a23ea21 | 175 | if(s==motorCalib) { |
| Frimzenner | 3:c63c0a23ea21 | 176 | bit1=1; |
| Frimzenner | 3:c63c0a23ea21 | 177 | bit2=0; |
| Frimzenner | 3:c63c0a23ea21 | 178 | bit3=0; |
| Frimzenner | 3:c63c0a23ea21 | 179 | } else if(s==bicepsCalib) { |
| Frimzenner | 3:c63c0a23ea21 | 180 | bit1=0; |
| Frimzenner | 3:c63c0a23ea21 | 181 | bit2=1; |
| Frimzenner | 3:c63c0a23ea21 | 182 | bit3=0; |
| Frimzenner | 3:c63c0a23ea21 | 183 | } else if(s==tricepsCalib) { |
| Frimzenner | 3:c63c0a23ea21 | 184 | bit1=1; |
| Frimzenner | 3:c63c0a23ea21 | 185 | bit2=1; |
| Frimzenner | 3:c63c0a23ea21 | 186 | bit3=0; |
| Frimzenner | 3:c63c0a23ea21 | 187 | } else if(s==carpiCalib) { |
| Frimzenner | 3:c63c0a23ea21 | 188 | bit1=0; |
| Frimzenner | 3:c63c0a23ea21 | 189 | bit2=0; |
| Frimzenner | 3:c63c0a23ea21 | 190 | bit3=1; |
| Frimzenner | 3:c63c0a23ea21 | 191 | } else if(s==palmarisCalib) { |
| Frimzenner | 3:c63c0a23ea21 | 192 | bit1=1; |
| Frimzenner | 3:c63c0a23ea21 | 193 | bit2=0; |
| Frimzenner | 3:c63c0a23ea21 | 194 | bit3=1; |
| Frimzenner | 3:c63c0a23ea21 | 195 | } else if(s==demo) { |
| Frimzenner | 3:c63c0a23ea21 | 196 | bit1=0; |
| Frimzenner | 3:c63c0a23ea21 | 197 | bit2=1; |
| Frimzenner | 3:c63c0a23ea21 | 198 | bit3=1; |
| Frimzenner | 3:c63c0a23ea21 | 199 | } else if(s==EMGControl) { |
| Frimzenner | 3:c63c0a23ea21 | 200 | bit1=1; |
| Frimzenner | 3:c63c0a23ea21 | 201 | bit2=1; |
| Frimzenner | 3:c63c0a23ea21 | 202 | bit3=1; |
| Frimzenner | 3:c63c0a23ea21 | 203 | } |
| Frimzenner | 3:c63c0a23ea21 | 204 | } |
| Frimzenner | 3:c63c0a23ea21 | 205 | |
| Frimzenner | 3:c63c0a23ea21 | 206 | void filterBiceps() |
| Frimzenner | 3:c63c0a23ea21 | 207 | { |
| Frimzenner | 3:c63c0a23ea21 | 208 | double bicepsSignal = bicepsEMG.read(); |
| Frimzenner | 3:c63c0a23ea21 | 209 | double bicepsFiltered = bicepsSignal; |
| Frimzenner | 3:c63c0a23ea21 | 210 | |
| Frimzenner | 3:c63c0a23ea21 | 211 | //Filter out 50Hz from all signals |
| Frimzenner | 3:c63c0a23ea21 | 212 | bicepsFiltered = bicepsNotch.step(bicepsFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 213 | |
| Frimzenner | 3:c63c0a23ea21 | 214 | //Highpass filtering |
| Frimzenner | 3:c63c0a23ea21 | 215 | bicepsFiltered = bicepsHigh.step(bicepsFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 216 | //Rectification |
| Frimzenner | 3:c63c0a23ea21 | 217 | bicepsFiltered = fabs(bicepsFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 218 | |
| Frimzenner | 3:c63c0a23ea21 | 219 | //caculate moving average |
| Frimzenner | 3:c63c0a23ea21 | 220 | for(int i=0; i<MOVAGLength-1; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 221 | bicepsMOVAG[i]=bicepsMOVAG[i+1]; |
| Frimzenner | 3:c63c0a23ea21 | 222 | } |
| Frimzenner | 3:c63c0a23ea21 | 223 | |
| Frimzenner | 3:c63c0a23ea21 | 224 | bicepsMOVAG[MOVAGLength-1]=bicepsFiltered; |
| Frimzenner | 3:c63c0a23ea21 | 225 | |
| Frimzenner | 3:c63c0a23ea21 | 226 | double bicepsSum; |
| Frimzenner | 3:c63c0a23ea21 | 227 | |
| Frimzenner | 3:c63c0a23ea21 | 228 | for(int i=0; i<MOVAGLength; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 229 | bicepsSum+= bicepsMOVAG[i]; |
| Frimzenner | 3:c63c0a23ea21 | 230 | } |
| Frimzenner | 3:c63c0a23ea21 | 231 | |
| Frimzenner | 3:c63c0a23ea21 | 232 | bicepsAvg= bicepsSum/MOVAGLength; |
| Frimzenner | 3:c63c0a23ea21 | 233 | } |
| Frimzenner | 3:c63c0a23ea21 | 234 | void filterTriceps() |
| Frimzenner | 3:c63c0a23ea21 | 235 | { |
| Frimzenner | 3:c63c0a23ea21 | 236 | double tricepsSignal = tricepsEMG.read(); |
| Frimzenner | 3:c63c0a23ea21 | 237 | double tricepsFiltered = tricepsSignal; |
| Frimzenner | 3:c63c0a23ea21 | 238 | |
| Frimzenner | 3:c63c0a23ea21 | 239 | //Filter out 50Hz from all signals |
| Frimzenner | 3:c63c0a23ea21 | 240 | tricepsFiltered = tricepsNotch.step(tricepsFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 241 | |
| Frimzenner | 3:c63c0a23ea21 | 242 | //Highpass filtering |
| Frimzenner | 3:c63c0a23ea21 | 243 | tricepsFiltered = tricepsHigh.step(tricepsFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 244 | |
| Frimzenner | 3:c63c0a23ea21 | 245 | //Rectification |
| Frimzenner | 3:c63c0a23ea21 | 246 | tricepsFiltered = fabs(tricepsFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 247 | |
| Frimzenner | 3:c63c0a23ea21 | 248 | //caculate moving average |
| Frimzenner | 3:c63c0a23ea21 | 249 | for(int i=0; i<MOVAGLength-1; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 250 | tricepsMOVAG[i]=tricepsMOVAG[i+1]; |
| Frimzenner | 3:c63c0a23ea21 | 251 | } |
| Frimzenner | 3:c63c0a23ea21 | 252 | |
| Frimzenner | 3:c63c0a23ea21 | 253 | tricepsMOVAG[MOVAGLength-1]=tricepsFiltered; |
| Frimzenner | 3:c63c0a23ea21 | 254 | |
| Frimzenner | 3:c63c0a23ea21 | 255 | double tricepsSum; |
| Frimzenner | 3:c63c0a23ea21 | 256 | |
| Frimzenner | 3:c63c0a23ea21 | 257 | for(int i=0; i<MOVAGLength; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 258 | tricepsSum+= tricepsMOVAG[i]; |
| Frimzenner | 3:c63c0a23ea21 | 259 | } |
| Frimzenner | 3:c63c0a23ea21 | 260 | tricepsAvg= tricepsSum/MOVAGLength; |
| Frimzenner | 3:c63c0a23ea21 | 261 | } |
| Frimzenner | 3:c63c0a23ea21 | 262 | |
| Frimzenner | 3:c63c0a23ea21 | 263 | void filterCarpiFlex() |
| Frimzenner | 3:c63c0a23ea21 | 264 | { |
| Frimzenner | 3:c63c0a23ea21 | 265 | double carpiFlexSignal = carpiFlexEMG.read(); |
| Frimzenner | 3:c63c0a23ea21 | 266 | double carpiFlexFiltered = carpiFlexSignal; |
| Frimzenner | 3:c63c0a23ea21 | 267 | |
| Frimzenner | 3:c63c0a23ea21 | 268 | //Filter out 50Hz from all signals |
| Frimzenner | 3:c63c0a23ea21 | 269 | carpiFlexFiltered = carpiFlexNotch.step(carpiFlexFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 270 | |
| Frimzenner | 3:c63c0a23ea21 | 271 | //Highpass filtering |
| Frimzenner | 3:c63c0a23ea21 | 272 | carpiFlexFiltered = carpiFlexHigh.step(carpiFlexFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 273 | |
| Frimzenner | 3:c63c0a23ea21 | 274 | //Rectification |
| Frimzenner | 3:c63c0a23ea21 | 275 | carpiFlexFiltered = fabs(carpiFlexFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 276 | |
| Frimzenner | 3:c63c0a23ea21 | 277 | //caculate moving average |
| Frimzenner | 3:c63c0a23ea21 | 278 | for(int i=0; i<MOVAGLength-1; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 279 | carpiFlexMOVAG[i]=carpiFlexMOVAG[i+1]; |
| Frimzenner | 3:c63c0a23ea21 | 280 | } |
| Frimzenner | 3:c63c0a23ea21 | 281 | |
| Frimzenner | 3:c63c0a23ea21 | 282 | carpiFlexMOVAG[MOVAGLength-1]=carpiFlexFiltered; |
| Frimzenner | 3:c63c0a23ea21 | 283 | |
| Frimzenner | 3:c63c0a23ea21 | 284 | double carpiFlexSum; |
| Frimzenner | 3:c63c0a23ea21 | 285 | for(int i=0; i<MOVAGLength; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 286 | carpiFlexSum+= carpiFlexMOVAG[i]; |
| Frimzenner | 3:c63c0a23ea21 | 287 | } |
| Frimzenner | 3:c63c0a23ea21 | 288 | |
| Frimzenner | 3:c63c0a23ea21 | 289 | carpiFlexAvg= carpiFlexSum/MOVAGLength; |
| Frimzenner | 3:c63c0a23ea21 | 290 | } |
| Frimzenner | 3:c63c0a23ea21 | 291 | void filterPalmaris() |
| Frimzenner | 3:c63c0a23ea21 | 292 | { |
| Frimzenner | 3:c63c0a23ea21 | 293 | double palmarisSignal = palmarisEMG.read(); |
| Frimzenner | 3:c63c0a23ea21 | 294 | double palmarisFiltered = palmarisSignal; |
| Frimzenner | 3:c63c0a23ea21 | 295 | |
| Frimzenner | 3:c63c0a23ea21 | 296 | //Filter out 50Hz from all signals |
| Frimzenner | 3:c63c0a23ea21 | 297 | palmarisFiltered = palmarisNotch.step(palmarisFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 298 | |
| Frimzenner | 3:c63c0a23ea21 | 299 | //Highpass filtering |
| Frimzenner | 3:c63c0a23ea21 | 300 | palmarisFiltered = palmarisHigh.step(palmarisFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 301 | |
| Frimzenner | 3:c63c0a23ea21 | 302 | //Rectification |
| Frimzenner | 3:c63c0a23ea21 | 303 | palmarisFiltered = fabs(palmarisFiltered); |
| Frimzenner | 3:c63c0a23ea21 | 304 | |
| Frimzenner | 3:c63c0a23ea21 | 305 | //caculate moving average |
| Frimzenner | 3:c63c0a23ea21 | 306 | for(int i=0; i<MOVAGLength-1; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 307 | palmarisMOVAG[i]=palmarisMOVAG[i+1]; |
| Frimzenner | 3:c63c0a23ea21 | 308 | } |
| Frimzenner | 3:c63c0a23ea21 | 309 | palmarisMOVAG[MOVAGLength-1]=palmarisFiltered; |
| Frimzenner | 3:c63c0a23ea21 | 310 | |
| Frimzenner | 3:c63c0a23ea21 | 311 | double palmarisSum; |
| Frimzenner | 3:c63c0a23ea21 | 312 | for(int i=0; i<MOVAGLength; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 313 | palmarisSum+= palmarisMOVAG[i]; |
| Frimzenner | 3:c63c0a23ea21 | 314 | } |
| Frimzenner | 3:c63c0a23ea21 | 315 | palmarisAvg= palmarisSum/MOVAGLength; |
| Frimzenner | 3:c63c0a23ea21 | 316 | } |
| Frimzenner | 3:c63c0a23ea21 | 317 | void filter() |
| Frimzenner | 3:c63c0a23ea21 | 318 | { |
| Frimzenner | 3:c63c0a23ea21 | 319 | filterBiceps(); |
| Frimzenner | 3:c63c0a23ea21 | 320 | filterTriceps(); |
| Frimzenner | 3:c63c0a23ea21 | 321 | filterCarpiFlex(); |
| Frimzenner | 3:c63c0a23ea21 | 322 | filterPalmaris(); |
| Frimzenner | 3:c63c0a23ea21 | 323 | } |
| Frimzenner | 3:c63c0a23ea21 | 324 | |
| Frimzenner | 1:864a5f8bb886 | 325 | //Code for motor angles as a function of the length and positions of the arms, for a double revolutional joint arm in 2D plane |
| Frimzenner | 2:69bfc537508f | 326 | void motorAngle() |
| Frimzenner | 2:69bfc537508f | 327 | { |
| Frimzenner | 1:864a5f8bb886 | 328 | //Function for making sure the arm does not exceed its maximum reach |
| Frimzenner | 1:864a5f8bb886 | 329 | //if it tries to go beyond its max. reach |
| Frimzenner | 1:864a5f8bb886 | 330 | //it will try to reach a point within reach in the same direction as desired. |
| Frimzenner | 2:69bfc537508f | 331 | //Works for all 4 quadrants of the (unit) circle |
| Frimzenner | 1:864a5f8bb886 | 332 | xe = x_des; |
| Frimzenner | 1:864a5f8bb886 | 333 | ye = y_des; |
| Frimzenner | 3:c63c0a23ea21 | 334 | |
| Frimzenner | 3:c63c0a23ea21 | 335 | //while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)){ //when attempted to go to a point out of reach |
| Frimzenner | 3:c63c0a23ea21 | 336 | // if (y_des == 0){ //make sure you do not divide by 0 if ye == 0 |
| Frimzenner | 3:c63c0a23ea21 | 337 | // xe = x_des - 1; |
| Frimzenner | 3:c63c0a23ea21 | 338 | // } else { |
| Frimzenner | 3:c63c0a23ea21 | 339 | // xe = x_des - (x_des/y_des)/10; //go to a smaller xe point on the same line |
| Frimzenner | 3:c63c0a23ea21 | 340 | // } |
| Frimzenner | 3:c63c0a23ea21 | 341 | // if (x_des == 0){ //make sure you do not divide by 0 if xe == 0 |
| Frimzenner | 3:c63c0a23ea21 | 342 | // ye = y_des - 1; |
| Frimzenner | 3:c63c0a23ea21 | 343 | // } else { |
| Frimzenner | 3:c63c0a23ea21 | 344 | // ye = y_des - (y_des/x_des)/10; //go to a smaller ye point on the same line |
| Frimzenner | 3:c63c0a23ea21 | 345 | // } |
| Frimzenner | 3:c63c0a23ea21 | 346 | // x_des = xe; |
| Frimzenner | 3:c63c0a23ea21 | 347 | // y_des = ye; |
| Frimzenner | 3:c63c0a23ea21 | 348 | // } |
| Frimzenner | 2:69bfc537508f | 349 | |
| Frimzenner | 2:69bfc537508f | 350 | |
| Frimzenner | 3:c63c0a23ea21 | 351 | // while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) { |
| Frimzenner | 3:c63c0a23ea21 | 352 | // if (xe > 0) { //right hand plane |
| Frimzenner | 3:c63c0a23ea21 | 353 | // if (ye > 0) { //positive x & y QUADRANT 1 |
| Frimzenner | 3:c63c0a23ea21 | 354 | // xe = x_des - (x_des/y_des); //go to a smaller xe point on the same line |
| Frimzenner | 3:c63c0a23ea21 | 355 | // //(x_des/y_des) can be multiplied or divided to make bigger or smaller steps back if you're trying to exceed max. reach |
| Frimzenner | 3:c63c0a23ea21 | 356 | // ye = y_des - (y_des/x_des); //go to a smaller ye point on the same line |
| Frimzenner | 3:c63c0a23ea21 | 357 | // } else if (ye < 0) { //positive x, negative y QUADRANT 2 |
| Frimzenner | 3:c63c0a23ea21 | 358 | // xe = x_des - (x_des/y_des); //smaller xe |
| Frimzenner | 3:c63c0a23ea21 | 359 | // ye = y_des + (y_des/x_des); //greater ye |
| Frimzenner | 3:c63c0a23ea21 | 360 | // } else if (ye == 0) { //positive x, y == 0 |
| Frimzenner | 3:c63c0a23ea21 | 361 | // xe = x_des - 1; //stay on the x-axis but at a smaller value (within reach) |
| Frimzenner | 3:c63c0a23ea21 | 362 | // } |
| Frimzenner | 3:c63c0a23ea21 | 363 | // } else if (xe < 0) { //left hand plane |
| Frimzenner | 3:c63c0a23ea21 | 364 | // if (ye > 0) { //negative x, positive y QUADRANT 4 |
| Frimzenner | 3:c63c0a23ea21 | 365 | // xe = x_des + (x_des/y_des); //greater xe |
| Frimzenner | 3:c63c0a23ea21 | 366 | // ye = y_des - (y_des/x_des); //smaller ye |
| Frimzenner | 3:c63c0a23ea21 | 367 | // } else if (ye < 0) { //negative x & y QUADRANT 3 |
| Frimzenner | 3:c63c0a23ea21 | 368 | // xe = x_des + (x_des/y_des); //greater xe |
| Frimzenner | 3:c63c0a23ea21 | 369 | // ye = y_des + (y_des/x_des); //greater ye |
| Frimzenner | 3:c63c0a23ea21 | 370 | // } else if (ye ==0) { //negative x, y == 0 |
| Frimzenner | 3:c63c0a23ea21 | 371 | // xe = x_des + 1; //stay on the x-axis but at a greater value (within reach) |
| Frimzenner | 3:c63c0a23ea21 | 372 | // } |
| Frimzenner | 3:c63c0a23ea21 | 373 | // |
| Frimzenner | 3:c63c0a23ea21 | 374 | // } else if (xe == 0) { //on the y-axis |
| Frimzenner | 3:c63c0a23ea21 | 375 | // if (ye > 0) { //x == 0, positive y |
| Frimzenner | 3:c63c0a23ea21 | 376 | // ye = y_des - 1; //stay on the y-axis but at a smaller value (within reach) |
| Frimzenner | 3:c63c0a23ea21 | 377 | // } else if (ye < 0) { //x == 0, negative y |
| Frimzenner | 3:c63c0a23ea21 | 378 | // ye = y_des + 1; //stay on the y-axis but at a greater value (within reach) |
| Frimzenner | 3:c63c0a23ea21 | 379 | // //x == 0 & y == 0 is a state that is unable to be reached, no need to cover that case |
| Frimzenner | 3:c63c0a23ea21 | 380 | // } |
| Frimzenner | 3:c63c0a23ea21 | 381 | // x_des = xe; |
| Frimzenner | 3:c63c0a23ea21 | 382 | // y_des = ye; |
| Frimzenner | 3:c63c0a23ea21 | 383 | // } |
| Frimzenner | 3:c63c0a23ea21 | 384 | // } |
| Frimzenner | 1:864a5f8bb886 | 385 | |
| Frimzenner | 2:69bfc537508f | 386 | |
| Frimzenner | 1:864a5f8bb886 | 387 | //from here on is the code for setting the angles for the motors |
| Frimzenner | 1:864a5f8bb886 | 388 | D = ((pow(l1,2)+pow(l2,2))-pow(xe,2)-pow(ye,2))/(2*l1*l2); //D = cos(phi) |
| Frimzenner | 1:864a5f8bb886 | 389 | phi = atan2(sqrt(1 - pow(D, 2)), D); //angle between arm1 and arm2, from arm1 to arm2 [rad] |
| Frimzenner | 1:864a5f8bb886 | 390 | //Use atan2(sqrt(1 - pow(D, 2)),D) for "elbow down" position (like your right arm) |
| Frimzenner | 1:864a5f8bb886 | 391 | q2 = pi - phi; //angle of arm2 with respect to the orientation of arm1, motor2 [rad] |
| Frimzenner | 1:864a5f8bb886 | 392 | if (-pi/2 > q2) { //Make sure the angle of motor2 doesn’t wreck our setup (max -90 or 90 degrees w.r.t. arm1) |
| Frimzenner | 2:69bfc537508f | 393 | q2 = 0; |
| Frimzenner | 1:864a5f8bb886 | 394 | } else if ( q2 > pi/2) { |
| Frimzenner | 1:864a5f8bb886 | 395 | q2 = pi/2; |
| Frimzenner | 1:864a5f8bb886 | 396 | } |
| Frimzenner | 1:864a5f8bb886 | 397 | beta = atan2(ye, xe); //angle between "line from origin to ee" and x-axis [rad] |
| Frimzenner | 1:864a5f8bb886 | 398 | alpha = atan2(l2*sin(q2), l1+l2*cos(q2)); //angle between "line from origin to ee" and arm1 [rad] |
| Frimzenner | 1:864a5f8bb886 | 399 | q1 = beta - alpha; //angle of arm 1 with respect to the x-axis, motor1 [rad] |
| Frimzenner | 2:69bfc537508f | 400 | radDeg = 180/pi; |
| Frimzenner | 2:69bfc537508f | 401 | rad2rot = radDeg/360; |
| Frimzenner | 1:864a5f8bb886 | 402 | desiredAngle1 = q1 * rad2rot; |
| Frimzenner | 1:864a5f8bb886 | 403 | desiredAngle2 = q2 * rad2rot; |
| Frimzenner | 2:69bfc537508f | 404 | pc.printf("\n New values \n\r"); |
| Frimzenner | 2:69bfc537508f | 405 | pc.printf("xloc = %f, yloc = %f \n \r", xe, ye); |
| Frimzenner | 2:69bfc537508f | 406 | pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n\r", q1, desiredAngle1, q2, desiredAngle2); |
| Frimzenner | 1:864a5f8bb886 | 407 | } |
| bako | 0:46cf63cba59a | 408 | |
| Frimzenner | 2:69bfc537508f | 409 | void motorButtonController() |
| Frimzenner | 2:69bfc537508f | 410 | { |
| Frimzenner | 2:69bfc537508f | 411 | double angle1= encoderToMotor*motor1Encoder.getPulses(); |
| Frimzenner | 2:69bfc537508f | 412 | double angleError1 = (desiredAngle1 - angle1)*1.25; |
| Frimzenner | 1:864a5f8bb886 | 413 | |
| bako | 0:46cf63cba59a | 414 | //change direction based on error sign |
| Frimzenner | 2:69bfc537508f | 415 | if(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)>0) { |
| bako | 0:46cf63cba59a | 416 | motor1Dir=0; |
| bako | 0:46cf63cba59a | 417 | } else { |
| bako | 0:46cf63cba59a | 418 | motor1Dir =1; |
| bako | 0:46cf63cba59a | 419 | } |
| bako | 0:46cf63cba59a | 420 | //set motor speed based on PI controller error |
| Frimzenner | 2:69bfc537508f | 421 | motor1 = fabs(PIDController( angleError1, motor1KP, motor1KI,motor1KD, controllerTickerTime, N, motor1ErrorInt ,motor1ErrorDif)); |
| Frimzenner | 1:864a5f8bb886 | 422 | |
| Frimzenner | 2:69bfc537508f | 423 | double angle2= encoderToMotor*motor2Encoder.getPulses(); |
| Frimzenner | 2:69bfc537508f | 424 | double angleError2 = desiredAngle2 - angle2; |
| Frimzenner | 1:864a5f8bb886 | 425 | |
| bako | 0:46cf63cba59a | 426 | //change direction based on error sign |
| Frimzenner | 2:69bfc537508f | 427 | if(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)>0) { |
| bako | 0:46cf63cba59a | 428 | motor2Dir=0; |
| bako | 0:46cf63cba59a | 429 | } else { |
| bako | 0:46cf63cba59a | 430 | motor2Dir =1; |
| bako | 0:46cf63cba59a | 431 | } |
| bako | 0:46cf63cba59a | 432 | //set motor speed based on PI controller error |
| Frimzenner | 2:69bfc537508f | 433 | motor2 = fabs(PIDController( angleError2, motor2KP, motor2KI,motor2KD, controllerTickerTime, N, motor2ErrorInt ,motor2ErrorDif)); |
| bako | 0:46cf63cba59a | 434 | |
| bako | 0:46cf63cba59a | 435 | } |
| bako | 0:46cf63cba59a | 436 | |
| Frimzenner | 1:864a5f8bb886 | 437 | int main() |
| Frimzenner | 1:864a5f8bb886 | 438 | { |
| bako | 0:46cf63cba59a | 439 | wait(2); |
| Frimzenner | 2:69bfc537508f | 440 | xe = x_des; |
| Frimzenner | 2:69bfc537508f | 441 | ye = y_des; |
| Frimzenner | 2:69bfc537508f | 442 | pc.baud(115200); |
| Frimzenner | 2:69bfc537508f | 443 | pc.printf("\n Start up complete \n \r"); |
| Frimzenner | 2:69bfc537508f | 444 | pc.printf("initial values: \n \r"); |
| Frimzenner | 2:69bfc537508f | 445 | pc.printf("xloc = %f, yloc = %f \n \r", xe, ye); |
| Frimzenner | 2:69bfc537508f | 446 | pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n \r", q1, desiredAngle1, q2, desiredAngle2); |
| bako | 0:46cf63cba59a | 447 | myControllerTicker.attach(&motorButtonController, controllerTickerTime); |
| Frimzenner | 3:c63c0a23ea21 | 448 | bit1=0; |
| Frimzenner | 3:c63c0a23ea21 | 449 | bit2=0; |
| Frimzenner | 3:c63c0a23ea21 | 450 | bit3=0; |
| Frimzenner | 3:c63c0a23ea21 | 451 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 452 | for(int i=0; i<MOVAGLength; i++) { |
| Frimzenner | 3:c63c0a23ea21 | 453 | bicepsMOVAG[i]=0; |
| Frimzenner | 3:c63c0a23ea21 | 454 | tricepsMOVAG[i]=0; |
| Frimzenner | 3:c63c0a23ea21 | 455 | carpiFlexMOVAG[i]=0; |
| Frimzenner | 3:c63c0a23ea21 | 456 | palmarisMOVAG[i]=0; |
| Frimzenner | 3:c63c0a23ea21 | 457 | } |
| Frimzenner | 3:c63c0a23ea21 | 458 | filterTicker.attach(filter,tickerF); |
| Frimzenner | 3:c63c0a23ea21 | 459 | |
| Frimzenner | 3:c63c0a23ea21 | 460 | //start up |
| Frimzenner | 3:c63c0a23ea21 | 461 | state=bicepsCalib; |
| Frimzenner | 3:c63c0a23ea21 | 462 | stateChange=true; |
| Frimzenner | 3:c63c0a23ea21 | 463 | |
| Frimzenner | 3:c63c0a23ea21 | 464 | while(true) { |
| bako | 0:46cf63cba59a | 465 | if(!button1) { |
| Frimzenner | 3:c63c0a23ea21 | 466 | x_des -= positionIncrement; |
| Frimzenner | 2:69bfc537508f | 467 | motorAngle(); |
| Frimzenner | 3:c63c0a23ea21 | 468 | wait(0.4f); |
| bako | 0:46cf63cba59a | 469 | } |
| bako | 0:46cf63cba59a | 470 | |
| bako | 0:46cf63cba59a | 471 | if(!button2) { |
| Frimzenner | 3:c63c0a23ea21 | 472 | x_des += positionIncrement; |
| Frimzenner | 2:69bfc537508f | 473 | motorAngle(); |
| Frimzenner | 3:c63c0a23ea21 | 474 | wait(0.4f); |
| Frimzenner | 2:69bfc537508f | 475 | } |
| Frimzenner | 3:c63c0a23ea21 | 476 | if(bicepsAvg*bicepsMulti > 0.9){ |
| Frimzenner | 3:c63c0a23ea21 | 477 | x_des -= positionIncrement; |
| Frimzenner | 3:c63c0a23ea21 | 478 | motorAngle(); |
| Frimzenner | 3:c63c0a23ea21 | 479 | wait(0.5f); |
| Frimzenner | 3:c63c0a23ea21 | 480 | } |
| Frimzenner | 3:c63c0a23ea21 | 481 | if(tricepsAvg*tricepsMulti > 0.9){ |
| Frimzenner | 2:69bfc537508f | 482 | x_des += positionIncrement; |
| Frimzenner | 2:69bfc537508f | 483 | motorAngle(); |
| Frimzenner | 3:c63c0a23ea21 | 484 | wait(0.5f); |
| Frimzenner | 2:69bfc537508f | 485 | } |
| Frimzenner | 3:c63c0a23ea21 | 486 | // |
| Frimzenner | 3:c63c0a23ea21 | 487 | // if(button3) { |
| Frimzenner | 3:c63c0a23ea21 | 488 | // x_des += positionIncrement; |
| Frimzenner | 3:c63c0a23ea21 | 489 | // motorAngle(); |
| Frimzenner | 3:c63c0a23ea21 | 490 | // wait(0.3f); |
| Frimzenner | 3:c63c0a23ea21 | 491 | // } |
| Frimzenner | 3:c63c0a23ea21 | 492 | // if(button4) { |
| Frimzenner | 3:c63c0a23ea21 | 493 | // x_des -= positionIncrement; |
| Frimzenner | 3:c63c0a23ea21 | 494 | // motorAngle(); |
| Frimzenner | 3:c63c0a23ea21 | 495 | // wait(0.3f); |
| Frimzenner | 3:c63c0a23ea21 | 496 | // } |
| Frimzenner | 3:c63c0a23ea21 | 497 | if(stateChange) { |
| Frimzenner | 3:c63c0a23ea21 | 498 | switch(state) { |
| Frimzenner | 3:c63c0a23ea21 | 499 | case motorCalib : |
| Frimzenner | 3:c63c0a23ea21 | 500 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 501 | sendState(motorCalib); |
| Frimzenner | 3:c63c0a23ea21 | 502 | break; |
| Frimzenner | 3:c63c0a23ea21 | 503 | case bicepsCalib : |
| Frimzenner | 3:c63c0a23ea21 | 504 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 505 | sendState(bicepsCalib); |
| Frimzenner | 3:c63c0a23ea21 | 506 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 507 | flex.reset(); |
| Frimzenner | 3:c63c0a23ea21 | 508 | flex.start(); |
| Frimzenner | 3:c63c0a23ea21 | 509 | while(flex.read()<2) { |
| Frimzenner | 3:c63c0a23ea21 | 510 | if(bicepsAvg>bicepsMax) { |
| Frimzenner | 3:c63c0a23ea21 | 511 | bicepsMax=bicepsAvg; |
| Frimzenner | 3:c63c0a23ea21 | 512 | } |
| Frimzenner | 3:c63c0a23ea21 | 513 | } |
| Frimzenner | 3:c63c0a23ea21 | 514 | bit1 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 515 | bit2 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 516 | bit3 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 517 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 518 | state=tricepsCalib; |
| Frimzenner | 3:c63c0a23ea21 | 519 | stateChange=true; |
| Frimzenner | 3:c63c0a23ea21 | 520 | break; |
| Frimzenner | 3:c63c0a23ea21 | 521 | case tricepsCalib : |
| Frimzenner | 3:c63c0a23ea21 | 522 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 523 | sendState(tricepsCalib); |
| Frimzenner | 3:c63c0a23ea21 | 524 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 525 | flex.reset(); |
| Frimzenner | 3:c63c0a23ea21 | 526 | flex.start(); |
| Frimzenner | 3:c63c0a23ea21 | 527 | while(flex.read()<2) { |
| Frimzenner | 3:c63c0a23ea21 | 528 | if(tricepsAvg>tricepsMax) { |
| Frimzenner | 3:c63c0a23ea21 | 529 | tricepsMax=tricepsAvg; |
| Frimzenner | 3:c63c0a23ea21 | 530 | } |
| Frimzenner | 3:c63c0a23ea21 | 531 | } |
| Frimzenner | 3:c63c0a23ea21 | 532 | bit1 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 533 | bit2 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 534 | bit3 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 535 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 536 | state=carpiCalib; |
| Frimzenner | 3:c63c0a23ea21 | 537 | stateChange=true; |
| Frimzenner | 3:c63c0a23ea21 | 538 | break; |
| Frimzenner | 3:c63c0a23ea21 | 539 | case carpiCalib : |
| Frimzenner | 3:c63c0a23ea21 | 540 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 541 | sendState(carpiCalib); |
| Frimzenner | 3:c63c0a23ea21 | 542 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 543 | flex.reset(); |
| Frimzenner | 3:c63c0a23ea21 | 544 | flex.start(); |
| Frimzenner | 3:c63c0a23ea21 | 545 | while(flex.read()<2) { |
| Frimzenner | 3:c63c0a23ea21 | 546 | if(carpiFlexAvg>carpiFlexMax) { |
| Frimzenner | 3:c63c0a23ea21 | 547 | carpiFlexMax=carpiFlexAvg; |
| Frimzenner | 3:c63c0a23ea21 | 548 | } |
| Frimzenner | 3:c63c0a23ea21 | 549 | } |
| Frimzenner | 3:c63c0a23ea21 | 550 | bit1 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 551 | bit2 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 552 | bit3 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 553 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 554 | state=palmarisCalib; |
| Frimzenner | 3:c63c0a23ea21 | 555 | stateChange=true; |
| Frimzenner | 3:c63c0a23ea21 | 556 | break; |
| Frimzenner | 3:c63c0a23ea21 | 557 | case palmarisCalib : |
| Frimzenner | 3:c63c0a23ea21 | 558 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 559 | sendState(palmarisCalib); |
| Frimzenner | 3:c63c0a23ea21 | 560 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 561 | flex.reset(); |
| Frimzenner | 3:c63c0a23ea21 | 562 | flex.start(); |
| Frimzenner | 3:c63c0a23ea21 | 563 | while(flex.read()<2) { |
| Frimzenner | 3:c63c0a23ea21 | 564 | if(palmarisAvg>palmarisMax) { |
| Frimzenner | 3:c63c0a23ea21 | 565 | palmarisMax=palmarisAvg; |
| Frimzenner | 3:c63c0a23ea21 | 566 | } |
| Frimzenner | 3:c63c0a23ea21 | 567 | } |
| Frimzenner | 3:c63c0a23ea21 | 568 | bit1 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 569 | bit2 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 570 | bit3 = 0; |
| Frimzenner | 3:c63c0a23ea21 | 571 | wait(5); |
| Frimzenner | 3:c63c0a23ea21 | 572 | state=EMGControl; |
| Frimzenner | 3:c63c0a23ea21 | 573 | stateChange=true; |
| Frimzenner | 3:c63c0a23ea21 | 574 | break; |
| Frimzenner | 3:c63c0a23ea21 | 575 | case demo : |
| Frimzenner | 3:c63c0a23ea21 | 576 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 577 | sendState(demo); |
| Frimzenner | 3:c63c0a23ea21 | 578 | break; |
| Frimzenner | 3:c63c0a23ea21 | 579 | case EMGControl : |
| Frimzenner | 3:c63c0a23ea21 | 580 | stateChange=false; |
| Frimzenner | 3:c63c0a23ea21 | 581 | sendState(EMGControl); |
| Frimzenner | 3:c63c0a23ea21 | 582 | if(bicepsMax != 0){ |
| Frimzenner | 3:c63c0a23ea21 | 583 | bicepsMulti=1/bicepsMax; |
| Frimzenner | 3:c63c0a23ea21 | 584 | } |
| Frimzenner | 3:c63c0a23ea21 | 585 | if(tricepsMax != 0){ |
| Frimzenner | 3:c63c0a23ea21 | 586 | tricepsMulti=1/tricepsMax; |
| Frimzenner | 3:c63c0a23ea21 | 587 | } |
| Frimzenner | 3:c63c0a23ea21 | 588 | if(carpiFlexMax != 0){ |
| Frimzenner | 3:c63c0a23ea21 | 589 | carpiFlexMulti=1/carpiFlexMax; |
| Frimzenner | 3:c63c0a23ea21 | 590 | } |
| Frimzenner | 3:c63c0a23ea21 | 591 | if(palmarisMax != 0){ |
| Frimzenner | 3:c63c0a23ea21 | 592 | palmarisMulti=1/palmarisMax; |
| Frimzenner | 3:c63c0a23ea21 | 593 | } |
| Frimzenner | 3:c63c0a23ea21 | 594 | break; |
| Frimzenner | 3:c63c0a23ea21 | 595 | } |
| bako | 0:46cf63cba59a | 596 | } |
| bako | 0:46cf63cba59a | 597 | } |
| bako | 0:46cf63cba59a | 598 | } |