Gerhard Berman / Mbed 2 deprecated prog_BioRobotics_Group9_StampRobot_EMGThreshold

Version for EMG Threshold finding

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_BioRobotics_Group9_StampRobot by Gerhard Berman

main.cpp@45:0f91abd76b93, 2016-11-05 (annotated)

Committer:
GerhardBerman
Date:
Sat Nov 05 16:02:57 2016 +0000
Revision:
45:0f91abd76b93
Parent:
44:43f200e04903
Child:
46:4af2f01da9f3
Final version as used in demo. Script has been cleaned from any unused stuff, comments have been restyled

Who changed what in which revision?

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