Gerhard Berman / Mbed 2 deprecated EMG_prog_forwardkin_feedback

EMG added to main IK program. No errors, not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_forwardkin_feedback_copy3 by Gerhard Berman

main.cpp@37:4d75c2432d71, 2016-11-02 (annotated)

Committer:
GerhardBerman
Date:
Wed Nov 02 16:12:00 2016 +0000
Revision:
37:4d75c2432d71
Parent:
36:a700f64ba747 
Sample Frequency changed to 500 Hz, right arm EMG is correct, left arm EMG is too small

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