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@48:73a5f7f62aec, 2016-11-07 (annotated)

Committer:
GerhardBerman
Date:
Mon Nov 07 12:17:35 2016 +0000
Revision:
48:73a5f7f62aec
Parent:
47:13b4a318a3d0 
Some comments changed.

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 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 48:73a5f7f62aec 143 //This function reads EMG, filters it to acquire an EMG envelope and generates a
GerhardBerman 48:73a5f7f62aec 144 //T-switch value which specifies the movement of the robot
GerhardBerman 46:4af2f01da9f3 145
GerhardBerman 41:68b170829965 146 double inLout = emg0.read();
GerhardBerman 41:68b170829965 147 double inRout = emg1.read();
GerhardBerman 41:68b170829965 148
GerhardBerman 41:68b170829965 149 double outRfilter1 = bcq1R.step(inRout);
GerhardBerman 41:68b170829965 150 double outRrect= fabs(outRfilter1);
GerhardBerman 41:68b170829965 151 envelopeRout = bcq2R.step(outRrect);
GerhardBerman 41:68b170829965 152
GerhardBerman 41:68b170829965 153 double outLfilter1 = bcq1L.step(inLout);
GerhardBerman 41:68b170829965 154 double outLrect = fabs(outLfilter1);
GerhardBerman 41:68b170829965 155 envelopeLout = bcq2L.step(outLrect);
GerhardBerman 41:68b170829965 156
GerhardBerman 46:4af2f01da9f3 157 double biceps_l = (double) envelopeLout * EMGgain; //emg0.read();
GerhardBerman 46:4af2f01da9f3 158 //left biceps filtered EMG signal with a gain
GerhardBerman 46:4af2f01da9f3 159
GerhardBerman 46:4af2f01da9f3 160 double biceps_r = (double) envelopeRout * EMGgain;
GerhardBerman 46:4af2f01da9f3 161 //right biceps filtered EMG signal with a gain
GerhardBerman 41:68b170829965 162 if (biceps_l > threshold_l && biceps_r > threshold_r){
GerhardBerman 41:68b170829965 163 //both arms activated: stamp moves down
GerhardBerman 41:68b170829965 164 Tout = -2;
GerhardBerman 41:68b170829965 165 }
GerhardBerman 41:68b170829965 166 else if (biceps_l > threshold_l && biceps_r <= threshold_r){
GerhardBerman 41:68b170829965 167 //arm 1 activated, move left
GerhardBerman 41:68b170829965 168 Tout = -1;
GerhardBerman 41:68b170829965 169 }
GerhardBerman 41:68b170829965 170 else if (biceps_l <= threshold_l && biceps_r > threshold_r){
GerhardBerman 41:68b170829965 171 //arm 1 activated, move right
GerhardBerman 41:68b170829965 172 Tout = 1;
GerhardBerman 41:68b170829965 173 }
GerhardBerman 41:68b170829965 174 else{
GerhardBerman 41:68b170829965 175 //wait(0.2);
GerhardBerman 41:68b170829965 176 Tout = 0;
GerhardBerman 44:43f200e04903 177 }
GerhardBerman 41:68b170829965 178 }
GerhardBerman 46:4af2f01da9f3 179
GerhardBerman 46:4af2f01da9f3 180 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 181 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 182
GerhardBerman 44:43f200e04903 183 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut)
GerhardBerman 44:43f200e04903 184 {
GerhardBerman 47:13b4a318a3d0 185 //This function reads current joint angles from encoder, calculates reference position from
GerhardBerman 47:13b4a318a3d0 186 //T-switch value, converts reference position to reference joint angles and checks boundaries.
GerhardBerman 7:2f74dfd1d411 187
GerhardBerman 19:cba54636bd62 188 //get joint positions q feedback from encoder
GerhardBerman 46:4af2f01da9f3 189 float Encoder1Position = counts1/resolution; //angular encoder position (rad)
GerhardBerman 46:4af2f01da9f3 190 float Encoder2Position = -1*counts2/resolution; //NEGATIVE due to opposite build //up in tower
GerhardBerman 39:cc7754330c26 191
GerhardBerman 39:cc7754330c26 192 q1Encoder = Encoder1Position*inverse_gear_ratio;
GerhardBerman 39:cc7754330c26 193 q12Encoder = Encoder2Position*inverse_gear_ratio;
GerhardBerman 46:4af2f01da9f3 194 q1Out = q1start + q1Encoder; //angular motor axis position (rad)
GerhardBerman 46:4af2f01da9f3 195 q12Out = q12start + q12Encoder; //encoder 2 gives sum of q1 and q2!
GerhardBerman 39:cc7754330c26 196 q2Out = q12Out - q1Out;
GerhardBerman 46:4af2f01da9f3 197 //float q1deg = q1Out*360/2/PI; //angle in degrees
GerhardBerman 46:4af2f01da9f3 198 //float q2deg = q2Out*360/2/PI; //angle in degrees
GerhardBerman 7:2f74dfd1d411 199
GerhardBerman 44:43f200e04903 200 //get end effector position with trigonometry
GerhardBerman 19:cba54636bd62 201 Position_x = (L1*sin(q1) + L2*sin(q1+q2));
GerhardBerman 28:6d8c6fe79394 202 Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;
GerhardBerman 29:caf3dd699617 203
GerhardBerman 44:43f200e04903 204 //get reference position from EMG based T-switch values
GerhardBerman 41:68b170829965 205 if (T == -2){
GerhardBerman 44:43f200e04903 206 //both EMGs active: stamp moves down
GerhardBerman 19:cba54636bd62 207 ReferencePosition_xnew = ReferencePosition_x;
GerhardBerman 44:43f200e04903 208 ReferencePosition_ynew = ReferencePosition_y - 0.015;
GerhardBerman 18:d2cfd07ae88a 209 }
GerhardBerman 41:68b170829965 210 else if (T==-1){
GerhardBerman 44:43f200e04903 211 //left EMG active, move stamp left
GerhardBerman 44:43f200e04903 212 ReferencePosition_xnew = ReferencePosition_x - 0.0009;
GerhardBerman 44:43f200e04903 213 ReferencePosition_ynew = y_stampup;
GerhardBerman 8:935abf8ecc27 214 }
GerhardBerman 41:68b170829965 215 else if (T==1){
GerhardBerman 44:43f200e04903 216 //right EMG active, move stamp right
GerhardBerman 44:43f200e04903 217 ReferencePosition_xnew = ReferencePosition_x + 0.0009;
GerhardBerman 44:43f200e04903 218 ReferencePosition_ynew = y_stampup;
GerhardBerman 44:43f200e04903 219 }
GerhardBerman 46:4af2f01da9f3 220 else{ //T==0
GerhardBerman 44:43f200e04903 221 //no EMG active, no x-movement, y-position restored to non-stamping position
GerhardBerman 44:43f200e04903 222 ReferencePosition_ynew = y_stampup;
GerhardBerman 8:935abf8ecc27 223 }
GerhardBerman 19:cba54636bd62 224
GerhardBerman 44:43f200e04903 225 //check reference position boundaries
GerhardBerman 39:cc7754330c26 226 if (ReferencePosition_xnew > maxStampDistance){
GerhardBerman 44:43f200e04903 227 //target too far to be reached
GerhardBerman 44:43f200e04903 228 ReferencePosition_x = maxStampDistance;
GerhardBerman 39:cc7754330c26 229 ReferencePosition_y = y_stampup;
GerhardBerman 44:43f200e04903 230 //pc.printf("Target too far! \r\n");
GerhardBerman 39:cc7754330c26 231 }
GerhardBerman 39:cc7754330c26 232 else if (ReferencePosition_xnew < minStampDistance){
GerhardBerman 44:43f200e04903 233 //target too close
GerhardBerman 44:43f200e04903 234 ReferencePosition_x = minStampDistance;
GerhardBerman 39:cc7754330c26 235 ReferencePosition_y = y_stampup;
GerhardBerman 44:43f200e04903 236 //pc.printf("Target too close! \r\n");
GerhardBerman 39:cc7754330c26 237 }
GerhardBerman 41:68b170829965 238 else if (ReferencePosition_ynew < y_stampdown){
GerhardBerman 44:43f200e04903 239 //target under table surface
GerhardBerman 44:43f200e04903 240 ReferencePosition_x = ReferencePosition_xnew;
GerhardBerman 41:68b170829965 241 ReferencePosition_y = y_stampdown;
GerhardBerman 44:43f200e04903 242 //pc.printf("Target too low! \r\n");
GerhardBerman 41:68b170829965 243 }
GerhardBerman 39:cc7754330c26 244 else {
GerhardBerman 44:43f200e04903 245 //target within reach
GerhardBerman 39:cc7754330c26 246 ReferencePosition_x = ReferencePosition_xnew;
GerhardBerman 39:cc7754330c26 247 ReferencePosition_y = ReferencePosition_ynew;
GerhardBerman 39:cc7754330c26 248 }
GerhardBerman 39:cc7754330c26 249
GerhardBerman 44:43f200e04903 250 //calculate reference joint angles for the new reference position
GerhardBerman 28:6d8c6fe79394 251 float PointPositionArm2_x = ReferencePosition_x;
GerhardBerman 28:6d8c6fe79394 252 float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
GerhardBerman 28:6d8c6fe79394 253 float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2));
GerhardBerman 19:cba54636bd62 254
GerhardBerman 28:6d8c6fe79394 255 float alpha = atan(PointPositionArm2_y/PointPositionArm2_x);
GerhardBerman 28:6d8c6fe79394 256 float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2));
GerhardBerman 19:cba54636bd62 257 q1_refOutNew = PI/2 - (alpha+beta);
GerhardBerman 28:6d8c6fe79394 258 q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2);
GerhardBerman 19:cba54636bd62 259
GerhardBerman 44:43f200e04903 260 //check reference angle boundaries
GerhardBerman 39:cc7754330c26 261 if (q1_refOutNew < q1_refOutMin){
GerhardBerman 44:43f200e04903 262 //new q1_ref too small
GerhardBerman 39:cc7754330c26 263 q1_refOut = q1_refOutMin;
GerhardBerman 44:43f200e04903 264 //pc.printf("\r\n Under q1 angle boundaries\r\n");
GerhardBerman 39:cc7754330c26 265 }
GerhardBerman 39:cc7754330c26 266 else if (q1_refOutNew > q1_refOutMax){
GerhardBerman 44:43f200e04903 267 //new q1_ref too large
GerhardBerman 39:cc7754330c26 268 q1_refOut = q1_refOutMax;
GerhardBerman 44:43f200e04903 269 //pc.printf("\r\n Above q1 angle boundaries\r\n");
GerhardBerman 19:cba54636bd62 270 }
GerhardBerman 19:cba54636bd62 271 else {
GerhardBerman 44:43f200e04903 272 //new q1_ref within reach
GerhardBerman 39:cc7754330c26 273 q1_refOut = q1_refOutNew;
GerhardBerman 19:cba54636bd62 274 }
GerhardBerman 39:cc7754330c26 275
GerhardBerman 39:cc7754330c26 276 if (q2_refOutNew < q2_refOutMin){
GerhardBerman 44:43f200e04903 277 //new q2_ref too small
GerhardBerman 39:cc7754330c26 278 q2_refOut = q2_refOutMin;
GerhardBerman 39:cc7754330c26 279 pc.printf("\r\n Under q2 angle boundaries");
GerhardBerman 39:cc7754330c26 280 }
GerhardBerman 39:cc7754330c26 281 else if (q2_refOutNew > q2_refOutMax){
GerhardBerman 44:43f200e04903 282 //new q2_ref too large
GerhardBerman 39:cc7754330c26 283 q2_refOut = q2_refOutMax;
GerhardBerman 39:cc7754330c26 284 pc.printf("\r\n Above q2 angle boundaries");
GerhardBerman 19:cba54636bd62 285 }
GerhardBerman 19:cba54636bd62 286 else {
GerhardBerman 44:43f200e04903 287 //new q2_ref within reach
GerhardBerman 39:cc7754330c26 288 q2_refOut = q2_refOutNew;
GerhardBerman 39:cc7754330c26 289 }
GerhardBerman 41:68b170829965 290
GerhardBerman 41:68b170829965 291 scope.set(0, ReferencePosition_xnew);
GerhardBerman 41:68b170829965 292 scope.set(1, ReferencePosition_ynew);
GerhardBerman 41:68b170829965 293 scope.set(2, envelopeL);
GerhardBerman 41:68b170829965 294 scope.set(3, envelopeR);
GerhardBerman 41:68b170829965 295 scope.set(4, T);
GerhardBerman 41:68b170829965 296 scope.send();
GerhardBerman 44:43f200e04903 297 }
GerhardBerman 46:4af2f01da9f3 298
GerhardBerman 46:4af2f01da9f3 299 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 300 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 301
GerhardBerman 44:43f200e04903 302 void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out)
GerhardBerman 44:43f200e04903 303 {
GerhardBerman 46:4af2f01da9f3 304 //This function calculates the error between angles and refernce angles, and provides
GerhardBerman 46:4af2f01da9f3 305 //motor values via a PIDF controller.
GerhardBerman 12:05e5964b69a4 306
GerhardBerman 44:43f200e04903 307 //calculate error values
GerhardBerman 46:4af2f01da9f3 308 float q1_error = q1_ref - q1; // proportional angular error in radians
GerhardBerman 46:4af2f01da9f3 309 float q2_error = q2_ref - q2; // proportional angular error in radians
GerhardBerman 29:caf3dd699617 310
GerhardBerman 44:43f200e04903 311 //PIDF total error output
GerhardBerman 40:9ecaab27acde 312 float TotalError1 = pidf.step(q1_error);
GerhardBerman 40:9ecaab27acde 313 float TotalError2 = pidf.step(q2_error);
GerhardBerman 40:9ecaab27acde 314
GerhardBerman 44:43f200e04903 315 //calculate motor values from the total errors
GerhardBerman 44:43f200e04903 316 motorValue1Out = TotalError1/MotorGain;
11i 21:dd51d78c0732 317 motorValue2Out = TotalError2/MotorGain;
11i 21:dd51d78c0732 318
GerhardBerman 44:43f200e04903 319 //prepare for next ticker cycle
GerhardBerman 19:cba54636bd62 320 q1_error_prev = q1_error;
GerhardBerman 19:cba54636bd62 321 q2_error_prev = q2_error;
GerhardBerman 25:596255732b65 322 TotalError1_prev = TotalError1;
GerhardBerman 33:b09608fa69e9 323 TotalError2_prev = TotalError2;
GerhardBerman 3:8caef4872b0c 324 }
GerhardBerman 46:4af2f01da9f3 325
GerhardBerman 46:4af2f01da9f3 326 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 327 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 328
GerhardBerman 9:e4c34f5665a0 329 void SetMotor1(float motorValue1, float motorValue2)
GerhardBerman 3:8caef4872b0c 330 {
GerhardBerman 44:43f200e04903 331 //This function sets the PWM and direction bits for the motors.
GerhardBerman 44:43f200e04903 332 //motorValues outside range are truncated to within range.
GerhardBerman 46:4af2f01da9f3 333
GerhardBerman 9:e4c34f5665a0 334 //control motor 1
GerhardBerman 44:43f200e04903 335 if (motorValue1 >=0){
GerhardBerman 44:43f200e04903 336 //clockwise rotation
GerhardBerman 44:43f200e04903 337 motor1DirectionPin=cw;
GerhardBerman 3:8caef4872b0c 338 }
GerhardBerman 44:43f200e04903 339 else{
GerhardBerman 44:43f200e04903 340 //counterclockwise rotation
GerhardBerman 44:43f200e04903 341 motor1DirectionPin=ccw;
GerhardBerman 3:8caef4872b0c 342 }
GerhardBerman 39:cc7754330c26 343 if (fabs(motorValue1)>MotorMaxSpeed){
GerhardBerman 46:4af2f01da9f3 344 motor1MagnitudePin = MotorMaxSpeed; //motor values truncated
GerhardBerman 29:caf3dd699617 345 }
GerhardBerman 29:caf3dd699617 346 else{
GerhardBerman 44:43f200e04903 347 motor1MagnitudePin = fabs(motorValue1);
GerhardBerman 29:caf3dd699617 348 }
GerhardBerman 29:caf3dd699617 349
GerhardBerman 9:e4c34f5665a0 350 //control motor 2
GerhardBerman 44:43f200e04903 351 if (motorValue2 >=0){
GerhardBerman 44:43f200e04903 352 //counterclockwise rotation due to inverse buildup in tower
GerhardBerman 46:4af2f01da9f3 353 motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 7:2f74dfd1d411 354 }
GerhardBerman 44:43f200e04903 355 else{
GerhardBerman 44:43f200e04903 356 //clockwise rotation due to inverse buildup in tower
GerhardBerman 46:4af2f01da9f3 357 motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 7:2f74dfd1d411 358 }
GerhardBerman 39:cc7754330c26 359 if (fabs(motorValue2)>MotorMaxSpeed){
GerhardBerman 39:cc7754330c26 360 motor2MagnitudePin = MotorMaxSpeed;
GerhardBerman 29:caf3dd699617 361 }
GerhardBerman 29:caf3dd699617 362 else{
GerhardBerman 29:caf3dd699617 363 motor2MagnitudePin = fabs(motorValue2);
GerhardBerman 29:caf3dd699617 364 }
GerhardBerman 3:8caef4872b0c 365 }
GerhardBerman 46:4af2f01da9f3 366
GerhardBerman 46:4af2f01da9f3 367 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 368 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 369
GerhardBerman 3:8caef4872b0c 370 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 371 {
GerhardBerman 44:43f200e04903 372 // This function measures the EMG of both arms, calculates via inverse kinematics
GerhardBerman 44:43f200e04903 373 // what the joint reference positions should be, and controls the motor with
GerhardBerman 44:43f200e04903 374 // a Feedback controller. This is called from a Ticker.
GerhardBerman 41:68b170829965 375 FilteredSample(T, envelopeL, envelopeR);
GerhardBerman 19:cba54636bd62 376 GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
GerhardBerman 24:393da1cc1fa8 377 FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
GerhardBerman 9:e4c34f5665a0 378 SetMotor1(motorValue1, motorValue2);
GerhardBerman 3:8caef4872b0c 379 }
GerhardBerman 46:4af2f01da9f3 380
GerhardBerman 46:4af2f01da9f3 381 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 382 //---------------------------------------------------------------------------------------
GerhardBerman 46:4af2f01da9f3 383
GerhardBerman 0:43160ef59f9f 384 int main()
GerhardBerman 0:43160ef59f9f 385 {
GerhardBerman 44:43f200e04903 386 //pc.baud(115200);
GerhardBerman 44:43f200e04903 387 //pc.printf("Test putty IK");
GerhardBerman 44:43f200e04903 388 ledRed=1;
GerhardBerman 44:43f200e04903 389 ledBlue=1;
GerhardBerman 44:43f200e04903 390 ledRed=0;
GerhardBerman 41:68b170829965 391
GerhardBerman 46:4af2f01da9f3 392 //set BiQuad chains
GerhardBerman 46:4af2f01da9f3 393 bcq1R.add(&bq1R).add(&bq2R);
GerhardBerman 44:43f200e04903 394 bcq2R.add(&bq3R);
GerhardBerman 44:43f200e04903 395 bcq1L.add(&bq1L).add(&bq2L);
GerhardBerman 44:43f200e04903 396 bcq2L.add(&bq3L);
GerhardBerman 46:4af2f01da9f3 397 pidf.PIDF( Kp, Ki, Kd, N, t_sample ); //set PID filter
GerhardBerman 44:43f200e04903 398
GerhardBerman 44:43f200e04903 399 //start up encoders
GerhardBerman 46:4af2f01da9f3 400 counts1 = Encoder1.getPulses(); //gives position of encoder in counts
GerhardBerman 46:4af2f01da9f3 401 counts2 = Encoder2.getPulses(); //gives position of encoder in counts
GerhardBerman 46:4af2f01da9f3 402 wait(20.0); //time to sart up HIDScope and EMG
GerhardBerman 46:4af2f01da9f3 403 MeasureTicker.attach(&MeasureTicker_act, 0.002); //initialize MeasureTicker, 500 Hz
GerhardBerman 44:43f200e04903 404
GerhardBerman 0:43160ef59f9f 405 while(1)
GerhardBerman 0:43160ef59f9f 406 {
GerhardBerman 3:8caef4872b0c 407 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 408 MeasureTicker_go=false;
GerhardBerman 41:68b170829965 409 ledGrn = 1;
GerhardBerman 41:68b170829965 410 ledBlue = 0;
GerhardBerman 46:4af2f01da9f3 411 MeasureAndControl(); //execute MeasureAndControl
GerhardBerman 46:4af2f01da9f3 412 counts1 = Encoder1.getPulses(); //get encoder counts again
GerhardBerman 46:4af2f01da9f3 413 counts2 = Encoder2.getPulses(); //get encoder counts again
GerhardBerman 41:68b170829965 414 ledBlue = 1;
GerhardBerman 41:68b170829965 415 ledGrn = 0;
GerhardBerman 3:8caef4872b0c 416 }
GerhardBerman 0:43160ef59f9f 417 }
GerhardBerman 0:43160ef59f9f 418 }