Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
main.cpp@47:13b4a318a3d0, 2016-11-07 (annotated)
- Committer:
- GerhardBerman
- Date:
- Mon Nov 07 12:14:16 2016 +0000
- Revision:
- 47:13b4a318a3d0
- Parent:
- 46:4af2f01da9f3
- Child:
- 48:73a5f7f62aec
Some comments changed.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GerhardBerman | 0:43160ef59f9f | 1 | #include "mbed.h" |
GerhardBerman | 0:43160ef59f9f | 2 | #include <math.h> |
GerhardBerman | 0:43160ef59f9f | 3 | #include "MODSERIAL.h" |
GerhardBerman | 0:43160ef59f9f | 4 | #include "QEI.h" |
GerhardBerman | 0:43160ef59f9f | 5 | #include "HIDScope.h" |
GerhardBerman | 0:43160ef59f9f | 6 | #include "BiQuad.h" |
GerhardBerman | 46:4af2f01da9f3 | 7 | |
GerhardBerman | 17:91d20d362e72 | 8 | /* |
GerhardBerman | 17:91d20d362e72 | 9 | THINGS TO CONSIDER |
GerhardBerman | 44:43f200e04903 | 10 | - Motor action of motor 1 is inverted because it is mounted |
GerhardBerman | 17:91d20d362e72 | 11 | opposite to motor 2 in the tower. Check if the clockwise directions of the |
GerhardBerman | 17:91d20d362e72 | 12 | motors correspond to the positive q1, q2-directions (both counterclockwise) |
GerhardBerman | 17:91d20d362e72 | 13 | in the original IK-sketch. |
GerhardBerman | 45:0f91abd76b93 | 14 | - Robot arms should be placed manually into reference position before resetting board. |
GerhardBerman | 17:91d20d362e72 | 15 | */ |
GerhardBerman | 46:4af2f01da9f3 | 16 | |
GerhardBerman | 0:43160ef59f9f | 17 | //set pins |
GerhardBerman | 46:4af2f01da9f3 | 18 | DigitalIn encoder1A (D13); //Channel A from Encoder 1 |
GerhardBerman | 46:4af2f01da9f3 | 19 | DigitalIn encoder1B (D12); //Channel B from Encoder 1 |
GerhardBerman | 46:4af2f01da9f3 | 20 | DigitalIn encoder2A (D11); //Channel A from Encoder 2 |
GerhardBerman | 46:4af2f01da9f3 | 21 | DigitalIn encoder2B (D10); //Channel B from Encoder 2 |
GerhardBerman | 44:43f200e04903 | 22 | AnalogIn emg0( A0 ); |
GerhardBerman | 44:43f200e04903 | 23 | AnalogIn emg1( A1 ); |
GerhardBerman | 44:43f200e04903 | 24 | DigitalIn button1(D3); |
GerhardBerman | 44:43f200e04903 | 25 | DigitalIn button2(D9); |
GerhardBerman | 46:4af2f01da9f3 | 26 | |
GerhardBerman | 0:43160ef59f9f | 27 | DigitalOut motor1DirectionPin(D7); |
GerhardBerman | 0:43160ef59f9f | 28 | PwmOut motor1MagnitudePin(D6); |
GerhardBerman | 7:2f74dfd1d411 | 29 | DigitalOut motor2DirectionPin(D4); |
GerhardBerman | 7:2f74dfd1d411 | 30 | PwmOut motor2MagnitudePin(D5); |
GerhardBerman | 41:68b170829965 | 31 | DigitalOut ledGrn(LED_GREEN); |
GerhardBerman | 41:68b170829965 | 32 | DigitalOut ledRed(LED_RED); |
GerhardBerman | 41:68b170829965 | 33 | DigitalOut ledBlue(LED_BLUE); |
GerhardBerman | 46:4af2f01da9f3 | 34 | |
GerhardBerman | 7:2f74dfd1d411 | 35 | //library settings |
GerhardBerman | 0:43160ef59f9f | 36 | Serial pc(USBTX,USBRX); |
GerhardBerman | 41:68b170829965 | 37 | HIDScope scope(6); |
GerhardBerman | 46:4af2f01da9f3 | 38 | |
GerhardBerman | 44:43f200e04903 | 39 | //go-Ticker settings |
GerhardBerman | 44:43f200e04903 | 40 | Ticker MeasureTicker; |
GerhardBerman | 44:43f200e04903 | 41 | volatile bool MeasureTicker_go=false; |
GerhardBerman | 46:4af2f01da9f3 | 42 | void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flag |
GerhardBerman | 46:4af2f01da9f3 | 43 | |
GerhardBerman | 44:43f200e04903 | 44 | //constant values |
GerhardBerman | 3:8caef4872b0c | 45 | const float PI = 3.141592653589793; |
GerhardBerman | 46:4af2f01da9f3 | 46 | const int cw = 0; //values for cw and ccw are inverted! cw=0 and ccw=1 |
GerhardBerman | 3:8caef4872b0c | 47 | const int ccw = 1; |
GerhardBerman | 46:4af2f01da9f3 | 48 | double threshold_l=0.09; //left arm EMG threshold |
GerhardBerman | 46:4af2f01da9f3 | 49 | double threshold_r = 0.08; //right arm EMG threshold |
GerhardBerman | 44:43f200e04903 | 50 | float EMGgain = 1.0; |
GerhardBerman | 46:4af2f01da9f3 | 51 | |
GerhardBerman | 44:43f200e04903 | 52 | //set lengths |
GerhardBerman | 46:4af2f01da9f3 | 53 | //float L0 = 0.232; //height of motor axes above table surface |
GerhardBerman | 46:4af2f01da9f3 | 54 | float L1 = 0.45; //length of proximal arm |
GerhardBerman | 46:4af2f01da9f3 | 55 | float L2 = 0.35; //length of distal arm |
GerhardBerman | 46:4af2f01da9f3 | 56 | float TowerHeight = 0.232; //height of motor axes above table surface |
GerhardBerman | 46:4af2f01da9f3 | 57 | float StampHeight = 0.056; // height of end effector |
GerhardBerman | 46:4af2f01da9f3 | 58 | float y_stampup = 0.1; //height stamp while not stamping: 10cm above table surface |
GerhardBerman | 46:4af2f01da9f3 | 59 | float y_stampdown = -0.04; //height stamp while stamping: at table surface |
GerhardBerman | 46:4af2f01da9f3 | 60 | |
GerhardBerman | 46:4af2f01da9f3 | 61 | float MotorGain = 8.4; // rad/s for PWM, is max motor speed |
GerhardBerman | 46:4af2f01da9f3 | 62 | float MotorMaxSpeed = 0.1; //define a maximum PWM speed for the motor |
GerhardBerman | 46:4af2f01da9f3 | 63 | float t_sample = 0.002; //seconds |
GerhardBerman | 44:43f200e04903 | 64 | const float maxStampDistance = 0.7; |
GerhardBerman | 44:43f200e04903 | 65 | const float minStampDistance = 0.3; |
GerhardBerman | 46:4af2f01da9f3 | 66 | float Kp = 4.0; //proportional controller constant |
GerhardBerman | 46:4af2f01da9f3 | 67 | float Ki = 0.04; //integrative controller constant |
GerhardBerman | 46:4af2f01da9f3 | 68 | float Kd = 0.02; //derivative controller constant |
GerhardBerman | 46:4af2f01da9f3 | 69 | float N = 25; //PIDF filter constant |
GerhardBerman | 44:43f200e04903 | 70 | //(Higher N is faster derivative action but more sensitive to noise) |
GerhardBerman | 44:43f200e04903 | 71 | |
GerhardBerman | 44:43f200e04903 | 72 | //set initial conditions for inputs and positions |
GerhardBerman | 44:43f200e04903 | 73 | float biceps_l = 0, biceps_r = 0; |
GerhardBerman | 44:43f200e04903 | 74 | double envelopeL = 0, envelopeR = 0; |
GerhardBerman | 46:4af2f01da9f3 | 75 | int T=0; //EMG 'switch' variable |
GerhardBerman | 46:4af2f01da9f3 | 76 | float ReferencePosition_x = 0.35; //starting position for x reference position |
GerhardBerman | 46:4af2f01da9f3 | 77 | float ReferencePosition_y = L1 + TowerHeight - StampHeight; |
GerhardBerman | 46:4af2f01da9f3 | 78 | //starting position for y reference position |
GerhardBerman | 44:43f200e04903 | 79 | float ReferencePosition_xnew = 0.35; |
GerhardBerman | 39:cc7754330c26 | 80 | float ReferencePosition_ynew = L1 + TowerHeight - StampHeight; |
GerhardBerman | 19:cba54636bd62 | 81 | float Position_x = 0.0; |
GerhardBerman | 19:cba54636bd62 | 82 | float Position_y = 0.0; |
GerhardBerman | 46:4af2f01da9f3 | 83 | |
GerhardBerman | 44:43f200e04903 | 84 | //set initial conditions for angles, errors and motor values |
GerhardBerman | 44:43f200e04903 | 85 | float q1 = 0,q2 = PI/2; |
GerhardBerman | 44:43f200e04903 | 86 | float q1_ref = 0, q2_ref = 0; |
GerhardBerman | 44:43f200e04903 | 87 | float q1start = 0; |
GerhardBerman | 39:cc7754330c26 | 88 | float q12start = PI/2; |
GerhardBerman | 44:43f200e04903 | 89 | float q1Encoder = 0, q12Encoder = 0; |
GerhardBerman | 39:cc7754330c26 | 90 | float q12Out = 0; |
GerhardBerman | 46:4af2f01da9f3 | 91 | |
GerhardBerman | 44:43f200e04903 | 92 | float q1_error_prev = 0, q2_error_prev = 0; |
GerhardBerman | 44:43f200e04903 | 93 | float DerTotalError1 = 0, DerTotalError2 = 0; |
GerhardBerman | 44:43f200e04903 | 94 | float q1IntError = 0, q2IntError = 0; |
GerhardBerman | 44:43f200e04903 | 95 | float TotalError1_prev = 0, TotalError2_prev = 0; |
GerhardBerman | 44:43f200e04903 | 96 | float TotalError1= 0, TotalError2= 0; |
GerhardBerman | 46:4af2f01da9f3 | 97 | |
GerhardBerman | 44:43f200e04903 | 98 | float motorValue1 = 0.0, motorValue2 = 0.0; |
GerhardBerman | 44:43f200e04903 | 99 | int counts1 = 0, counts2 = 0; |
GerhardBerman | 44:43f200e04903 | 100 | int counts1Prev = 0, counts2Prev = 0; |
GerhardBerman | 46:4af2f01da9f3 | 101 | |
GerhardBerman | 44:43f200e04903 | 102 | //set reference angle boundaries |
GerhardBerman | 44:43f200e04903 | 103 | float q1_refOutNew = 0, q2_refOutNew = 0; |
GerhardBerman | 46:4af2f01da9f3 | 104 | float q1_refOutMin = 0; //Physical min angle 0.00 radians |
GerhardBerman | 46:4af2f01da9f3 | 105 | float q1_refOutMax = 1.37; //Physical max angle 1.47 radians - 0.1 rad |
GerhardBerman | 46:4af2f01da9f3 | 106 | float q2_refOutMin = 0.91; //Physical min angle 0.81 radians + 0.1 rad |
GerhardBerman | 46:4af2f01da9f3 | 107 | float q2_refOutMax = 2.07; //Physical max angle 2.17 radians - 0.1 rad |
GerhardBerman | 46:4af2f01da9f3 | 108 | |
GerhardBerman | 44:43f200e04903 | 109 | //set BiQuad filters/filter chains |
GerhardBerman | 46:4af2f01da9f3 | 110 | BiQuad pidf; //PID Filter |
GerhardBerman | 46:4af2f01da9f3 | 111 | |
GerhardBerman | 46:4af2f01da9f3 | 112 | BiQuadChain bcq1R; //Right EMG filter chain 1: notch filter+highpass |
GerhardBerman | 46:4af2f01da9f3 | 113 | BiQuadChain bcq2R; //Right EMG filter chain 2: lowpass |
GerhardBerman | 46:4af2f01da9f3 | 114 | BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01); |
GerhardBerman | 46:4af2f01da9f3 | 115 | // Notch filter wo=50; bandwidth=wo/35 |
GerhardBerman | 46:4af2f01da9f3 | 116 | BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01); |
GerhardBerman | 46:4af2f01da9f3 | 117 | // High pass Butterworth filter 2nd order, Fc=10; |
GerhardBerman | 46:4af2f01da9f3 | 118 | BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01); |
GerhardBerman | 46:4af2f01da9f3 | 119 | // Low pass Butterworth filter 2nd order, Fc = 8; |
GerhardBerman | 46:4af2f01da9f3 | 120 | |
GerhardBerman | 46:4af2f01da9f3 | 121 | BiQuadChain bcq1L; //Left EMG filter chain 1: notch filter+highpass |
GerhardBerman | 46:4af2f01da9f3 | 122 | BiQuadChain bcq2L; //Left EMG filter chain 2: lowpass |
GerhardBerman | 46:4af2f01da9f3 | 123 | BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01); |
GerhardBerman | 46:4af2f01da9f3 | 124 | // Notch filter wo=50; bandwidth=wo/35 |
GerhardBerman | 46:4af2f01da9f3 | 125 | BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01); |
GerhardBerman | 46:4af2f01da9f3 | 126 | // High pass Butterworth filter 2nd order, Fc=10; |
GerhardBerman | 46:4af2f01da9f3 | 127 | BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01); |
GerhardBerman | 46:4af2f01da9f3 | 128 | // Low pass Butterworth filter 2nd order, Fc = 8; |
GerhardBerman | 44:43f200e04903 | 129 | // In the following: R is used for right arm EMG, L is used for left arm EMG |
GerhardBerman | 46:4af2f01da9f3 | 130 | |
GerhardBerman | 3:8caef4872b0c | 131 | //define encoder counts and degrees |
GerhardBerman | 46:4af2f01da9f3 | 132 | QEI Encoder1(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 46:4af2f01da9f3 | 133 | QEI Encoder2(D10, D11, NC, 32); // turns on encoder |
GerhardBerman | 46:4af2f01da9f3 | 134 | const int counts_per_revolution = 4200; //counts per motor axis revolution |
GerhardBerman | 44:43f200e04903 | 135 | const int inverse_gear_ratio = 131; |
GerhardBerman | 44:43f200e04903 | 136 | const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis |
GerhardBerman | 46:4af2f01da9f3 | 137 | |
GerhardBerman | 46:4af2f01da9f3 | 138 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 139 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 140 | |
GerhardBerman | 41:68b170829965 | 141 | void FilteredSample(int &Tout, double &envelopeLout, double &envelopeRout) |
GerhardBerman | 41:68b170829965 | 142 | { |
GerhardBerman | 44:43f200e04903 | 143 | //This function reads EMG, filters it and generates a T-switch value which specifies the movement of the robot |
GerhardBerman | 46:4af2f01da9f3 | 144 | |
GerhardBerman | 41:68b170829965 | 145 | double inLout = emg0.read(); |
GerhardBerman | 41:68b170829965 | 146 | double inRout = emg1.read(); |
GerhardBerman | 41:68b170829965 | 147 | |
GerhardBerman | 41:68b170829965 | 148 | double outRfilter1 = bcq1R.step(inRout); |
GerhardBerman | 41:68b170829965 | 149 | double outRrect= fabs(outRfilter1); |
GerhardBerman | 41:68b170829965 | 150 | envelopeRout = bcq2R.step(outRrect); |
GerhardBerman | 41:68b170829965 | 151 | |
GerhardBerman | 41:68b170829965 | 152 | double outLfilter1 = bcq1L.step(inLout); |
GerhardBerman | 41:68b170829965 | 153 | double outLrect = fabs(outLfilter1); |
GerhardBerman | 41:68b170829965 | 154 | envelopeLout = bcq2L.step(outLrect); |
GerhardBerman | 41:68b170829965 | 155 | |
GerhardBerman | 46:4af2f01da9f3 | 156 | double biceps_l = (double) envelopeLout * EMGgain; //emg0.read(); |
GerhardBerman | 46:4af2f01da9f3 | 157 | //left biceps filtered EMG signal with a gain |
GerhardBerman | 46:4af2f01da9f3 | 158 | |
GerhardBerman | 46:4af2f01da9f3 | 159 | double biceps_r = (double) envelopeRout * EMGgain; |
GerhardBerman | 46:4af2f01da9f3 | 160 | //right biceps filtered EMG signal with a gain |
GerhardBerman | 41:68b170829965 | 161 | if (biceps_l > threshold_l && biceps_r > threshold_r){ |
GerhardBerman | 41:68b170829965 | 162 | //both arms activated: stamp moves down |
GerhardBerman | 41:68b170829965 | 163 | Tout = -2; |
GerhardBerman | 41:68b170829965 | 164 | } |
GerhardBerman | 41:68b170829965 | 165 | else if (biceps_l > threshold_l && biceps_r <= threshold_r){ |
GerhardBerman | 41:68b170829965 | 166 | //arm 1 activated, move left |
GerhardBerman | 41:68b170829965 | 167 | Tout = -1; |
GerhardBerman | 41:68b170829965 | 168 | } |
GerhardBerman | 41:68b170829965 | 169 | else if (biceps_l <= threshold_l && biceps_r > threshold_r){ |
GerhardBerman | 41:68b170829965 | 170 | //arm 1 activated, move right |
GerhardBerman | 41:68b170829965 | 171 | Tout = 1; |
GerhardBerman | 41:68b170829965 | 172 | } |
GerhardBerman | 41:68b170829965 | 173 | else{ |
GerhardBerman | 41:68b170829965 | 174 | //wait(0.2); |
GerhardBerman | 41:68b170829965 | 175 | Tout = 0; |
GerhardBerman | 44:43f200e04903 | 176 | } |
GerhardBerman | 41:68b170829965 | 177 | } |
GerhardBerman | 46:4af2f01da9f3 | 178 | |
GerhardBerman | 46:4af2f01da9f3 | 179 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 180 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 181 | |
GerhardBerman | 44:43f200e04903 | 182 | void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut) |
GerhardBerman | 44:43f200e04903 | 183 | { |
GerhardBerman | 47:13b4a318a3d0 | 184 | //This function reads current joint angles from encoder, calculates reference position from |
GerhardBerman | 47:13b4a318a3d0 | 185 | //T-switch value, converts reference position to reference joint angles and checks boundaries. |
GerhardBerman | 7:2f74dfd1d411 | 186 | |
GerhardBerman | 19:cba54636bd62 | 187 | //get joint positions q feedback from encoder |
GerhardBerman | 46:4af2f01da9f3 | 188 | float Encoder1Position = counts1/resolution; //angular encoder position (rad) |
GerhardBerman | 46:4af2f01da9f3 | 189 | float Encoder2Position = -1*counts2/resolution; //NEGATIVE due to opposite build //up in tower |
GerhardBerman | 39:cc7754330c26 | 190 | |
GerhardBerman | 39:cc7754330c26 | 191 | q1Encoder = Encoder1Position*inverse_gear_ratio; |
GerhardBerman | 39:cc7754330c26 | 192 | q12Encoder = Encoder2Position*inverse_gear_ratio; |
GerhardBerman | 46:4af2f01da9f3 | 193 | q1Out = q1start + q1Encoder; //angular motor axis position (rad) |
GerhardBerman | 46:4af2f01da9f3 | 194 | q12Out = q12start + q12Encoder; //encoder 2 gives sum of q1 and q2! |
GerhardBerman | 39:cc7754330c26 | 195 | q2Out = q12Out - q1Out; |
GerhardBerman | 46:4af2f01da9f3 | 196 | //float q1deg = q1Out*360/2/PI; //angle in degrees |
GerhardBerman | 46:4af2f01da9f3 | 197 | //float q2deg = q2Out*360/2/PI; //angle in degrees |
GerhardBerman | 7:2f74dfd1d411 | 198 | |
GerhardBerman | 44:43f200e04903 | 199 | //get end effector position with trigonometry |
GerhardBerman | 19:cba54636bd62 | 200 | Position_x = (L1*sin(q1) + L2*sin(q1+q2)); |
GerhardBerman | 28:6d8c6fe79394 | 201 | Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight; |
GerhardBerman | 29:caf3dd699617 | 202 | |
GerhardBerman | 44:43f200e04903 | 203 | //get reference position from EMG based T-switch values |
GerhardBerman | 41:68b170829965 | 204 | if (T == -2){ |
GerhardBerman | 44:43f200e04903 | 205 | //both EMGs active: stamp moves down |
GerhardBerman | 19:cba54636bd62 | 206 | ReferencePosition_xnew = ReferencePosition_x; |
GerhardBerman | 44:43f200e04903 | 207 | ReferencePosition_ynew = ReferencePosition_y - 0.015; |
GerhardBerman | 18:d2cfd07ae88a | 208 | } |
GerhardBerman | 41:68b170829965 | 209 | else if (T==-1){ |
GerhardBerman | 44:43f200e04903 | 210 | //left EMG active, move stamp left |
GerhardBerman | 44:43f200e04903 | 211 | ReferencePosition_xnew = ReferencePosition_x - 0.0009; |
GerhardBerman | 44:43f200e04903 | 212 | ReferencePosition_ynew = y_stampup; |
GerhardBerman | 8:935abf8ecc27 | 213 | } |
GerhardBerman | 41:68b170829965 | 214 | else if (T==1){ |
GerhardBerman | 44:43f200e04903 | 215 | //right EMG active, move stamp right |
GerhardBerman | 44:43f200e04903 | 216 | ReferencePosition_xnew = ReferencePosition_x + 0.0009; |
GerhardBerman | 44:43f200e04903 | 217 | ReferencePosition_ynew = y_stampup; |
GerhardBerman | 44:43f200e04903 | 218 | } |
GerhardBerman | 46:4af2f01da9f3 | 219 | else{ //T==0 |
GerhardBerman | 44:43f200e04903 | 220 | //no EMG active, no x-movement, y-position restored to non-stamping position |
GerhardBerman | 44:43f200e04903 | 221 | ReferencePosition_ynew = y_stampup; |
GerhardBerman | 8:935abf8ecc27 | 222 | } |
GerhardBerman | 19:cba54636bd62 | 223 | |
GerhardBerman | 44:43f200e04903 | 224 | //check reference position boundaries |
GerhardBerman | 39:cc7754330c26 | 225 | if (ReferencePosition_xnew > maxStampDistance){ |
GerhardBerman | 44:43f200e04903 | 226 | //target too far to be reached |
GerhardBerman | 44:43f200e04903 | 227 | ReferencePosition_x = maxStampDistance; |
GerhardBerman | 39:cc7754330c26 | 228 | ReferencePosition_y = y_stampup; |
GerhardBerman | 44:43f200e04903 | 229 | //pc.printf("Target too far! \r\n"); |
GerhardBerman | 39:cc7754330c26 | 230 | } |
GerhardBerman | 39:cc7754330c26 | 231 | else if (ReferencePosition_xnew < minStampDistance){ |
GerhardBerman | 44:43f200e04903 | 232 | //target too close |
GerhardBerman | 44:43f200e04903 | 233 | ReferencePosition_x = minStampDistance; |
GerhardBerman | 39:cc7754330c26 | 234 | ReferencePosition_y = y_stampup; |
GerhardBerman | 44:43f200e04903 | 235 | //pc.printf("Target too close! \r\n"); |
GerhardBerman | 39:cc7754330c26 | 236 | } |
GerhardBerman | 41:68b170829965 | 237 | else if (ReferencePosition_ynew < y_stampdown){ |
GerhardBerman | 44:43f200e04903 | 238 | //target under table surface |
GerhardBerman | 44:43f200e04903 | 239 | ReferencePosition_x = ReferencePosition_xnew; |
GerhardBerman | 41:68b170829965 | 240 | ReferencePosition_y = y_stampdown; |
GerhardBerman | 44:43f200e04903 | 241 | //pc.printf("Target too low! \r\n"); |
GerhardBerman | 41:68b170829965 | 242 | } |
GerhardBerman | 39:cc7754330c26 | 243 | else { |
GerhardBerman | 44:43f200e04903 | 244 | //target within reach |
GerhardBerman | 39:cc7754330c26 | 245 | ReferencePosition_x = ReferencePosition_xnew; |
GerhardBerman | 39:cc7754330c26 | 246 | ReferencePosition_y = ReferencePosition_ynew; |
GerhardBerman | 39:cc7754330c26 | 247 | } |
GerhardBerman | 39:cc7754330c26 | 248 | |
GerhardBerman | 44:43f200e04903 | 249 | //calculate reference joint angles for the new reference position |
GerhardBerman | 28:6d8c6fe79394 | 250 | float PointPositionArm2_x = ReferencePosition_x; |
GerhardBerman | 28:6d8c6fe79394 | 251 | float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight; |
GerhardBerman | 28:6d8c6fe79394 | 252 | float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2)); |
GerhardBerman | 19:cba54636bd62 | 253 | |
GerhardBerman | 28:6d8c6fe79394 | 254 | float alpha = atan(PointPositionArm2_y/PointPositionArm2_x); |
GerhardBerman | 28:6d8c6fe79394 | 255 | float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2)); |
GerhardBerman | 19:cba54636bd62 | 256 | q1_refOutNew = PI/2 - (alpha+beta); |
GerhardBerman | 28:6d8c6fe79394 | 257 | q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2); |
GerhardBerman | 19:cba54636bd62 | 258 | |
GerhardBerman | 44:43f200e04903 | 259 | //check reference angle boundaries |
GerhardBerman | 39:cc7754330c26 | 260 | if (q1_refOutNew < q1_refOutMin){ |
GerhardBerman | 44:43f200e04903 | 261 | //new q1_ref too small |
GerhardBerman | 39:cc7754330c26 | 262 | q1_refOut = q1_refOutMin; |
GerhardBerman | 44:43f200e04903 | 263 | //pc.printf("\r\n Under q1 angle boundaries\r\n"); |
GerhardBerman | 39:cc7754330c26 | 264 | } |
GerhardBerman | 39:cc7754330c26 | 265 | else if (q1_refOutNew > q1_refOutMax){ |
GerhardBerman | 44:43f200e04903 | 266 | //new q1_ref too large |
GerhardBerman | 39:cc7754330c26 | 267 | q1_refOut = q1_refOutMax; |
GerhardBerman | 44:43f200e04903 | 268 | //pc.printf("\r\n Above q1 angle boundaries\r\n"); |
GerhardBerman | 19:cba54636bd62 | 269 | } |
GerhardBerman | 19:cba54636bd62 | 270 | else { |
GerhardBerman | 44:43f200e04903 | 271 | //new q1_ref within reach |
GerhardBerman | 39:cc7754330c26 | 272 | q1_refOut = q1_refOutNew; |
GerhardBerman | 19:cba54636bd62 | 273 | } |
GerhardBerman | 39:cc7754330c26 | 274 | |
GerhardBerman | 39:cc7754330c26 | 275 | if (q2_refOutNew < q2_refOutMin){ |
GerhardBerman | 44:43f200e04903 | 276 | //new q2_ref too small |
GerhardBerman | 39:cc7754330c26 | 277 | q2_refOut = q2_refOutMin; |
GerhardBerman | 39:cc7754330c26 | 278 | pc.printf("\r\n Under q2 angle boundaries"); |
GerhardBerman | 39:cc7754330c26 | 279 | } |
GerhardBerman | 39:cc7754330c26 | 280 | else if (q2_refOutNew > q2_refOutMax){ |
GerhardBerman | 44:43f200e04903 | 281 | //new q2_ref too large |
GerhardBerman | 39:cc7754330c26 | 282 | q2_refOut = q2_refOutMax; |
GerhardBerman | 39:cc7754330c26 | 283 | pc.printf("\r\n Above q2 angle boundaries"); |
GerhardBerman | 19:cba54636bd62 | 284 | } |
GerhardBerman | 19:cba54636bd62 | 285 | else { |
GerhardBerman | 44:43f200e04903 | 286 | //new q2_ref within reach |
GerhardBerman | 39:cc7754330c26 | 287 | q2_refOut = q2_refOutNew; |
GerhardBerman | 39:cc7754330c26 | 288 | } |
GerhardBerman | 41:68b170829965 | 289 | |
GerhardBerman | 41:68b170829965 | 290 | scope.set(0, ReferencePosition_xnew); |
GerhardBerman | 41:68b170829965 | 291 | scope.set(1, ReferencePosition_ynew); |
GerhardBerman | 41:68b170829965 | 292 | scope.set(2, envelopeL); |
GerhardBerman | 41:68b170829965 | 293 | scope.set(3, envelopeR); |
GerhardBerman | 41:68b170829965 | 294 | scope.set(4, T); |
GerhardBerman | 41:68b170829965 | 295 | scope.send(); |
GerhardBerman | 44:43f200e04903 | 296 | } |
GerhardBerman | 46:4af2f01da9f3 | 297 | |
GerhardBerman | 46:4af2f01da9f3 | 298 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 299 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 300 | |
GerhardBerman | 44:43f200e04903 | 301 | void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out) |
GerhardBerman | 44:43f200e04903 | 302 | { |
GerhardBerman | 46:4af2f01da9f3 | 303 | //This function calculates the error between angles and refernce angles, and provides |
GerhardBerman | 46:4af2f01da9f3 | 304 | //motor values via a PIDF controller. |
GerhardBerman | 12:05e5964b69a4 | 305 | |
GerhardBerman | 44:43f200e04903 | 306 | //calculate error values |
GerhardBerman | 46:4af2f01da9f3 | 307 | float q1_error = q1_ref - q1; // proportional angular error in radians |
GerhardBerman | 46:4af2f01da9f3 | 308 | float q2_error = q2_ref - q2; // proportional angular error in radians |
GerhardBerman | 29:caf3dd699617 | 309 | |
GerhardBerman | 44:43f200e04903 | 310 | //PIDF total error output |
GerhardBerman | 40:9ecaab27acde | 311 | float TotalError1 = pidf.step(q1_error); |
GerhardBerman | 40:9ecaab27acde | 312 | float TotalError2 = pidf.step(q2_error); |
GerhardBerman | 40:9ecaab27acde | 313 | |
GerhardBerman | 44:43f200e04903 | 314 | //calculate motor values from the total errors |
GerhardBerman | 44:43f200e04903 | 315 | motorValue1Out = TotalError1/MotorGain; |
11i | 21:dd51d78c0732 | 316 | motorValue2Out = TotalError2/MotorGain; |
11i | 21:dd51d78c0732 | 317 | |
GerhardBerman | 44:43f200e04903 | 318 | //prepare for next ticker cycle |
GerhardBerman | 19:cba54636bd62 | 319 | q1_error_prev = q1_error; |
GerhardBerman | 19:cba54636bd62 | 320 | q2_error_prev = q2_error; |
GerhardBerman | 25:596255732b65 | 321 | TotalError1_prev = TotalError1; |
GerhardBerman | 33:b09608fa69e9 | 322 | TotalError2_prev = TotalError2; |
GerhardBerman | 3:8caef4872b0c | 323 | } |
GerhardBerman | 46:4af2f01da9f3 | 324 | |
GerhardBerman | 46:4af2f01da9f3 | 325 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 326 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 327 | |
GerhardBerman | 9:e4c34f5665a0 | 328 | void SetMotor1(float motorValue1, float motorValue2) |
GerhardBerman | 3:8caef4872b0c | 329 | { |
GerhardBerman | 44:43f200e04903 | 330 | //This function sets the PWM and direction bits for the motors. |
GerhardBerman | 44:43f200e04903 | 331 | //motorValues outside range are truncated to within range. |
GerhardBerman | 46:4af2f01da9f3 | 332 | |
GerhardBerman | 9:e4c34f5665a0 | 333 | //control motor 1 |
GerhardBerman | 44:43f200e04903 | 334 | if (motorValue1 >=0){ |
GerhardBerman | 44:43f200e04903 | 335 | //clockwise rotation |
GerhardBerman | 44:43f200e04903 | 336 | motor1DirectionPin=cw; |
GerhardBerman | 3:8caef4872b0c | 337 | } |
GerhardBerman | 44:43f200e04903 | 338 | else{ |
GerhardBerman | 44:43f200e04903 | 339 | //counterclockwise rotation |
GerhardBerman | 44:43f200e04903 | 340 | motor1DirectionPin=ccw; |
GerhardBerman | 3:8caef4872b0c | 341 | } |
GerhardBerman | 39:cc7754330c26 | 342 | if (fabs(motorValue1)>MotorMaxSpeed){ |
GerhardBerman | 46:4af2f01da9f3 | 343 | motor1MagnitudePin = MotorMaxSpeed; //motor values truncated |
GerhardBerman | 29:caf3dd699617 | 344 | } |
GerhardBerman | 29:caf3dd699617 | 345 | else{ |
GerhardBerman | 44:43f200e04903 | 346 | motor1MagnitudePin = fabs(motorValue1); |
GerhardBerman | 29:caf3dd699617 | 347 | } |
GerhardBerman | 29:caf3dd699617 | 348 | |
GerhardBerman | 9:e4c34f5665a0 | 349 | //control motor 2 |
GerhardBerman | 44:43f200e04903 | 350 | if (motorValue2 >=0){ |
GerhardBerman | 44:43f200e04903 | 351 | //counterclockwise rotation due to inverse buildup in tower |
GerhardBerman | 46:4af2f01da9f3 | 352 | motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted) |
GerhardBerman | 7:2f74dfd1d411 | 353 | } |
GerhardBerman | 44:43f200e04903 | 354 | else{ |
GerhardBerman | 44:43f200e04903 | 355 | //clockwise rotation due to inverse buildup in tower |
GerhardBerman | 46:4af2f01da9f3 | 356 | motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted) |
GerhardBerman | 7:2f74dfd1d411 | 357 | } |
GerhardBerman | 39:cc7754330c26 | 358 | if (fabs(motorValue2)>MotorMaxSpeed){ |
GerhardBerman | 39:cc7754330c26 | 359 | motor2MagnitudePin = MotorMaxSpeed; |
GerhardBerman | 29:caf3dd699617 | 360 | } |
GerhardBerman | 29:caf3dd699617 | 361 | else{ |
GerhardBerman | 29:caf3dd699617 | 362 | motor2MagnitudePin = fabs(motorValue2); |
GerhardBerman | 29:caf3dd699617 | 363 | } |
GerhardBerman | 3:8caef4872b0c | 364 | } |
GerhardBerman | 46:4af2f01da9f3 | 365 | |
GerhardBerman | 46:4af2f01da9f3 | 366 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 367 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 368 | |
GerhardBerman | 3:8caef4872b0c | 369 | void MeasureAndControl() |
GerhardBerman | 3:8caef4872b0c | 370 | { |
GerhardBerman | 44:43f200e04903 | 371 | // This function measures the EMG of both arms, calculates via inverse kinematics |
GerhardBerman | 44:43f200e04903 | 372 | // what the joint reference positions should be, and controls the motor with |
GerhardBerman | 44:43f200e04903 | 373 | // a Feedback controller. This is called from a Ticker. |
GerhardBerman | 41:68b170829965 | 374 | FilteredSample(T, envelopeL, envelopeR); |
GerhardBerman | 19:cba54636bd62 | 375 | GetReferenceKinematics1(q1, q2, q1_ref, q2_ref); |
GerhardBerman | 24:393da1cc1fa8 | 376 | FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2); |
GerhardBerman | 9:e4c34f5665a0 | 377 | SetMotor1(motorValue1, motorValue2); |
GerhardBerman | 3:8caef4872b0c | 378 | } |
GerhardBerman | 46:4af2f01da9f3 | 379 | |
GerhardBerman | 46:4af2f01da9f3 | 380 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 381 | //--------------------------------------------------------------------------------------- |
GerhardBerman | 46:4af2f01da9f3 | 382 | |
GerhardBerman | 0:43160ef59f9f | 383 | int main() |
GerhardBerman | 0:43160ef59f9f | 384 | { |
GerhardBerman | 44:43f200e04903 | 385 | //pc.baud(115200); |
GerhardBerman | 44:43f200e04903 | 386 | //pc.printf("Test putty IK"); |
GerhardBerman | 44:43f200e04903 | 387 | ledRed=1; |
GerhardBerman | 44:43f200e04903 | 388 | ledBlue=1; |
GerhardBerman | 44:43f200e04903 | 389 | ledRed=0; |
GerhardBerman | 41:68b170829965 | 390 | |
GerhardBerman | 46:4af2f01da9f3 | 391 | //set BiQuad chains |
GerhardBerman | 46:4af2f01da9f3 | 392 | bcq1R.add(&bq1R).add(&bq2R); |
GerhardBerman | 44:43f200e04903 | 393 | bcq2R.add(&bq3R); |
GerhardBerman | 44:43f200e04903 | 394 | bcq1L.add(&bq1L).add(&bq2L); |
GerhardBerman | 44:43f200e04903 | 395 | bcq2L.add(&bq3L); |
GerhardBerman | 46:4af2f01da9f3 | 396 | pidf.PIDF( Kp, Ki, Kd, N, t_sample ); //set PID filter |
GerhardBerman | 44:43f200e04903 | 397 | |
GerhardBerman | 44:43f200e04903 | 398 | //start up encoders |
GerhardBerman | 46:4af2f01da9f3 | 399 | counts1 = Encoder1.getPulses(); //gives position of encoder in counts |
GerhardBerman | 46:4af2f01da9f3 | 400 | counts2 = Encoder2.getPulses(); //gives position of encoder in counts |
GerhardBerman | 46:4af2f01da9f3 | 401 | wait(20.0); //time to sart up HIDScope and EMG |
GerhardBerman | 46:4af2f01da9f3 | 402 | MeasureTicker.attach(&MeasureTicker_act, 0.002); //initialize MeasureTicker, 500 Hz |
GerhardBerman | 44:43f200e04903 | 403 | |
GerhardBerman | 0:43160ef59f9f | 404 | while(1) |
GerhardBerman | 0:43160ef59f9f | 405 | { |
GerhardBerman | 3:8caef4872b0c | 406 | if (MeasureTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 407 | MeasureTicker_go=false; |
GerhardBerman | 41:68b170829965 | 408 | ledGrn = 1; |
GerhardBerman | 41:68b170829965 | 409 | ledBlue = 0; |
GerhardBerman | 46:4af2f01da9f3 | 410 | MeasureAndControl(); //execute MeasureAndControl |
GerhardBerman | 46:4af2f01da9f3 | 411 | counts1 = Encoder1.getPulses(); //get encoder counts again |
GerhardBerman | 46:4af2f01da9f3 | 412 | counts2 = Encoder2.getPulses(); //get encoder counts again |
GerhardBerman | 41:68b170829965 | 413 | ledBlue = 1; |
GerhardBerman | 41:68b170829965 | 414 | ledGrn = 0; |
GerhardBerman | 3:8caef4872b0c | 415 | } |
GerhardBerman | 0:43160ef59f9f | 416 | } |
GerhardBerman | 0:43160ef59f9f | 417 | } |