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@37:1360978f49ba, 2016-11-03 (annotated)

Committer:
GerhardBerman
Date:
Thu Nov 03 10:34:51 2016 +0000
Revision:
37:1360978f49ba
Parent:
36:72f0913c5460 
Check for encoder counts and angles, rest disabled

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 0:43160ef59f9f 40
GerhardBerman 33:b09608fa69e9 41 DigitalOut ledgrn(LED_GREEN);
GerhardBerman 33:b09608fa69e9 42 DigitalOut ledred(LED_RED);
GerhardBerman 33:b09608fa69e9 43 DigitalOut ledblue(LED_BLUE);
GerhardBerman 33:b09608fa69e9 44
GerhardBerman 7:2f74dfd1d411 45 //library settings
GerhardBerman 0:43160ef59f9f 46 Serial pc(USBTX,USBRX);
GerhardBerman 3:8caef4872b0c 47 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
GerhardBerman 33:b09608fa69e9 48 //HIDScope scope(6);
GerhardBerman 0:43160ef59f9f 49
GerhardBerman 26:bb7e14f59ddd 50 //initial values
GerhardBerman 7:2f74dfd1d411 51 float dx;
GerhardBerman 7:2f74dfd1d411 52 float dy;
GerhardBerman 23:3a4d5e19c20d 53 double DerivativeCounts;
GerhardBerman 19:cba54636bd62 54 //float referenceVelocity = 0;
GerhardBerman 19:cba54636bd62 55 //float bqcDerivativeCounts = 0;
GerhardBerman 3:8caef4872b0c 56 const float PI = 3.141592653589793;
GerhardBerman 3:8caef4872b0c 57 const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1
GerhardBerman 3:8caef4872b0c 58 const int ccw = 1;
GerhardBerman 0:43160ef59f9f 59
GerhardBerman 26:bb7e14f59ddd 60 //set lengths (VALUES HAVE TO BE CHANGED)
GerhardBerman 36:72f0913c5460 61 //float x0 = 1.0;
GerhardBerman 36:72f0913c5460 62 float L0 = 0.232;
GerhardBerman 36:72f0913c5460 63 float L1 = 0.45;
GerhardBerman 36:72f0913c5460 64 float L2 = 0.35;
GerhardBerman 36:72f0913c5460 65 float TowerHeight = 0.232; //height of motor axes above table surface!
GerhardBerman 36:72f0913c5460 66 float StampHeight = 0.06; // height of end effector
GerhardBerman 34:96bcbddc7d2d 67 float y_stampup = 0.1; //height stamp while not stamping: 10cm above table surface
GerhardBerman 34:96bcbddc7d2d 68 float y_stampdown = 0.0; //height stamp while stamping: at table surface
GerhardBerman 34:96bcbddc7d2d 69
GerhardBerman 26:bb7e14f59ddd 70
GerhardBerman 19:cba54636bd62 71 //set initial conditions
GerhardBerman 19:cba54636bd62 72 float biceps_l = 0;
GerhardBerman 19:cba54636bd62 73 float biceps_r = 0;
GerhardBerman 19:cba54636bd62 74 float ReferencePosition_x = L2;
GerhardBerman 28:6d8c6fe79394 75 float ReferencePosition_y = L1 + TowerHeight - StampHeight;
GerhardBerman 19:cba54636bd62 76 float ReferencePosition_xnew = 0;
GerhardBerman 19:cba54636bd62 77 float ReferencePosition_ynew = 0;
GerhardBerman 19:cba54636bd62 78 float Position_x = 0.0;
GerhardBerman 19:cba54636bd62 79 float Position_y = 0.0;
GerhardBerman 19:cba54636bd62 80 float q1 = 0;
GerhardBerman 19:cba54636bd62 81 float q2 = 0;
GerhardBerman 19:cba54636bd62 82 float q1_ref = 0;
GerhardBerman 19:cba54636bd62 83 float q2_ref = 0;
GerhardBerman 19:cba54636bd62 84 float q1start = 0;
GerhardBerman 19:cba54636bd62 85 float q2start = PI/2;
GerhardBerman 19:cba54636bd62 86
GerhardBerman 19:cba54636bd62 87 float q1_error_prev = 0;
GerhardBerman 19:cba54636bd62 88 float q2_error_prev = 0;
GerhardBerman 25:596255732b65 89 float DerTotalError1 = 0;
GerhardBerman 25:596255732b65 90 float DerTotalError2 = 0;
GerhardBerman 19:cba54636bd62 91 float q1IntError = 0;
GerhardBerman 19:cba54636bd62 92 float q2IntError = 0;
GerhardBerman 29:caf3dd699617 93 float TotalError1_prev = 0;
GerhardBerman 29:caf3dd699617 94 float TotalError2_prev = 0;
GerhardBerman 19:cba54636bd62 95
GerhardBerman 19:cba54636bd62 96 float motorValue1 = 0.0;
GerhardBerman 19:cba54636bd62 97 float motorValue2 = 0.0;
GerhardBerman 19:cba54636bd62 98 int counts1 = 0;
GerhardBerman 19:cba54636bd62 99 int counts2 = 0;
GerhardBerman 19:cba54636bd62 100 int counts1Prev = 0;
GerhardBerman 19:cba54636bd62 101 int counts2Prev = 0;
GerhardBerman 26:bb7e14f59ddd 102
GerhardBerman 26:bb7e14f59ddd 103
GerhardBerman 26:bb7e14f59ddd 104 //set constant or variable values (VALUES HAVE TO BE EDITED)
GerhardBerman 26:bb7e14f59ddd 105 float EMGgain = 1.0;
GerhardBerman 26:bb7e14f59ddd 106 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
GerhardBerman 26:bb7e14f59ddd 107 float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
GerhardBerman 34:96bcbddc7d2d 108 float t_sample = 0.1; //seconds
GerhardBerman 34:96bcbddc7d2d 109 const float maxStampDistance = 2.0;
GerhardBerman 26:bb7e14f59ddd 110
GerhardBerman 26:bb7e14f59ddd 111 float q1_refOutNew = 0;
GerhardBerman 36:72f0913c5460 112 float q1_refOutMin = 0; //WRONG values
GerhardBerman 36:72f0913c5460 113 float q1_refOutMax = 0.4722*PI; //WRONG values
GerhardBerman 26:bb7e14f59ddd 114 float q2_refOutNew = 0;
GerhardBerman 36:72f0913c5460 115 float q2_refOutMin = 0.2611*PI; //WRONG values
GerhardBerman 36:72f0913c5460 116 float q2_refOutMax = 0.6667*PI; //WRONG values
GerhardBerman 26:bb7e14f59ddd 117 float TotalError1= 0;
GerhardBerman 26:bb7e14f59ddd 118 float TotalError2= 0;
GerhardBerman 26:bb7e14f59ddd 119 float TotalErrorMin= 0;
GerhardBerman 19:cba54636bd62 120
GerhardBerman 0:43160ef59f9f 121 //set BiQuad
GerhardBerman 0:43160ef59f9f 122 BiQuadChain bqc;
GerhardBerman 0:43160ef59f9f 123 BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
GerhardBerman 0:43160ef59f9f 124 BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
GerhardBerman 0:43160ef59f9f 125
GerhardBerman 0:43160ef59f9f 126 //set go-Ticker settings
GerhardBerman 3:8caef4872b0c 127 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
GerhardBerman 3:8caef4872b0c 128 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
GerhardBerman 3:8caef4872b0c 129 void BiQuadTicker_act(){BiQuadTicker_go=true;};
GerhardBerman 3:8caef4872b0c 130 void FeedbackTicker_act(){FeedbackTicker_go=true;};
GerhardBerman 3:8caef4872b0c 131 void TimeTracker_act(){TimeTracker_go=true;};
GerhardBerman 3:8caef4872b0c 132 //void sampleT_act(){sampleT_go=true;};
GerhardBerman 3:8caef4872b0c 133
GerhardBerman 3:8caef4872b0c 134 //define encoder counts and degrees
GerhardBerman 7:2f74dfd1d411 135 QEI Encoder1(D12, D13, NC, 32); // turns on encoder
GerhardBerman 15:9061cf7db23e 136 QEI Encoder2(D10, D11, NC, 32); // turns on encoder
GerhardBerman 14:725a608b6709 137
GerhardBerman 4:19e376d31380 138 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 139 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 140 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 141
GerhardBerman 19:cba54636bd62 142 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
GerhardBerman 7:2f74dfd1d411 143
GerhardBerman 19:cba54636bd62 144 //get joint positions q feedback from encoder
GerhardBerman 19:cba54636bd62 145 float Encoder1Position = counts1/resolution; //angular position in radians, encoder axis
GerhardBerman 37:1360978f49ba 146 float Encoder2Position = -1*counts2/resolution;
GerhardBerman 19:cba54636bd62 147
GerhardBerman 19:cba54636bd62 148 q1Out = q1start + Encoder1Position*inverse_gear_ratio; //angular position in radians, motor axis
GerhardBerman 19:cba54636bd62 149 q2Out = q2start + Encoder2Position*inverse_gear_ratio;
GerhardBerman 37:1360978f49ba 150
GerhardBerman 19:cba54636bd62 151 //get end effector position feedback with trigonometry
GerhardBerman 19:cba54636bd62 152 Position_x = (L1*sin(q1) + L2*sin(q1+q2));
GerhardBerman 28:6d8c6fe79394 153 Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;
GerhardBerman 37:1360978f49ba 154
GerhardBerman 34:96bcbddc7d2d 155 pc.baud(115200);
GerhardBerman 37:1360978f49ba 156 pc.printf("Counts1: % f ", counts1);
GerhardBerman 37:1360978f49ba 157 pc.printf("Counts2: % f ", counts2);
GerhardBerman 36:72f0913c5460 158 pc.printf("posX: %f ",Position_x);
GerhardBerman 36:72f0913c5460 159 pc.printf("posY: %f ",Position_y);
GerhardBerman 34:96bcbddc7d2d 160 pc.printf("q1: %f ", q1Out);
GerhardBerman 37:1360978f49ba 161 pc.printf("q2: %f \r\n", q2Out);
GerhardBerman 37:1360978f49ba 162
GerhardBerman 7:2f74dfd1d411 163 }
GerhardBerman 18:d2cfd07ae88a 164
GerhardBerman 7:2f74dfd1d411 165
GerhardBerman 24:393da1cc1fa8 166 void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
GerhardBerman 7:2f74dfd1d411 167
GerhardBerman 7:2f74dfd1d411 168 // linear feedback control
GerhardBerman 19:cba54636bd62 169 float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians
GerhardBerman 19:cba54636bd62 170 float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians
GerhardBerman 36:72f0913c5460 171 float Kp = 6; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 172
GerhardBerman 19:cba54636bd62 173 float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians
GerhardBerman 19:cba54636bd62 174 float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 175 //float maxKi = 0.2;
GerhardBerman 34:96bcbddc7d2d 176 float Ki = 0.04*Kp; //0.01*Kp; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 177
GerhardBerman 27:456e8a47f815 178 float q1DerivativeError = (q1_error - q1_error_prev)/t_sample; // derivative of error in radians
GerhardBerman 19:cba54636bd62 179 float q2DerivativeError = (q2_error_prev + q2_error)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 180 //float maxKd = 0.2;
GerhardBerman 33:b09608fa69e9 181 float Kd = 0.0; //0.04*Kp; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 182
GerhardBerman 7:2f74dfd1d411 183 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 184 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 185 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 186 //scope.send();
GerhardBerman 7:2f74dfd1d411 187
GerhardBerman 23:3a4d5e19c20d 188 TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output in radians = motor input
GerhardBerman 23:3a4d5e19c20d 189 TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output in radians = motor input
11i 21:dd51d78c0732 190
GerhardBerman 33:b09608fa69e9 191 /*
GerhardBerman 33:b09608fa69e9 192 if (fabs(TotalError1) < TotalErrorMin) {
11i 21:dd51d78c0732 193 TotalError1=0;
11i 21:dd51d78c0732 194 }
11i 21:dd51d78c0732 195 else {
11i 21:dd51d78c0732 196 TotalError1=TotalError1;
11i 21:dd51d78c0732 197 }
GerhardBerman 33:b09608fa69e9 198 if (fabs(TotalError2) < TotalErrorMin) {
11i 21:dd51d78c0732 199 TotalError2=0;
11i 21:dd51d78c0732 200 }
11i 21:dd51d78c0732 201 else {
11i 21:dd51d78c0732 202 TotalError2=TotalError2;
11i 21:dd51d78c0732 203 }
GerhardBerman 33:b09608fa69e9 204 */
GerhardBerman 25:596255732b65 205 /*
GerhardBerman 25:596255732b65 206 DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
GerhardBerman 25:596255732b65 207 DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
GerhardBerman 25:596255732b65 208 motorValue1Out = DerTotalError1/MotorGain;
GerhardBerman 25:596255732b65 209 motorValue2Out = DerTotalError2/MotorGain;
GerhardBerman 25:596255732b65 210 */
GerhardBerman 25:596255732b65 211
11i 21:dd51d78c0732 212 motorValue1Out = TotalError1/MotorGain;
11i 21:dd51d78c0732 213 motorValue2Out = TotalError2/MotorGain;
11i 21:dd51d78c0732 214
GerhardBerman 33:b09608fa69e9 215 /*
GerhardBerman 29:caf3dd699617 216 scope.set(0,q1_ref);
GerhardBerman 29:caf3dd699617 217 scope.set(1,q1);
GerhardBerman 29:caf3dd699617 218 scope.set(2,q2_ref);
GerhardBerman 29:caf3dd699617 219 scope.set(3,q2);
GerhardBerman 29:caf3dd699617 220 scope.set(4,motorValue1Out);
GerhardBerman 29:caf3dd699617 221 scope.set(5,motorValue2Out);
GerhardBerman 29:caf3dd699617 222 scope.send();
GerhardBerman 33:b09608fa69e9 223 */
GerhardBerman 33:b09608fa69e9 224 /*
GerhardBerman 33:b09608fa69e9 225 pc.printf("E1: %f ", q1_error);
GerhardBerman 33:b09608fa69e9 226 pc.printf("IE1: %f ", q1IntError);
GerhardBerman 33:b09608fa69e9 227 pc.printf("DE1: %f ", q2DerivativeError);
GerhardBerman 33:b09608fa69e9 228 pc.printf("E2: %f ", q2_error);
GerhardBerman 33:b09608fa69e9 229 pc.printf("IE2: %f ", q2IntError);
GerhardBerman 33:b09608fa69e9 230 pc.printf("DE2: %f ", q2DerivativeError);
GerhardBerman 34:96bcbddc7d2d 231 */
GerhardBerman 33:b09608fa69e9 232 pc.printf("TE1: %f ", TotalError1);
GerhardBerman 33:b09608fa69e9 233 pc.printf("TE2: %f ", TotalError2);
GerhardBerman 33:b09608fa69e9 234 pc.printf("M1: %f ", motorValue1Out);
GerhardBerman 33:b09608fa69e9 235 pc.printf("M2: %f \r\n", motorValue2Out);
GerhardBerman 34:96bcbddc7d2d 236
GerhardBerman 19:cba54636bd62 237 q1_error_prev = q1_error;
GerhardBerman 19:cba54636bd62 238 q2_error_prev = q2_error;
GerhardBerman 25:596255732b65 239 TotalError1_prev = TotalError1;
GerhardBerman 33:b09608fa69e9 240 TotalError2_prev = TotalError2;
GerhardBerman 3:8caef4872b0c 241 }
GerhardBerman 3:8caef4872b0c 242
GerhardBerman 9:e4c34f5665a0 243 void SetMotor1(float motorValue1, float motorValue2)
GerhardBerman 3:8caef4872b0c 244 {
GerhardBerman 3:8caef4872b0c 245 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 246 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 247 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 248 // within range
GerhardBerman 9:e4c34f5665a0 249 //control motor 1
GerhardBerman 15:9061cf7db23e 250 if (motorValue1 >=0) //clockwise rotation
GerhardBerman 19:cba54636bd62 251 {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 252 //led1=1;
GerhardBerman 9:e4c34f5665a0 253 //led2=0;
GerhardBerman 3:8caef4872b0c 254 }
GerhardBerman 15:9061cf7db23e 255 else //counterclockwise rotation
GerhardBerman 19:cba54636bd62 256 {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 257 //led1=0;
GerhardBerman 9:e4c34f5665a0 258 //led2=1;
GerhardBerman 3:8caef4872b0c 259 }
GerhardBerman 29:caf3dd699617 260 if (fabs(motorValue1)>1){
GerhardBerman 29:caf3dd699617 261 motor1MagnitudePin = 1;
GerhardBerman 29:caf3dd699617 262 }
GerhardBerman 29:caf3dd699617 263 else{
GerhardBerman 29:caf3dd699617 264 motor1MagnitudePin = fabs(motorValue1); //fabs(motorValue1);
GerhardBerman 29:caf3dd699617 265 }
GerhardBerman 29:caf3dd699617 266
GerhardBerman 9:e4c34f5665a0 267 //control motor 2
GerhardBerman 15:9061cf7db23e 268 if (motorValue2 >=0) //clockwise rotation
GerhardBerman 14:725a608b6709 269 {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 270 //led1=1;
GerhardBerman 10:45473f650198 271 //led2=0;
GerhardBerman 7:2f74dfd1d411 272 }
GerhardBerman 15:9061cf7db23e 273 else //counterclockwise rotation
GerhardBerman 15:9061cf7db23e 274 {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 275 //led1=0;
GerhardBerman 10:45473f650198 276 //led2=1;
GerhardBerman 7:2f74dfd1d411 277 }
GerhardBerman 29:caf3dd699617 278 if (fabs(motorValue2)>1){
GerhardBerman 29:caf3dd699617 279 motor2MagnitudePin = 1;
GerhardBerman 29:caf3dd699617 280 }
GerhardBerman 29:caf3dd699617 281 else{
GerhardBerman 29:caf3dd699617 282 motor2MagnitudePin = fabs(motorValue2);
GerhardBerman 29:caf3dd699617 283 }
GerhardBerman 19:cba54636bd62 284 float ReadMagn1 = motor1MagnitudePin.read();
GerhardBerman 19:cba54636bd62 285 float ReadMagn2 = motor2MagnitudePin.read();
GerhardBerman 33:b09608fa69e9 286 //pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);
GerhardBerman 33:b09608fa69e9 287 //pc.printf("motor2Magnitude: %f \r\n", ReadMagn2);
GerhardBerman 3:8caef4872b0c 288 }
GerhardBerman 3:8caef4872b0c 289
GerhardBerman 3:8caef4872b0c 290 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 291 {
GerhardBerman 9:e4c34f5665a0 292 // This function measures the EMG of both arms, calculates via IK what
GerhardBerman 24:393da1cc1fa8 293 // the joint positions should be, and controls the motor with
GerhardBerman 24:393da1cc1fa8 294 // a Feedback controller. This is called from a Ticker.
GerhardBerman 19:cba54636bd62 295 GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
GerhardBerman 37:1360978f49ba 296 //FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
GerhardBerman 37:1360978f49ba 297 //SetMotor1(motorValue1, motorValue2);
GerhardBerman 3:8caef4872b0c 298 }
GerhardBerman 3:8caef4872b0c 299
GerhardBerman 3:8caef4872b0c 300 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 301 //wait(1);
GerhardBerman 3:8caef4872b0c 302 //float Potmeter1 = potMeter1.read();
GerhardBerman 7:2f74dfd1d411 303 //float referencePosition1 = GetReferencePosition();
GerhardBerman 7:2f74dfd1d411 304 //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
GerhardBerman 3:8caef4872b0c 305 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 306 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 7:2f74dfd1d411 307 //pc.printf("TTCounts: %i \r\n", counts1);
GerhardBerman 3:8caef4872b0c 308 }
GerhardBerman 7:2f74dfd1d411 309
GerhardBerman 3:8caef4872b0c 310 /*
GerhardBerman 3:8caef4872b0c 311 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 312 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 313 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 314 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 315 }
GerhardBerman 9:e4c34f5665a0 316 */
GerhardBerman 6:3c4f3f2ce54f 317
GerhardBerman 0:43160ef59f9f 318 int main()
GerhardBerman 0:43160ef59f9f 319 {
GerhardBerman 0:43160ef59f9f 320 //Initialize
GerhardBerman 15:9061cf7db23e 321 //int led1val = led1.read();
GerhardBerman 15:9061cf7db23e 322 //int led2val = led2.read();
GerhardBerman 34:96bcbddc7d2d 323 pc.baud(115200);
GerhardBerman 34:96bcbddc7d2d 324 pc.printf("Test putty IK");
GerhardBerman 37:1360978f49ba 325 // counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 37:1360978f49ba 326 // counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 37:1360978f49ba 327 //wait(10.0);
GerhardBerman 34:96bcbddc7d2d 328 MeasureTicker.attach(&MeasureTicker_act, 0.1f);
GerhardBerman 10:45473f650198 329 bqc.add(&bq1).add(&bq2);
GerhardBerman 10:45473f650198 330
GerhardBerman 0:43160ef59f9f 331 while(1)
GerhardBerman 0:43160ef59f9f 332 {
GerhardBerman 3:8caef4872b0c 333 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 334 MeasureTicker_go=false;
GerhardBerman 33:b09608fa69e9 335 ledgrn = 1;
GerhardBerman 33:b09608fa69e9 336 ledblue = 0;
GerhardBerman 3:8caef4872b0c 337 MeasureAndControl();
GerhardBerman 15:9061cf7db23e 338 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 15:9061cf7db23e 339 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 37:1360978f49ba 340 pc.printf("counts1main: %i ", counts1);
GerhardBerman 37:1360978f49ba 341 pc.printf("counts2main: %i ", counts2);
GerhardBerman 33:b09608fa69e9 342 ledblue = 1;
GerhardBerman 33:b09608fa69e9 343 ledgrn = 0;
GerhardBerman 3:8caef4872b0c 344 }
GerhardBerman 10:45473f650198 345 /*
GerhardBerman 3:8caef4872b0c 346 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 347 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 348 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 349 }
GerhardBerman 10:45473f650198 350 */
GerhardBerman 0:43160ef59f9f 351 }
GerhardBerman 0:43160ef59f9f 352 }