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