Gerhard Berman / Mbed 2 deprecated prog_forwardkin_feedback_copy3

New reference frame: y=0 is now at table height.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_forwardkin_feedback_copy by Elfi Hofmeijer

main.cpp@43:2b2e0bff0b39, 2016-11-05 (annotated)

Committer:
GerhardBerman
Date:
Sat Nov 05 13:54:09 2016 +0000
Revision:
43:2b2e0bff0b39
Parent:
41:68b170829965
Child:
44:43f200e04903
FINAL VERSION used for demonstration

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 17:91d20d362e72 10 - Line 234, 239: 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 17:91d20d362e72 14 - Line 244,257: motor values have been scaled down for safety at first test, restore
GerhardBerman 17:91d20d362e72 15 after testing to get proper action.
GerhardBerman 19:cba54636bd62 16 - Set angle and length boundaries!!
GerhardBerman 17:91d20d362e72 17 - Set robot constants (lengths etc.)
GerhardBerman 18:d2cfd07ae88a 18 - Set EMGgain and thresholds
GerhardBerman 23:3a4d5e19c20d 19 - Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation!
GerhardBerman 22:c679f753a8bd 20 - Add (lower) boundaries to TotalErrors
GerhardBerman 22:c679f753a8bd 21 - MotorGain could change due to arm weight!!
GerhardBerman 22:c679f753a8bd 22 - Arms should be placed manually into reference position.
GerhardBerman 17:91d20d362e72 23 */
GerhardBerman 17:91d20d362e72 24
GerhardBerman 0:43160ef59f9f 25 //set pins
GerhardBerman 0:43160ef59f9f 26 DigitalIn encoder1A (D13); //Channel A van Encoder 1
GerhardBerman 0:43160ef59f9f 27 DigitalIn encoder1B (D12); //Channel B van Encoder 1
GerhardBerman 14:725a608b6709 28 DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
GerhardBerman 14:725a608b6709 29 DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
GerhardBerman 14:725a608b6709 30 //DigitalOut led1 (D11);
GerhardBerman 14:725a608b6709 31 //DigitalOut led2 (D10);
GerhardBerman 19:cba54636bd62 32 //AnalogIn potMeter1(A2);
GerhardBerman 19:cba54636bd62 33 //AnalogIn potMeter2(A1);
GerhardBerman 0:43160ef59f9f 34 DigitalOut motor1DirectionPin(D7);
GerhardBerman 0:43160ef59f9f 35 PwmOut motor1MagnitudePin(D6);
GerhardBerman 7:2f74dfd1d411 36 DigitalOut motor2DirectionPin(D4);
GerhardBerman 7:2f74dfd1d411 37 PwmOut motor2MagnitudePin(D5);
GerhardBerman 9:e4c34f5665a0 38 DigitalIn button1(D3);
GerhardBerman 7:2f74dfd1d411 39 DigitalIn button2(D9);
GerhardBerman 41:68b170829965 40 AnalogIn emg0( A0 );
GerhardBerman 41:68b170829965 41 AnalogIn emg1( A1 );
GerhardBerman 0:43160ef59f9f 42
GerhardBerman 41:68b170829965 43 DigitalOut ledGrn(LED_GREEN);
GerhardBerman 41:68b170829965 44 DigitalOut ledRed(LED_RED);
GerhardBerman 41:68b170829965 45 DigitalOut ledBlue(LED_BLUE);
GerhardBerman 33:b09608fa69e9 46
GerhardBerman 7:2f74dfd1d411 47 //library settings
GerhardBerman 0:43160ef59f9f 48 Serial pc(USBTX,USBRX);
GerhardBerman 3:8caef4872b0c 49 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
GerhardBerman 41:68b170829965 50 HIDScope scope(6);
GerhardBerman 0:43160ef59f9f 51
GerhardBerman 26:bb7e14f59ddd 52 //initial values
GerhardBerman 7:2f74dfd1d411 53 float dx;
GerhardBerman 7:2f74dfd1d411 54 float dy;
GerhardBerman 23:3a4d5e19c20d 55 double DerivativeCounts;
GerhardBerman 19:cba54636bd62 56 //float referenceVelocity = 0;
GerhardBerman 19:cba54636bd62 57 //float bqcDerivativeCounts = 0;
GerhardBerman 3:8caef4872b0c 58 const float PI = 3.141592653589793;
GerhardBerman 3:8caef4872b0c 59 const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1
GerhardBerman 3:8caef4872b0c 60 const int ccw = 1;
GerhardBerman 0:43160ef59f9f 61
GerhardBerman 26:bb7e14f59ddd 62 //set lengths (VALUES HAVE TO BE CHANGED)
GerhardBerman 36:72f0913c5460 63 //float x0 = 1.0;
GerhardBerman 36:72f0913c5460 64 float L0 = 0.232;
GerhardBerman 36:72f0913c5460 65 float L1 = 0.45;
GerhardBerman 36:72f0913c5460 66 float L2 = 0.35;
GerhardBerman 36:72f0913c5460 67 float TowerHeight = 0.232; //height of motor axes above table surface!
GerhardBerman 39:cc7754330c26 68 float StampHeight = 0.056; // height of end effector
GerhardBerman 34:96bcbddc7d2d 69 float y_stampup = 0.1; //height stamp while not stamping: 10cm above table surface
GerhardBerman 41:68b170829965 70 float y_stampdown = -0.04; //height stamp while stamping: at table surface
GerhardBerman 34:96bcbddc7d2d 71
GerhardBerman 26:bb7e14f59ddd 72
GerhardBerman 19:cba54636bd62 73 //set initial conditions
GerhardBerman 19:cba54636bd62 74 float biceps_l = 0;
GerhardBerman 19:cba54636bd62 75 float biceps_r = 0;
GerhardBerman 43:2b2e0bff0b39 76 float ReferencePosition_x = 0.35; //L2;
GerhardBerman 28:6d8c6fe79394 77 float ReferencePosition_y = L1 + TowerHeight - StampHeight;
GerhardBerman 43:2b2e0bff0b39 78 float ReferencePosition_xnew = 0.35; //L2;
GerhardBerman 39:cc7754330c26 79 float ReferencePosition_ynew = L1 + TowerHeight - StampHeight;
GerhardBerman 19:cba54636bd62 80 float Position_x = 0.0;
GerhardBerman 19:cba54636bd62 81 float Position_y = 0.0;
GerhardBerman 19:cba54636bd62 82 float q1 = 0;
GerhardBerman 39:cc7754330c26 83 float q2 = PI/2;
GerhardBerman 19:cba54636bd62 84 float q1_ref = 0;
GerhardBerman 19:cba54636bd62 85 float q2_ref = 0;
GerhardBerman 19:cba54636bd62 86 float q1start = 0;
GerhardBerman 39:cc7754330c26 87 float q12start = PI/2;
GerhardBerman 39:cc7754330c26 88 float q1Encoder = 0;
GerhardBerman 39:cc7754330c26 89 float q12Encoder = 0;
GerhardBerman 39:cc7754330c26 90 float q12Out = 0;
GerhardBerman 19:cba54636bd62 91
GerhardBerman 19:cba54636bd62 92 float q1_error_prev = 0;
GerhardBerman 19:cba54636bd62 93 float q2_error_prev = 0;
GerhardBerman 25:596255732b65 94 float DerTotalError1 = 0;
GerhardBerman 25:596255732b65 95 float DerTotalError2 = 0;
GerhardBerman 19:cba54636bd62 96 float q1IntError = 0;
GerhardBerman 19:cba54636bd62 97 float q2IntError = 0;
GerhardBerman 29:caf3dd699617 98 float TotalError1_prev = 0;
GerhardBerman 29:caf3dd699617 99 float TotalError2_prev = 0;
GerhardBerman 19:cba54636bd62 100
GerhardBerman 19:cba54636bd62 101 float motorValue1 = 0.0;
GerhardBerman 19:cba54636bd62 102 float motorValue2 = 0.0;
GerhardBerman 19:cba54636bd62 103 int counts1 = 0;
GerhardBerman 19:cba54636bd62 104 int counts2 = 0;
GerhardBerman 19:cba54636bd62 105 int counts1Prev = 0;
GerhardBerman 19:cba54636bd62 106 int counts2Prev = 0;
GerhardBerman 41:68b170829965 107 double envelopeL = 0;
GerhardBerman 41:68b170829965 108 double envelopeR = 0;
GerhardBerman 26:bb7e14f59ddd 109
GerhardBerman 26:bb7e14f59ddd 110 //set constant or variable values (VALUES HAVE TO BE EDITED)
GerhardBerman 41:68b170829965 111 int T=0; //EMG 'switch' variable
GerhardBerman 43:2b2e0bff0b39 112 double threshold_l=0.09; //left arm EMG threshold
GerhardBerman 41:68b170829965 113 double threshold_r = 0.08; //right arm EMG threshold
GerhardBerman 26:bb7e14f59ddd 114 float EMGgain = 1.0;
GerhardBerman 41:68b170829965 115
GerhardBerman 26:bb7e14f59ddd 116 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
GerhardBerman 26:bb7e14f59ddd 117 float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
GerhardBerman 39:cc7754330c26 118 float Motor1ExtraGain = 1.0;
GerhardBerman 41:68b170829965 119 float MotorMaxSpeed = 0.1; //define a maximum PWM speed for the motor
GerhardBerman 39:cc7754330c26 120 float t_sample = 0.01; //seconds
GerhardBerman 41:68b170829965 121 const float maxStampDistance = 0.7; //0.66;
GerhardBerman 43:2b2e0bff0b39 122 const float minStampDistance = 0.3;
GerhardBerman 41:68b170829965 123 float Kp = 4.0;//potMeter2.read();
GerhardBerman 43:2b2e0bff0b39 124 float Ki = 0.04; //0.01*Kp; //potMeter2.read();
GerhardBerman 41:68b170829965 125 float Kd = 0.02; //0.1;(unstable!) //0.05; //0.02; //0.04*Kp; //potMeter2.read();
GerhardBerman 41:68b170829965 126 float N = 25; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise.
GerhardBerman 26:bb7e14f59ddd 127
GerhardBerman 26:bb7e14f59ddd 128 float q1_refOutNew = 0;
GerhardBerman 39:cc7754330c26 129 float q1_refOutMin = 0; //Physical min angle 0.00 radians + 0.1 rad
GerhardBerman 39:cc7754330c26 130 float q1_refOutMax = 1.37; //Physical max angle 1.47 radians - 0.1 rad
GerhardBerman 26:bb7e14f59ddd 131 float q2_refOutNew = 0;
GerhardBerman 39:cc7754330c26 132 float q2_refOutMin = 0.91; //Physical min angle 0.81 radians + 0.1 rad
GerhardBerman 39:cc7754330c26 133 float q2_refOutMax = 2.07; //Physical max angle 2.17 radians - 0.1 rad
GerhardBerman 26:bb7e14f59ddd 134 float TotalError1= 0;
GerhardBerman 26:bb7e14f59ddd 135 float TotalError2= 0;
GerhardBerman 26:bb7e14f59ddd 136 float TotalErrorMin= 0;
GerhardBerman 19:cba54636bd62 137
GerhardBerman 0:43160ef59f9f 138 //set BiQuad
GerhardBerman 0:43160ef59f9f 139 BiQuadChain bqc;
GerhardBerman 0:43160ef59f9f 140 BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
GerhardBerman 0:43160ef59f9f 141 BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
GerhardBerman 0:43160ef59f9f 142
GerhardBerman 40:9ecaab27acde 143 BiQuad pidf;
GerhardBerman 40:9ecaab27acde 144
GerhardBerman 41:68b170829965 145 BiQuadChain bcq1R;
GerhardBerman 41:68b170829965 146 BiQuadChain bcq2R;
GerhardBerman 41:68b170829965 147 // Notch filter wo=50; bw=wo/35
GerhardBerman 41:68b170829965 148 BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
GerhardBerman 41:68b170829965 149 // High pass Butterworth filter 2nd order, Fc=10;
GerhardBerman 41:68b170829965 150 BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
GerhardBerman 41:68b170829965 151 // Low pass Butterworth filter 2nd order, Fc = 8;
GerhardBerman 41:68b170829965 152 BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);
GerhardBerman 41:68b170829965 153
GerhardBerman 41:68b170829965 154 BiQuadChain bcq1L;
GerhardBerman 41:68b170829965 155 BiQuadChain bcq2L;
GerhardBerman 41:68b170829965 156 // Notch filter wo=50; bw=wo/35
GerhardBerman 41:68b170829965 157 BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
GerhardBerman 41:68b170829965 158 // High pass Butterworth filter 2nd order, Fc=10;
GerhardBerman 41:68b170829965 159 BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
GerhardBerman 41:68b170829965 160 // Low pass Butterworth filter 2nd order, Fc = 8;
GerhardBerman 41:68b170829965 161 BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);
GerhardBerman 41:68b170829965 162 // In the following: R is used for right arm, L is used for left arm!
GerhardBerman 41:68b170829965 163
GerhardBerman 0:43160ef59f9f 164 //set go-Ticker settings
GerhardBerman 3:8caef4872b0c 165 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
GerhardBerman 3:8caef4872b0c 166 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
GerhardBerman 3:8caef4872b0c 167 void BiQuadTicker_act(){BiQuadTicker_go=true;};
GerhardBerman 3:8caef4872b0c 168 void FeedbackTicker_act(){FeedbackTicker_go=true;};
GerhardBerman 3:8caef4872b0c 169 void TimeTracker_act(){TimeTracker_go=true;};
GerhardBerman 3:8caef4872b0c 170 //void sampleT_act(){sampleT_go=true;};
GerhardBerman 3:8caef4872b0c 171
GerhardBerman 3:8caef4872b0c 172 //define encoder counts and degrees
GerhardBerman 7:2f74dfd1d411 173 QEI Encoder1(D12, D13, NC, 32); // turns on encoder
GerhardBerman 15:9061cf7db23e 174 QEI Encoder2(D10, D11, NC, 32); // turns on encoder
GerhardBerman 14:725a608b6709 175
GerhardBerman 4:19e376d31380 176 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 177 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 178 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 179
GerhardBerman 41:68b170829965 180 void FilteredSample(int &Tout, double &envelopeLout, double &envelopeRout)
GerhardBerman 41:68b170829965 181 {
GerhardBerman 41:68b170829965 182 double inLout = emg0.read();
GerhardBerman 41:68b170829965 183 double inRout = emg1.read();
GerhardBerman 41:68b170829965 184
GerhardBerman 41:68b170829965 185 double outRfilter1 = bcq1R.step(inRout);
GerhardBerman 41:68b170829965 186 double outRrect= fabs(outRfilter1);
GerhardBerman 41:68b170829965 187 envelopeRout = bcq2R.step(outRrect);
GerhardBerman 41:68b170829965 188
GerhardBerman 41:68b170829965 189 double outLfilter1 = bcq1L.step(inLout);
GerhardBerman 41:68b170829965 190 double outLrect = fabs(outLfilter1);
GerhardBerman 41:68b170829965 191 envelopeLout = bcq2L.step(outLrect);
GerhardBerman 41:68b170829965 192
GerhardBerman 41:68b170829965 193 double biceps_l = (double) envelopeLout * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
GerhardBerman 41:68b170829965 194 double biceps_r = (double) envelopeRout * EMGgain; //emg1.read();
GerhardBerman 41:68b170829965 195 if (biceps_l > threshold_l && biceps_r > threshold_r){
GerhardBerman 41:68b170829965 196 //both arms activated: stamp moves down
GerhardBerman 41:68b170829965 197 Tout = -2;
GerhardBerman 41:68b170829965 198 }
GerhardBerman 41:68b170829965 199 else if (biceps_l > threshold_l && biceps_r <= threshold_r){
GerhardBerman 41:68b170829965 200 //arm 1 activated, move left
GerhardBerman 41:68b170829965 201 Tout = -1;
GerhardBerman 41:68b170829965 202 }
GerhardBerman 41:68b170829965 203 else if (biceps_l <= threshold_l && biceps_r > threshold_r){
GerhardBerman 41:68b170829965 204 //arm 1 activated, move right
GerhardBerman 41:68b170829965 205 Tout = 1;
GerhardBerman 41:68b170829965 206 }
GerhardBerman 41:68b170829965 207 else{
GerhardBerman 41:68b170829965 208 //wait(0.2);
GerhardBerman 41:68b170829965 209 Tout = 0;
GerhardBerman 41:68b170829965 210 }
GerhardBerman 41:68b170829965 211
GerhardBerman 41:68b170829965 212 // scope.set(0, inLout);
GerhardBerman 41:68b170829965 213 // scope.set(1, inRout);
GerhardBerman 41:68b170829965 214
GerhardBerman 41:68b170829965 215 }
GerhardBerman 41:68b170829965 216
GerhardBerman 19:cba54636bd62 217 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
GerhardBerman 7:2f74dfd1d411 218
GerhardBerman 19:cba54636bd62 219 //get joint positions q feedback from encoder
GerhardBerman 19:cba54636bd62 220 float Encoder1Position = counts1/resolution; //angular position in radians, encoder axis
GerhardBerman 39:cc7754330c26 221 float Encoder2Position = -1*counts2/resolution; //NEGATIVE!
GerhardBerman 39:cc7754330c26 222
GerhardBerman 39:cc7754330c26 223 q1Encoder = Encoder1Position*inverse_gear_ratio;
GerhardBerman 39:cc7754330c26 224 q12Encoder = Encoder2Position*inverse_gear_ratio;
GerhardBerman 39:cc7754330c26 225 q1Out = q1start + q1Encoder; //angular position in radians, motor axis
GerhardBerman 39:cc7754330c26 226 q12Out = q12start + q12Encoder; //encoder 2 gives sum of both angles!
GerhardBerman 39:cc7754330c26 227 q2Out = q12Out - q1Out;
GerhardBerman 39:cc7754330c26 228 float q1deg = q1Out*360/2/PI;
GerhardBerman 39:cc7754330c26 229 float q2deg = q2Out*360/2/PI;
GerhardBerman 7:2f74dfd1d411 230
GerhardBerman 19:cba54636bd62 231 /*
GerhardBerman 19:cba54636bd62 232 //get end effector position feedback with Brockett
GerhardBerman 18:d2cfd07ae88a 233 float Position_x = ((L2 + x0)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - L0*sin(q1) + (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) - cos(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1)) - sin(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2))); //calculate end effector x-position from motor angles with Brockett, rx
GerhardBerman 18:d2cfd07ae88a 234 float Position_y = (L0 - (L2 + x0)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) - L0*cos(q1) - cos(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2)) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1))); //calculate end effector y-position from motor angles with Brockett, ry
GerhardBerman 19:cba54636bd62 235 */
GerhardBerman 19:cba54636bd62 236 //get end effector position feedback with trigonometry
GerhardBerman 19:cba54636bd62 237 Position_x = (L1*sin(q1) + L2*sin(q1+q2));
GerhardBerman 28:6d8c6fe79394 238 Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;
GerhardBerman 19:cba54636bd62 239 //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
GerhardBerman 29:caf3dd699617 240
GerhardBerman 33:b09608fa69e9 241 /*
GerhardBerman 29:caf3dd699617 242 if (Position_y < (0.5*TowerHeight)){
GerhardBerman 29:caf3dd699617 243 wait(1.0);
GerhardBerman 29:caf3dd699617 244 ReferencePosition_ynew = L1 + TowerHeight - StampHeight; //Reset vertical position after stamping
GerhardBerman 29:caf3dd699617 245 }
GerhardBerman 29:caf3dd699617 246 else{
GerhardBerman 33:b09608fa69e9 247 */
GerhardBerman 7:2f74dfd1d411 248 //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
GerhardBerman 19:cba54636bd62 249 biceps_l = !button1.read() * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
GerhardBerman 19:cba54636bd62 250 biceps_r = !button2.read() * EMGgain; //emg1.read();
GerhardBerman 41:68b170829965 251 if (T == -2){
GerhardBerman 8:935abf8ecc27 252 //both arms activated: stamp moves down
GerhardBerman 15:9061cf7db23e 253 //led1 = 1;
GerhardBerman 15:9061cf7db23e 254 //led2 = 1;
GerhardBerman 19:cba54636bd62 255 ReferencePosition_xnew = ReferencePosition_x;
GerhardBerman 41:68b170829965 256 ReferencePosition_ynew = ReferencePosition_y - 0.015; //ReferencePosition_y - dy_stampdown; //into stamping vertical position ~the stamp down action
GerhardBerman 18:d2cfd07ae88a 257
GerhardBerman 18:d2cfd07ae88a 258 }
GerhardBerman 41:68b170829965 259 else if (T==-1){
GerhardBerman 18:d2cfd07ae88a 260 //arm 1 activated, move left
GerhardBerman 18:d2cfd07ae88a 261 //led1 = 1;
GerhardBerman 18:d2cfd07ae88a 262 //led2 = 0;
GerhardBerman 41:68b170829965 263 ReferencePosition_xnew = ReferencePosition_x - 0.0009; //biceps_l;
GerhardBerman 34:96bcbddc7d2d 264 ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
GerhardBerman 12:05e5964b69a4 265 /*
GerhardBerman 18:d2cfd07ae88a 266 PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 267 PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 268
GerhardBerman 18:d2cfd07ae88a 269 dx = PositionError_x;
GerhardBerman 18:d2cfd07ae88a 270 dy = PositionError_y;
GerhardBerman 12:05e5964b69a4 271 q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
GerhardBerman 12:05e5964b69a4 272 q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
GerhardBerman 12:05e5964b69a4 273 */
GerhardBerman 8:935abf8ecc27 274 }
GerhardBerman 41:68b170829965 275 else if (T==1){
GerhardBerman 18:d2cfd07ae88a 276 //arm 1 activated, move right
GerhardBerman 18:d2cfd07ae88a 277 //led1 = 0;
GerhardBerman 18:d2cfd07ae88a 278 //led2 = 1;
GerhardBerman 41:68b170829965 279 ReferencePosition_xnew = ReferencePosition_x + 0.0009; //biceps_r;
GerhardBerman 34:96bcbddc7d2d 280 ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
GerhardBerman 18:d2cfd07ae88a 281 /*PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 282 PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 283
GerhardBerman 18:d2cfd07ae88a 284 dx = PositionError_x;
GerhardBerman 18:d2cfd07ae88a 285 dy = PositionError_y;
GerhardBerman 12:05e5964b69a4 286 q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
GerhardBerman 12:05e5964b69a4 287 q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
GerhardBerman 18:d2cfd07ae88a 288 */
GerhardBerman 8:935abf8ecc27 289 }
GerhardBerman 41:68b170829965 290 else{ //T==0
GerhardBerman 15:9061cf7db23e 291 //led1 = 0;
GerhardBerman 15:9061cf7db23e 292 //led2 = 0;
GerhardBerman 39:cc7754330c26 293 //ReferencePosition_xnew = ReferencePosition_x;
GerhardBerman 36:72f0913c5460 294 ReferencePosition_ynew = y_stampup; //ReferencePosition_y; //
GerhardBerman 19:cba54636bd62 295 }
GerhardBerman 33:b09608fa69e9 296 // }
GerhardBerman 19:cba54636bd62 297
GerhardBerman 39:cc7754330c26 298 //check position boundaries
GerhardBerman 39:cc7754330c26 299 if (ReferencePosition_xnew > maxStampDistance){
GerhardBerman 39:cc7754330c26 300 ReferencePosition_x = maxStampDistance; // - 0.1;
GerhardBerman 39:cc7754330c26 301 ReferencePosition_y = y_stampup;
GerhardBerman 39:cc7754330c26 302 pc.printf("Target too far! \r\n");
GerhardBerman 39:cc7754330c26 303 }
GerhardBerman 39:cc7754330c26 304 else if (ReferencePosition_xnew < minStampDistance){
GerhardBerman 39:cc7754330c26 305 ReferencePosition_x = minStampDistance; // + 0.1;
GerhardBerman 39:cc7754330c26 306 ReferencePosition_y = y_stampup;
GerhardBerman 39:cc7754330c26 307 pc.printf("Target too close! \r\n");
GerhardBerman 39:cc7754330c26 308 }
GerhardBerman 41:68b170829965 309 else if (ReferencePosition_ynew < y_stampdown){
GerhardBerman 41:68b170829965 310 ReferencePosition_x = ReferencePosition_xnew; // + 0.1;
GerhardBerman 41:68b170829965 311 ReferencePosition_y = y_stampdown;
GerhardBerman 41:68b170829965 312 pc.printf("Target too close! \r\n");
GerhardBerman 41:68b170829965 313 }
GerhardBerman 39:cc7754330c26 314 else {
GerhardBerman 39:cc7754330c26 315 ReferencePosition_x = ReferencePosition_xnew;
GerhardBerman 39:cc7754330c26 316 ReferencePosition_y = ReferencePosition_ynew;
GerhardBerman 39:cc7754330c26 317 }
GerhardBerman 39:cc7754330c26 318
GerhardBerman 28:6d8c6fe79394 319 float PointPositionArm2_x = ReferencePosition_x;
GerhardBerman 28:6d8c6fe79394 320 float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
GerhardBerman 28:6d8c6fe79394 321 float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2));
GerhardBerman 19:cba54636bd62 322
GerhardBerman 19:cba54636bd62 323 //calculate reference joint angles for the new reference position
GerhardBerman 28:6d8c6fe79394 324 float alpha = atan(PointPositionArm2_y/PointPositionArm2_x);
GerhardBerman 28:6d8c6fe79394 325 float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2));
GerhardBerman 19:cba54636bd62 326 q1_refOutNew = PI/2 - (alpha+beta);
GerhardBerman 28:6d8c6fe79394 327 q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2);
GerhardBerman 19:cba54636bd62 328
GerhardBerman 19:cba54636bd62 329 //check angle boundaries
GerhardBerman 39:cc7754330c26 330 if (q1_refOutNew < q1_refOutMin){
GerhardBerman 39:cc7754330c26 331 q1_refOut = q1_refOutMin;
GerhardBerman 39:cc7754330c26 332 pc.printf("\r\n Under q1 angle boundaries\r\n");
GerhardBerman 39:cc7754330c26 333 }
GerhardBerman 39:cc7754330c26 334 else if (q1_refOutNew > q1_refOutMax){
GerhardBerman 39:cc7754330c26 335 q1_refOut = q1_refOutMax;
GerhardBerman 39:cc7754330c26 336 pc.printf("\r\n Above q1 angle boundaries\r\n");
GerhardBerman 19:cba54636bd62 337 }
GerhardBerman 19:cba54636bd62 338 else {
GerhardBerman 39:cc7754330c26 339 q1_refOut = q1_refOutNew;
GerhardBerman 19:cba54636bd62 340 }
GerhardBerman 39:cc7754330c26 341
GerhardBerman 39:cc7754330c26 342 if (q2_refOutNew < q2_refOutMin){
GerhardBerman 39:cc7754330c26 343 q2_refOut = q2_refOutMin;
GerhardBerman 39:cc7754330c26 344 pc.printf("\r\n Under q2 angle boundaries");
GerhardBerman 39:cc7754330c26 345 }
GerhardBerman 39:cc7754330c26 346 else if (q2_refOutNew > q2_refOutMax){
GerhardBerman 39:cc7754330c26 347 q2_refOut = q2_refOutMax;
GerhardBerman 39:cc7754330c26 348 pc.printf("\r\n Above q2 angle boundaries");
GerhardBerman 19:cba54636bd62 349 }
GerhardBerman 19:cba54636bd62 350 else {
GerhardBerman 39:cc7754330c26 351 q2_refOut = q2_refOutNew;
GerhardBerman 39:cc7754330c26 352 }
GerhardBerman 7:2f74dfd1d411 353 //update joint angles
GerhardBerman 18:d2cfd07ae88a 354 //q1Out = q1Out + q1_dotOut; //in radians
GerhardBerman 18:d2cfd07ae88a 355 //q2Out = q2Out + q2_dotOut;
GerhardBerman 12:05e5964b69a4 356
GerhardBerman 34:96bcbddc7d2d 357 pc.baud(115200);
GerhardBerman 36:72f0913c5460 358 pc.printf("posX: %f ",Position_x);
GerhardBerman 36:72f0913c5460 359 pc.printf("posY: %f ",Position_y);
GerhardBerman 34:96bcbddc7d2d 360 pc.printf("refX: %f ",ReferencePosition_xnew);
GerhardBerman 34:96bcbddc7d2d 361 pc.printf("refY: %f ",ReferencePosition_ynew);
GerhardBerman 34:96bcbddc7d2d 362 pc.printf("q1: %f ", q1Out);
GerhardBerman 34:96bcbddc7d2d 363 pc.printf("q1ref: %f ", q1_refOut);
GerhardBerman 34:96bcbddc7d2d 364 pc.printf("q2: %f ", q2Out);
GerhardBerman 34:96bcbddc7d2d 365 pc.printf("q2ref: %f ", q2_refOut);
GerhardBerman 39:cc7754330c26 366 pc.printf("q1deg: %f ", q1deg);
GerhardBerman 39:cc7754330c26 367 pc.printf("q2deg: %f \r\n", q2deg);
GerhardBerman 41:68b170829965 368
GerhardBerman 41:68b170829965 369 scope.set(0, ReferencePosition_xnew);
GerhardBerman 41:68b170829965 370 scope.set(1, ReferencePosition_ynew);
GerhardBerman 41:68b170829965 371 scope.set(2, envelopeL);
GerhardBerman 41:68b170829965 372 scope.set(3, envelopeR);
GerhardBerman 41:68b170829965 373 scope.set(4, T);
GerhardBerman 41:68b170829965 374 scope.send();
GerhardBerman 18:d2cfd07ae88a 375 /*
GerhardBerman 12:05e5964b69a4 376 pc.printf("dx: %f \r\n", dx);
GerhardBerman 12:05e5964b69a4 377 pc.printf("dy: %f \r\n", dy);
GerhardBerman 12:05e5964b69a4 378 pc.printf("q1: %f \r\n", q1Out);
GerhardBerman 12:05e5964b69a4 379 pc.printf("q1_dot: %f \r\n", q1_dotOut);
GerhardBerman 12:05e5964b69a4 380 pc.printf("q2: %f \r\n", q2Out);
GerhardBerman 12:05e5964b69a4 381 pc.printf("q2_dot: %f \r\n", q2_dotOut);
GerhardBerman 12:05e5964b69a4 382
GerhardBerman 14:725a608b6709 383 pc.printf("Counts1: %f \r\n", counts1);
GerhardBerman 14:725a608b6709 384 pc.printf("Encoder1: %f \r\n", Encoder1Position);
GerhardBerman 18:d2cfd07ae88a 385 pc.printf("Motor1: %f \r\n", q1Out);
GerhardBerman 14:725a608b6709 386 pc.printf("Counts2: %f \r\n", counts2);
GerhardBerman 14:725a608b6709 387 pc.printf("Encoder2: %f \r\n", Encoder2Position);
GerhardBerman 18:d2cfd07ae88a 388 pc.printf("Motor2: %f \r\n", q2Out);
GerhardBerman 18:d2cfd07ae88a 389 */
GerhardBerman 29:caf3dd699617 390
GerhardBerman 7:2f74dfd1d411 391 }
GerhardBerman 18:d2cfd07ae88a 392
GerhardBerman 7:2f74dfd1d411 393
GerhardBerman 24:393da1cc1fa8 394 void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
GerhardBerman 7:2f74dfd1d411 395
GerhardBerman 7:2f74dfd1d411 396 // linear feedback control
GerhardBerman 19:cba54636bd62 397 float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians
GerhardBerman 19:cba54636bd62 398 float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians
GerhardBerman 7:2f74dfd1d411 399
GerhardBerman 40:9ecaab27acde 400 float TotalError1 = pidf.step(q1_error);
GerhardBerman 40:9ecaab27acde 401 float TotalError2 = pidf.step(q2_error);
GerhardBerman 40:9ecaab27acde 402
GerhardBerman 40:9ecaab27acde 403 /*
GerhardBerman 19:cba54636bd62 404 float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians
GerhardBerman 19:cba54636bd62 405 float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 406 //float maxKi = 0.2;
GerhardBerman 34:96bcbddc7d2d 407 float Ki = 0.04*Kp; //0.01*Kp; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 408
GerhardBerman 27:456e8a47f815 409 float q1DerivativeError = (q1_error - q1_error_prev)/t_sample; // derivative of error in radians
GerhardBerman 19:cba54636bd62 410 float q2DerivativeError = (q2_error_prev + q2_error)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 411 //float maxKd = 0.2;
GerhardBerman 33:b09608fa69e9 412 float Kd = 0.0; //0.04*Kp; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 413
GerhardBerman 7:2f74dfd1d411 414 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 415 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 416 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 417 //scope.send();
GerhardBerman 7:2f74dfd1d411 418
GerhardBerman 23:3a4d5e19c20d 419 TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output in radians = motor input
GerhardBerman 23:3a4d5e19c20d 420 TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output in radians = motor input
GerhardBerman 40:9ecaab27acde 421 */
GerhardBerman 33:b09608fa69e9 422 /*
GerhardBerman 33:b09608fa69e9 423 if (fabs(TotalError1) < TotalErrorMin) {
11i 21:dd51d78c0732 424 TotalError1=0;
11i 21:dd51d78c0732 425 }
11i 21:dd51d78c0732 426 else {
11i 21:dd51d78c0732 427 TotalError1=TotalError1;
11i 21:dd51d78c0732 428 }
GerhardBerman 33:b09608fa69e9 429 if (fabs(TotalError2) < TotalErrorMin) {
11i 21:dd51d78c0732 430 TotalError2=0;
11i 21:dd51d78c0732 431 }
11i 21:dd51d78c0732 432 else {
11i 21:dd51d78c0732 433 TotalError2=TotalError2;
11i 21:dd51d78c0732 434 }
GerhardBerman 33:b09608fa69e9 435 */
GerhardBerman 25:596255732b65 436 /*
GerhardBerman 25:596255732b65 437 DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
GerhardBerman 25:596255732b65 438 DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
GerhardBerman 25:596255732b65 439 motorValue1Out = DerTotalError1/MotorGain;
GerhardBerman 25:596255732b65 440 motorValue2Out = DerTotalError2/MotorGain;
GerhardBerman 25:596255732b65 441 */
GerhardBerman 25:596255732b65 442
GerhardBerman 39:cc7754330c26 443 motorValue1Out = Motor1ExtraGain*(TotalError1/MotorGain);
11i 21:dd51d78c0732 444 motorValue2Out = TotalError2/MotorGain;
11i 21:dd51d78c0732 445
GerhardBerman 33:b09608fa69e9 446 /*
GerhardBerman 29:caf3dd699617 447 scope.set(0,q1_ref);
GerhardBerman 29:caf3dd699617 448 scope.set(1,q1);
GerhardBerman 29:caf3dd699617 449 scope.set(2,q2_ref);
GerhardBerman 29:caf3dd699617 450 scope.set(3,q2);
GerhardBerman 29:caf3dd699617 451 scope.set(4,motorValue1Out);
GerhardBerman 29:caf3dd699617 452 scope.set(5,motorValue2Out);
GerhardBerman 29:caf3dd699617 453 scope.send();
GerhardBerman 33:b09608fa69e9 454 */
GerhardBerman 33:b09608fa69e9 455 /*
GerhardBerman 33:b09608fa69e9 456 pc.printf("E1: %f ", q1_error);
GerhardBerman 33:b09608fa69e9 457 pc.printf("IE1: %f ", q1IntError);
GerhardBerman 33:b09608fa69e9 458 pc.printf("DE1: %f ", q2DerivativeError);
GerhardBerman 33:b09608fa69e9 459 pc.printf("E2: %f ", q2_error);
GerhardBerman 33:b09608fa69e9 460 pc.printf("IE2: %f ", q2IntError);
GerhardBerman 33:b09608fa69e9 461 pc.printf("DE2: %f ", q2DerivativeError);
GerhardBerman 34:96bcbddc7d2d 462 */
GerhardBerman 39:cc7754330c26 463 //pc.printf("TE1: %f ", TotalError1);
GerhardBerman 39:cc7754330c26 464 //pc.printf("TE2: %f ", TotalError2);
GerhardBerman 39:cc7754330c26 465 pc.printf("M1: %f \r\n", motorValue1Out);
GerhardBerman 33:b09608fa69e9 466 pc.printf("M2: %f \r\n", motorValue2Out);
GerhardBerman 34:96bcbddc7d2d 467
GerhardBerman 19:cba54636bd62 468 q1_error_prev = q1_error;
GerhardBerman 19:cba54636bd62 469 q2_error_prev = q2_error;
GerhardBerman 25:596255732b65 470 TotalError1_prev = TotalError1;
GerhardBerman 33:b09608fa69e9 471 TotalError2_prev = TotalError2;
GerhardBerman 3:8caef4872b0c 472 }
GerhardBerman 3:8caef4872b0c 473
GerhardBerman 9:e4c34f5665a0 474 void SetMotor1(float motorValue1, float motorValue2)
GerhardBerman 3:8caef4872b0c 475 {
GerhardBerman 3:8caef4872b0c 476 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 477 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 478 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 479 // within range
GerhardBerman 9:e4c34f5665a0 480 //control motor 1
GerhardBerman 15:9061cf7db23e 481 if (motorValue1 >=0) //clockwise rotation
GerhardBerman 19:cba54636bd62 482 {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 483 //led1=1;
GerhardBerman 9:e4c34f5665a0 484 //led2=0;
GerhardBerman 3:8caef4872b0c 485 }
GerhardBerman 15:9061cf7db23e 486 else //counterclockwise rotation
GerhardBerman 19:cba54636bd62 487 {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 488 //led1=0;
GerhardBerman 9:e4c34f5665a0 489 //led2=1;
GerhardBerman 3:8caef4872b0c 490 }
GerhardBerman 39:cc7754330c26 491 if (fabs(motorValue1)>MotorMaxSpeed){
GerhardBerman 39:cc7754330c26 492 motor1MagnitudePin = MotorMaxSpeed;
GerhardBerman 29:caf3dd699617 493 }
GerhardBerman 29:caf3dd699617 494 else{
GerhardBerman 29:caf3dd699617 495 motor1MagnitudePin = fabs(motorValue1); //fabs(motorValue1);
GerhardBerman 29:caf3dd699617 496 }
GerhardBerman 29:caf3dd699617 497
GerhardBerman 9:e4c34f5665a0 498 //control motor 2
GerhardBerman 15:9061cf7db23e 499 if (motorValue2 >=0) //clockwise rotation
GerhardBerman 39:cc7754330c26 500 {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 501 //led1=1;
GerhardBerman 10:45473f650198 502 //led2=0;
GerhardBerman 7:2f74dfd1d411 503 }
GerhardBerman 15:9061cf7db23e 504 else //counterclockwise rotation
GerhardBerman 39:cc7754330c26 505 {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 506 //led1=0;
GerhardBerman 10:45473f650198 507 //led2=1;
GerhardBerman 7:2f74dfd1d411 508 }
GerhardBerman 39:cc7754330c26 509 if (fabs(motorValue2)>MotorMaxSpeed){
GerhardBerman 39:cc7754330c26 510 motor2MagnitudePin = MotorMaxSpeed;
GerhardBerman 29:caf3dd699617 511 }
GerhardBerman 29:caf3dd699617 512 else{
GerhardBerman 29:caf3dd699617 513 motor2MagnitudePin = fabs(motorValue2);
GerhardBerman 29:caf3dd699617 514 }
GerhardBerman 39:cc7754330c26 515 float ReadDir1 = motor1DirectionPin.read();
GerhardBerman 39:cc7754330c26 516 float ReadDir2 = motor2DirectionPin.read();
GerhardBerman 39:cc7754330c26 517 pc.printf("M1 dir: %f \r\n", ReadDir1);
GerhardBerman 39:cc7754330c26 518 pc.printf("M2 dir: %f \r\n", ReadDir2);
GerhardBerman 3:8caef4872b0c 519 }
GerhardBerman 3:8caef4872b0c 520
GerhardBerman 3:8caef4872b0c 521 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 522 {
GerhardBerman 9:e4c34f5665a0 523 // This function measures the EMG of both arms, calculates via IK what
GerhardBerman 24:393da1cc1fa8 524 // the joint positions should be, and controls the motor with
GerhardBerman 24:393da1cc1fa8 525 // a Feedback controller. This is called from a Ticker.
GerhardBerman 41:68b170829965 526 FilteredSample(T, envelopeL, envelopeR);
GerhardBerman 19:cba54636bd62 527 GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
GerhardBerman 24:393da1cc1fa8 528 FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
GerhardBerman 9:e4c34f5665a0 529 SetMotor1(motorValue1, motorValue2);
GerhardBerman 3:8caef4872b0c 530 }
GerhardBerman 3:8caef4872b0c 531
GerhardBerman 3:8caef4872b0c 532 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 533 //wait(1);
GerhardBerman 3:8caef4872b0c 534 //float Potmeter1 = potMeter1.read();
GerhardBerman 7:2f74dfd1d411 535 //float referencePosition1 = GetReferencePosition();
GerhardBerman 7:2f74dfd1d411 536 //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
GerhardBerman 3:8caef4872b0c 537 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 538 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 7:2f74dfd1d411 539 //pc.printf("TTCounts: %i \r\n", counts1);
GerhardBerman 3:8caef4872b0c 540 }
GerhardBerman 7:2f74dfd1d411 541
GerhardBerman 3:8caef4872b0c 542 /*
GerhardBerman 3:8caef4872b0c 543 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 544 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 545 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 546 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 547 }
GerhardBerman 9:e4c34f5665a0 548 */
GerhardBerman 6:3c4f3f2ce54f 549
GerhardBerman 0:43160ef59f9f 550 int main()
GerhardBerman 0:43160ef59f9f 551 {
GerhardBerman 0:43160ef59f9f 552 //Initialize
GerhardBerman 15:9061cf7db23e 553 //int led1val = led1.read();
GerhardBerman 15:9061cf7db23e 554 //int led2val = led2.read();
GerhardBerman 34:96bcbddc7d2d 555 pc.baud(115200);
GerhardBerman 34:96bcbddc7d2d 556 pc.printf("Test putty IK");
GerhardBerman 41:68b170829965 557 ledRed=1;
GerhardBerman 41:68b170829965 558 ledBlue=1;
GerhardBerman 41:68b170829965 559 ledRed=0; //red
GerhardBerman 41:68b170829965 560
GerhardBerman 41:68b170829965 561 bcq1R.add(&bq1R).add(&bq2R); //set BiQuad chains
GerhardBerman 41:68b170829965 562 bcq2R.add(&bq3R);
GerhardBerman 41:68b170829965 563
GerhardBerman 41:68b170829965 564 bcq1L.add(&bq1L).add(&bq2L);
GerhardBerman 41:68b170829965 565 bcq2L.add(&bq3L);
GerhardBerman 41:68b170829965 566
GerhardBerman 36:72f0913c5460 567 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 36:72f0913c5460 568 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 41:68b170829965 569 wait(20.0);
GerhardBerman 41:68b170829965 570 MeasureTicker.attach(&MeasureTicker_act, 0.002); //0.1f);
GerhardBerman 41:68b170829965 571 //bqc.add(&bq1).add(&bq2); //set BiQuad chain
GerhardBerman 40:9ecaab27acde 572 pidf.PIDF( Kp, Ki, Kd, N, t_sample ); //set PID filter
GerhardBerman 0:43160ef59f9f 573 while(1)
GerhardBerman 0:43160ef59f9f 574 {
GerhardBerman 3:8caef4872b0c 575 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 576 MeasureTicker_go=false;
GerhardBerman 41:68b170829965 577 ledGrn = 1;
GerhardBerman 41:68b170829965 578 ledBlue = 0;
GerhardBerman 3:8caef4872b0c 579 MeasureAndControl();
GerhardBerman 15:9061cf7db23e 580 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 15:9061cf7db23e 581 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 33:b09608fa69e9 582 //pc.printf("counts1: %i ", counts1);
GerhardBerman 33:b09608fa69e9 583 //pc.printf("counts2: %i \r\n", counts2);
GerhardBerman 41:68b170829965 584 ledBlue = 1;
GerhardBerman 41:68b170829965 585 ledGrn = 0;
GerhardBerman 3:8caef4872b0c 586 }
GerhardBerman 10:45473f650198 587 /*
GerhardBerman 3:8caef4872b0c 588 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 589 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 590 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 591 }
GerhardBerman 10:45473f650198 592 */
GerhardBerman 0:43160ef59f9f 593 }
GerhardBerman 0:43160ef59f9f 594 }