Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
main.cpp@31:880ebd2e8145, 2016-10-28 (annotated)
- Committer:
- GerhardBerman
- Date:
- Fri Oct 28 14:21:46 2016 +0000
- Revision:
- 31:880ebd2e8145
- Parent:
- 30:676ecd59467a
ErrorSwitch loop is introduced, not yet tested
Who changed what in which revision?
User | Revision | Line number | New 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 | 31:880ebd2e8145 | 19 | //- Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation! |
GerhardBerman | 31:880ebd2e8145 | 20 | - MotorGain could change due to arm weight! |
GerhardBerman | 22:c679f753a8bd | 21 | - Arms should be placed manually into reference position. |
GerhardBerman | 17:91d20d362e72 | 22 | */ |
GerhardBerman | 17:91d20d362e72 | 23 | |
GerhardBerman | 0:43160ef59f9f | 24 | //set pins |
GerhardBerman | 0:43160ef59f9f | 25 | DigitalIn encoder1A (D13); //Channel A van Encoder 1 |
GerhardBerman | 0:43160ef59f9f | 26 | DigitalIn encoder1B (D12); //Channel B van Encoder 1 |
GerhardBerman | 14:725a608b6709 | 27 | DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15 |
GerhardBerman | 14:725a608b6709 | 28 | DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14 |
GerhardBerman | 14:725a608b6709 | 29 | //DigitalOut led1 (D11); |
GerhardBerman | 14:725a608b6709 | 30 | //DigitalOut led2 (D10); |
GerhardBerman | 19:cba54636bd62 | 31 | //AnalogIn potMeter1(A2); |
GerhardBerman | 19:cba54636bd62 | 32 | //AnalogIn potMeter2(A1); |
GerhardBerman | 0:43160ef59f9f | 33 | DigitalOut motor1DirectionPin(D7); |
GerhardBerman | 0:43160ef59f9f | 34 | PwmOut motor1MagnitudePin(D6); |
GerhardBerman | 7:2f74dfd1d411 | 35 | DigitalOut motor2DirectionPin(D4); |
GerhardBerman | 7:2f74dfd1d411 | 36 | PwmOut motor2MagnitudePin(D5); |
GerhardBerman | 9:e4c34f5665a0 | 37 | DigitalIn button1(D3); |
GerhardBerman | 7:2f74dfd1d411 | 38 | DigitalIn button2(D9); |
GerhardBerman | 0:43160ef59f9f | 39 | |
GerhardBerman | 7:2f74dfd1d411 | 40 | //library settings |
GerhardBerman | 0:43160ef59f9f | 41 | Serial pc(USBTX,USBRX); |
GerhardBerman | 3:8caef4872b0c | 42 | Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT; |
GerhardBerman | 31:880ebd2e8145 | 43 | //HIDScope scope(6); |
GerhardBerman | 0:43160ef59f9f | 44 | |
GerhardBerman | 26:bb7e14f59ddd | 45 | //initial values |
GerhardBerman | 7:2f74dfd1d411 | 46 | float dx; |
GerhardBerman | 7:2f74dfd1d411 | 47 | float dy; |
GerhardBerman | 23:3a4d5e19c20d | 48 | double DerivativeCounts; |
GerhardBerman | 19:cba54636bd62 | 49 | //float referenceVelocity = 0; |
GerhardBerman | 19:cba54636bd62 | 50 | //float bqcDerivativeCounts = 0; |
GerhardBerman | 3:8caef4872b0c | 51 | const float PI = 3.141592653589793; |
GerhardBerman | 3:8caef4872b0c | 52 | const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1 |
GerhardBerman | 3:8caef4872b0c | 53 | const int ccw = 1; |
GerhardBerman | 0:43160ef59f9f | 54 | |
GerhardBerman | 26:bb7e14f59ddd | 55 | //set lengths (VALUES HAVE TO BE CHANGED) |
GerhardBerman | 26:bb7e14f59ddd | 56 | float x0 = 1.0; |
GerhardBerman | 26:bb7e14f59ddd | 57 | float L0 = 1.0; |
GerhardBerman | 26:bb7e14f59ddd | 58 | float L1 = 1.0; |
GerhardBerman | 26:bb7e14f59ddd | 59 | float L2 = 1.0; |
GerhardBerman | 26:bb7e14f59ddd | 60 | float TowerHeight = 0.4; //height of motor axes above table surface! |
GerhardBerman | 28:6d8c6fe79394 | 61 | float StampHeight = 0.1; // height of end effector |
GerhardBerman | 26:bb7e14f59ddd | 62 | |
GerhardBerman | 19:cba54636bd62 | 63 | //set initial conditions |
GerhardBerman | 19:cba54636bd62 | 64 | float biceps_l = 0; |
GerhardBerman | 19:cba54636bd62 | 65 | float biceps_r = 0; |
GerhardBerman | 19:cba54636bd62 | 66 | float ReferencePosition_x = L2; |
GerhardBerman | 28:6d8c6fe79394 | 67 | float ReferencePosition_y = L1 + TowerHeight - StampHeight; |
GerhardBerman | 19:cba54636bd62 | 68 | float ReferencePosition_xnew = 0; |
GerhardBerman | 19:cba54636bd62 | 69 | float ReferencePosition_ynew = 0; |
GerhardBerman | 19:cba54636bd62 | 70 | float Position_x = 0.0; |
GerhardBerman | 19:cba54636bd62 | 71 | float Position_y = 0.0; |
GerhardBerman | 19:cba54636bd62 | 72 | float q1 = 0; |
GerhardBerman | 19:cba54636bd62 | 73 | float q2 = 0; |
GerhardBerman | 19:cba54636bd62 | 74 | float q1_ref = 0; |
GerhardBerman | 19:cba54636bd62 | 75 | float q2_ref = 0; |
GerhardBerman | 19:cba54636bd62 | 76 | float q1start = 0; |
GerhardBerman | 19:cba54636bd62 | 77 | float q2start = PI/2; |
GerhardBerman | 19:cba54636bd62 | 78 | |
GerhardBerman | 19:cba54636bd62 | 79 | float q1_error_prev = 0; |
GerhardBerman | 19:cba54636bd62 | 80 | float q2_error_prev = 0; |
GerhardBerman | 31:880ebd2e8145 | 81 | float TotalError1 = 0; |
GerhardBerman | 31:880ebd2e8145 | 82 | float TotalError2 = 0; |
GerhardBerman | 31:880ebd2e8145 | 83 | float TotalErrorMin= 0; |
GerhardBerman | 31:880ebd2e8145 | 84 | |
GerhardBerman | 25:596255732b65 | 85 | float DerTotalError1 = 0; |
GerhardBerman | 25:596255732b65 | 86 | float DerTotalError2 = 0; |
GerhardBerman | 19:cba54636bd62 | 87 | float q1IntError = 0; |
GerhardBerman | 19:cba54636bd62 | 88 | float q2IntError = 0; |
GerhardBerman | 29:caf3dd699617 | 89 | float TotalError1_prev = 0; |
GerhardBerman | 29:caf3dd699617 | 90 | float TotalError2_prev = 0; |
GerhardBerman | 19:cba54636bd62 | 91 | |
GerhardBerman | 19:cba54636bd62 | 92 | float motorValue1 = 0.0; |
GerhardBerman | 19:cba54636bd62 | 93 | float motorValue2 = 0.0; |
GerhardBerman | 19:cba54636bd62 | 94 | int counts1 = 0; |
GerhardBerman | 19:cba54636bd62 | 95 | int counts2 = 0; |
GerhardBerman | 19:cba54636bd62 | 96 | int counts1Prev = 0; |
GerhardBerman | 19:cba54636bd62 | 97 | int counts2Prev = 0; |
GerhardBerman | 26:bb7e14f59ddd | 98 | |
GerhardBerman | 31:880ebd2e8145 | 99 | int errorSwitch = 0; |
GerhardBerman | 26:bb7e14f59ddd | 100 | |
GerhardBerman | 26:bb7e14f59ddd | 101 | //set constant or variable values (VALUES HAVE TO BE EDITED) |
GerhardBerman | 31:880ebd2e8145 | 102 | const float EMGgain = 1.0; |
GerhardBerman | 31:880ebd2e8145 | 103 | //float dy_stampdown = L1 + TowerHeight - StampHeight; //0.05; //5 cm movement downward to stamp |
GerhardBerman | 31:880ebd2e8145 | 104 | const float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1) |
GerhardBerman | 31:880ebd2e8145 | 105 | const float MotorSpeedGain = 30; //MotorValue is divided by MotorSpeedGain to give the motor higher speeds |
GerhardBerman | 31:880ebd2e8145 | 106 | const float t_sample = 0.01; //seconds |
GerhardBerman | 26:bb7e14f59ddd | 107 | const float maxStampDistance = 1.5; |
GerhardBerman | 31:880ebd2e8145 | 108 | const float EMGThreshold = 0.0; |
GerhardBerman | 26:bb7e14f59ddd | 109 | |
GerhardBerman | 26:bb7e14f59ddd | 110 | float q1_refOutNew = 0; |
GerhardBerman | 26:bb7e14f59ddd | 111 | float q1_refOutMin = 0; //WRONG values |
GerhardBerman | 26:bb7e14f59ddd | 112 | float q1_refOutMax = PI; //WRONG values |
GerhardBerman | 26:bb7e14f59ddd | 113 | float q2_refOutNew = 0; |
GerhardBerman | 26:bb7e14f59ddd | 114 | float q2_refOutMin = 0; //WRONG values |
GerhardBerman | 26:bb7e14f59ddd | 115 | float q2_refOutMax = PI; //WRONG values |
GerhardBerman | 19:cba54636bd62 | 116 | |
GerhardBerman | 0:43160ef59f9f | 117 | //set BiQuad |
GerhardBerman | 0:43160ef59f9f | 118 | BiQuadChain bqc; |
GerhardBerman | 0:43160ef59f9f | 119 | BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB |
GerhardBerman | 0:43160ef59f9f | 120 | BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762); |
GerhardBerman | 0:43160ef59f9f | 121 | |
GerhardBerman | 0:43160ef59f9f | 122 | //set go-Ticker settings |
GerhardBerman | 3:8caef4872b0c | 123 | volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false; |
GerhardBerman | 3:8caef4872b0c | 124 | void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags |
GerhardBerman | 3:8caef4872b0c | 125 | void BiQuadTicker_act(){BiQuadTicker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 126 | void FeedbackTicker_act(){FeedbackTicker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 127 | void TimeTracker_act(){TimeTracker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 128 | //void sampleT_act(){sampleT_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 129 | |
GerhardBerman | 3:8caef4872b0c | 130 | //define encoder counts and degrees |
GerhardBerman | 7:2f74dfd1d411 | 131 | QEI Encoder1(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 15:9061cf7db23e | 132 | QEI Encoder2(D10, D11, NC, 32); // turns on encoder |
GerhardBerman | 14:725a608b6709 | 133 | |
GerhardBerman | 4:19e376d31380 | 134 | const int counts_per_revolution = 4200; //counts per motor axis revolution |
GerhardBerman | 4:19e376d31380 | 135 | const int inverse_gear_ratio = 131; |
GerhardBerman | 4:19e376d31380 | 136 | const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis |
GerhardBerman | 3:8caef4872b0c | 137 | |
GerhardBerman | 19:cba54636bd62 | 138 | void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){ |
GerhardBerman | 7:2f74dfd1d411 | 139 | |
GerhardBerman | 19:cba54636bd62 | 140 | //get joint positions q feedback from encoder |
GerhardBerman | 19:cba54636bd62 | 141 | float Encoder1Position = counts1/resolution; //angular position in radians, encoder axis |
GerhardBerman | 9:e4c34f5665a0 | 142 | float Encoder2Position = counts2/resolution; |
GerhardBerman | 19:cba54636bd62 | 143 | |
GerhardBerman | 19:cba54636bd62 | 144 | q1Out = q1start + Encoder1Position*inverse_gear_ratio; //angular position in radians, motor axis |
GerhardBerman | 19:cba54636bd62 | 145 | q2Out = q2start + Encoder2Position*inverse_gear_ratio; |
GerhardBerman | 7:2f74dfd1d411 | 146 | |
GerhardBerman | 19:cba54636bd62 | 147 | /* |
GerhardBerman | 19:cba54636bd62 | 148 | //get end effector position feedback with Brockett |
GerhardBerman | 18:d2cfd07ae88a | 149 | 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 | 150 | 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 | 151 | */ |
GerhardBerman | 19:cba54636bd62 | 152 | //get end effector position feedback with trigonometry |
GerhardBerman | 19:cba54636bd62 | 153 | Position_x = (L1*sin(q1) + L2*sin(q1+q2)); |
GerhardBerman | 28:6d8c6fe79394 | 154 | Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight; |
GerhardBerman | 19:cba54636bd62 | 155 | //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2)); |
GerhardBerman | 29:caf3dd699617 | 156 | |
GerhardBerman | 29:caf3dd699617 | 157 | if (Position_y < (0.5*TowerHeight)){ |
GerhardBerman | 29:caf3dd699617 | 158 | wait(1.0); |
GerhardBerman | 29:caf3dd699617 | 159 | ReferencePosition_ynew = L1 + TowerHeight - StampHeight; //Reset vertical position after stamping |
GerhardBerman | 29:caf3dd699617 | 160 | } |
GerhardBerman | 31:880ebd2e8145 | 161 | else{ //move as reaction on EMG signals |
GerhardBerman | 7:2f74dfd1d411 | 162 | //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG |
GerhardBerman | 19:cba54636bd62 | 163 | biceps_l = !button1.read() * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain |
GerhardBerman | 19:cba54636bd62 | 164 | biceps_r = !button2.read() * EMGgain; //emg1.read(); |
GerhardBerman | 31:880ebd2e8145 | 165 | if (biceps_1 > EMGThreshold || biceps_r > EMGThreshold){ //if new movement is initiated |
GerhardBerman | 31:880ebd2e8145 | 166 | errorSwitch = 0; |
GerhardBerman | 31:880ebd2e8145 | 167 | } |
GerhardBerman | 31:880ebd2e8145 | 168 | else { |
GerhardBerman | 31:880ebd2e8145 | 169 | errorSwitch = errorSwitch; |
GerhardBerman | 31:880ebd2e8145 | 170 | } |
GerhardBerman | 31:880ebd2e8145 | 171 | if (biceps_l > EMGThreshold && biceps_r > EMGThreshold){ |
GerhardBerman | 8:935abf8ecc27 | 172 | //both arms activated: stamp moves down |
GerhardBerman | 15:9061cf7db23e | 173 | //led1 = 1; |
GerhardBerman | 15:9061cf7db23e | 174 | //led2 = 1; |
GerhardBerman | 31:880ebd2e8145 | 175 | float dy_stampdown = Position_y; //Position_y movement downward to stamp |
GerhardBerman | 19:cba54636bd62 | 176 | ReferencePosition_xnew = ReferencePosition_x; |
GerhardBerman | 19:cba54636bd62 | 177 | ReferencePosition_ynew = ReferencePosition_y - dy_stampdown; //into stamping vertical position ~the stamp down action |
GerhardBerman | 18:d2cfd07ae88a | 178 | |
GerhardBerman | 18:d2cfd07ae88a | 179 | } |
GerhardBerman | 31:880ebd2e8145 | 180 | else if (biceps_l > EMGThreshold && biceps_r <= EMGThreshold){ |
GerhardBerman | 18:d2cfd07ae88a | 181 | //arm 1 activated, move left |
GerhardBerman | 18:d2cfd07ae88a | 182 | //led1 = 1; |
GerhardBerman | 18:d2cfd07ae88a | 183 | //led2 = 0; |
GerhardBerman | 19:cba54636bd62 | 184 | ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l; |
GerhardBerman | 19:cba54636bd62 | 185 | ReferencePosition_ynew = ReferencePosition_y; |
GerhardBerman | 12:05e5964b69a4 | 186 | /* |
GerhardBerman | 18:d2cfd07ae88a | 187 | PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy |
GerhardBerman | 18:d2cfd07ae88a | 188 | PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy |
GerhardBerman | 18:d2cfd07ae88a | 189 | |
GerhardBerman | 18:d2cfd07ae88a | 190 | dx = PositionError_x; |
GerhardBerman | 18:d2cfd07ae88a | 191 | dy = PositionError_y; |
GerhardBerman | 12:05e5964b69a4 | 192 | 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 | 193 | 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 | 194 | */ |
GerhardBerman | 8:935abf8ecc27 | 195 | } |
GerhardBerman | 31:880ebd2e8145 | 196 | else if (biceps_l <= EMGThreshold && biceps_r > EMGThreshold){ |
GerhardBerman | 18:d2cfd07ae88a | 197 | //arm 1 activated, move right |
GerhardBerman | 18:d2cfd07ae88a | 198 | //led1 = 0; |
GerhardBerman | 18:d2cfd07ae88a | 199 | //led2 = 1; |
GerhardBerman | 19:cba54636bd62 | 200 | ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r; |
GerhardBerman | 19:cba54636bd62 | 201 | ReferencePosition_ynew = ReferencePosition_y; |
GerhardBerman | 18:d2cfd07ae88a | 202 | /*PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy |
GerhardBerman | 18:d2cfd07ae88a | 203 | PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy |
GerhardBerman | 18:d2cfd07ae88a | 204 | |
GerhardBerman | 18:d2cfd07ae88a | 205 | dx = PositionError_x; |
GerhardBerman | 18:d2cfd07ae88a | 206 | dy = PositionError_y; |
GerhardBerman | 12:05e5964b69a4 | 207 | 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 | 208 | 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 | 209 | */ |
GerhardBerman | 8:935abf8ecc27 | 210 | } |
GerhardBerman | 8:935abf8ecc27 | 211 | else{ |
GerhardBerman | 15:9061cf7db23e | 212 | //led1 = 0; |
GerhardBerman | 15:9061cf7db23e | 213 | //led2 = 0; |
GerhardBerman | 19:cba54636bd62 | 214 | ReferencePosition_xnew = ReferencePosition_x; |
GerhardBerman | 19:cba54636bd62 | 215 | ReferencePosition_ynew = ReferencePosition_y; |
GerhardBerman | 19:cba54636bd62 | 216 | } |
GerhardBerman | 29:caf3dd699617 | 217 | } |
GerhardBerman | 19:cba54636bd62 | 218 | |
GerhardBerman | 28:6d8c6fe79394 | 219 | float PointPositionArm2_x = ReferencePosition_x; |
GerhardBerman | 28:6d8c6fe79394 | 220 | float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight; |
GerhardBerman | 28:6d8c6fe79394 | 221 | float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2)); |
GerhardBerman | 19:cba54636bd62 | 222 | |
GerhardBerman | 19:cba54636bd62 | 223 | //check position boundaries |
GerhardBerman | 28:6d8c6fe79394 | 224 | if (PointVectorArm2 > maxStampDistance){ |
GerhardBerman | 18:d2cfd07ae88a | 225 | ReferencePosition_x = ReferencePosition_x; |
GerhardBerman | 18:d2cfd07ae88a | 226 | ReferencePosition_y = ReferencePosition_y; |
GerhardBerman | 8:935abf8ecc27 | 227 | } |
GerhardBerman | 19:cba54636bd62 | 228 | else if (ReferencePosition_ynew < 0){ |
GerhardBerman | 19:cba54636bd62 | 229 | ReferencePosition_y = 0; //could also be little negative value to get more pressure on table |
GerhardBerman | 19:cba54636bd62 | 230 | } |
GerhardBerman | 19:cba54636bd62 | 231 | else { |
GerhardBerman | 19:cba54636bd62 | 232 | ReferencePosition_x = ReferencePosition_xnew; |
GerhardBerman | 19:cba54636bd62 | 233 | ReferencePosition_y = ReferencePosition_ynew; |
11i | 20:201175fa8a2a | 234 | } |
GerhardBerman | 19:cba54636bd62 | 235 | |
GerhardBerman | 19:cba54636bd62 | 236 | //calculate reference joint angles for the new reference position |
GerhardBerman | 28:6d8c6fe79394 | 237 | float alpha = atan(PointPositionArm2_y/PointPositionArm2_x); |
GerhardBerman | 28:6d8c6fe79394 | 238 | float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2)); |
GerhardBerman | 19:cba54636bd62 | 239 | q1_refOutNew = PI/2 - (alpha+beta); |
GerhardBerman | 28:6d8c6fe79394 | 240 | q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2); |
GerhardBerman | 19:cba54636bd62 | 241 | |
GerhardBerman | 19:cba54636bd62 | 242 | //check angle boundaries |
GerhardBerman | 27:456e8a47f815 | 243 | if (q1_refOutNew > q1_refOutMin && q1_refOutNew < q1_refOutMax){ |
GerhardBerman | 19:cba54636bd62 | 244 | q1_refOut = q1_refOutNew; |
GerhardBerman | 19:cba54636bd62 | 245 | } |
GerhardBerman | 19:cba54636bd62 | 246 | else { |
GerhardBerman | 19:cba54636bd62 | 247 | q1_refOut = q1_refOut; |
GerhardBerman | 19:cba54636bd62 | 248 | } |
GerhardBerman | 27:456e8a47f815 | 249 | if (q2_refOutNew > q2_refOutMin && q2_refOutNew < q2_refOutMax){ |
GerhardBerman | 19:cba54636bd62 | 250 | q2_refOut = q2_refOutNew; |
GerhardBerman | 19:cba54636bd62 | 251 | } |
GerhardBerman | 19:cba54636bd62 | 252 | else { |
GerhardBerman | 19:cba54636bd62 | 253 | q2_refOut = q2_refOut; |
GerhardBerman | 19:cba54636bd62 | 254 | } |
GerhardBerman | 18:d2cfd07ae88a | 255 | |
GerhardBerman | 12:05e5964b69a4 | 256 | |
GerhardBerman | 7:2f74dfd1d411 | 257 | //update joint angles |
GerhardBerman | 18:d2cfd07ae88a | 258 | //q1Out = q1Out + q1_dotOut; //in radians |
GerhardBerman | 18:d2cfd07ae88a | 259 | //q2Out = q2Out + q2_dotOut; |
GerhardBerman | 12:05e5964b69a4 | 260 | |
GerhardBerman | 12:05e5964b69a4 | 261 | pc.baud(115200); |
GerhardBerman | 31:880ebd2e8145 | 262 | |
GerhardBerman | 31:880ebd2e8145 | 263 | //pc.printf("biceps_l: %f \r\n", biceps_l); |
GerhardBerman | 31:880ebd2e8145 | 264 | //pc.printf("biceps_r: %f \r\n", biceps_r); |
GerhardBerman | 31:880ebd2e8145 | 265 | //pc.printf("Position_x: %f m\r\n", Position_x); |
GerhardBerman | 31:880ebd2e8145 | 266 | //pc.printf("q1Out: %f rad ", q1Out); |
GerhardBerman | 31:880ebd2e8145 | 267 | //pc.printf("Reference_x: %f m \r\n", ReferencePosition_x); |
GerhardBerman | 31:880ebd2e8145 | 268 | //pc.printf(" q1ref: %f rad ", q1_refOut); |
GerhardBerman | 31:880ebd2e8145 | 269 | //pc.printf("Position_y: %f m\r\n", Position_y); |
GerhardBerman | 31:880ebd2e8145 | 270 | //pc.printf(" q2Out: %f rad ", q2Out); |
GerhardBerman | 31:880ebd2e8145 | 271 | //pc.printf("Reference_y: %f m \r\n", ReferencePosition_y); |
GerhardBerman | 31:880ebd2e8145 | 272 | //pc.printf(" q2ref: %f rad ", q2_refOut); |
GerhardBerman | 31:880ebd2e8145 | 273 | //pc.printf("alpha: %f rad \r\n", alpha); |
GerhardBerman | 31:880ebd2e8145 | 274 | //pc.printf("beta: %f rad\r\n", beta); |
GerhardBerman | 18:d2cfd07ae88a | 275 | |
GerhardBerman | 19:cba54636bd62 | 276 | |
GerhardBerman | 18:d2cfd07ae88a | 277 | /* |
GerhardBerman | 12:05e5964b69a4 | 278 | pc.printf("dx: %f \r\n", dx); |
GerhardBerman | 12:05e5964b69a4 | 279 | pc.printf("dy: %f \r\n", dy); |
GerhardBerman | 12:05e5964b69a4 | 280 | pc.printf("q1: %f \r\n", q1Out); |
GerhardBerman | 12:05e5964b69a4 | 281 | pc.printf("q1_dot: %f \r\n", q1_dotOut); |
GerhardBerman | 12:05e5964b69a4 | 282 | pc.printf("q2: %f \r\n", q2Out); |
GerhardBerman | 12:05e5964b69a4 | 283 | pc.printf("q2_dot: %f \r\n", q2_dotOut); |
GerhardBerman | 12:05e5964b69a4 | 284 | |
GerhardBerman | 14:725a608b6709 | 285 | pc.printf("Counts1: %f \r\n", counts1); |
GerhardBerman | 14:725a608b6709 | 286 | pc.printf("Encoder1: %f \r\n", Encoder1Position); |
GerhardBerman | 18:d2cfd07ae88a | 287 | pc.printf("Motor1: %f \r\n", q1Out); |
GerhardBerman | 14:725a608b6709 | 288 | pc.printf("Counts2: %f \r\n", counts2); |
GerhardBerman | 14:725a608b6709 | 289 | pc.printf("Encoder2: %f \r\n", Encoder2Position); |
GerhardBerman | 18:d2cfd07ae88a | 290 | pc.printf("Motor2: %f \r\n", q2Out); |
GerhardBerman | 18:d2cfd07ae88a | 291 | */ |
GerhardBerman | 29:caf3dd699617 | 292 | |
GerhardBerman | 7:2f74dfd1d411 | 293 | } |
GerhardBerman | 18:d2cfd07ae88a | 294 | |
GerhardBerman | 7:2f74dfd1d411 | 295 | |
GerhardBerman | 24:393da1cc1fa8 | 296 | void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){ |
GerhardBerman | 7:2f74dfd1d411 | 297 | |
GerhardBerman | 7:2f74dfd1d411 | 298 | // linear feedback control |
GerhardBerman | 19:cba54636bd62 | 299 | float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians |
GerhardBerman | 19:cba54636bd62 | 300 | float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians |
GerhardBerman | 31:880ebd2e8145 | 301 | float Kp = 1.0; //potMeter2.read(); |
GerhardBerman | 31:880ebd2e8145 | 302 | |
GerhardBerman | 19:cba54636bd62 | 303 | float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians |
GerhardBerman | 19:cba54636bd62 | 304 | float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians |
GerhardBerman | 7:2f74dfd1d411 | 305 | //float maxKi = 0.2; |
GerhardBerman | 7:2f74dfd1d411 | 306 | float Ki = 0.1; //potMeter2.read(); |
GerhardBerman | 7:2f74dfd1d411 | 307 | |
GerhardBerman | 27:456e8a47f815 | 308 | float q1DerivativeError = (q1_error - q1_error_prev)/t_sample; // derivative of error in radians |
GerhardBerman | 19:cba54636bd62 | 309 | float q2DerivativeError = (q2_error_prev + q2_error)/t_sample; // derivative of error in radians |
GerhardBerman | 7:2f74dfd1d411 | 310 | //float maxKd = 0.2; |
GerhardBerman | 7:2f74dfd1d411 | 311 | float Kd = 0.0; //potMeter2.read(); |
GerhardBerman | 7:2f74dfd1d411 | 312 | |
GerhardBerman | 23:3a4d5e19c20d | 313 | TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output in radians = motor input |
GerhardBerman | 23:3a4d5e19c20d | 314 | TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output in radians = motor input |
11i | 21:dd51d78c0732 | 315 | |
GerhardBerman | 31:880ebd2e8145 | 316 | |
GerhardBerman | 31:880ebd2e8145 | 317 | if (TotalError1 < 0 && errorSwitch <= 0) { |
11i | 21:dd51d78c0732 | 318 | TotalError1=0; |
GerhardBerman | 31:880ebd2e8145 | 319 | errorSwitch = -1; |
11i | 21:dd51d78c0732 | 320 | } |
11i | 21:dd51d78c0732 | 321 | else { |
11i | 21:dd51d78c0732 | 322 | TotalError1=TotalError1; |
11i | 21:dd51d78c0732 | 323 | } |
GerhardBerman | 31:880ebd2e8145 | 324 | if (TotalError1 >= 0 && errorSwitch >= 0) { |
GerhardBerman | 31:880ebd2e8145 | 325 | TotalError1=TotalError1; |
GerhardBerman | 31:880ebd2e8145 | 326 | errorSwitch = 1; |
11i | 21:dd51d78c0732 | 327 | } |
11i | 21:dd51d78c0732 | 328 | else { |
11i | 21:dd51d78c0732 | 329 | TotalError2=TotalError2; |
11i | 21:dd51d78c0732 | 330 | } |
11i | 21:dd51d78c0732 | 331 | |
GerhardBerman | 25:596255732b65 | 332 | /* |
GerhardBerman | 25:596255732b65 | 333 | DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample; |
GerhardBerman | 25:596255732b65 | 334 | DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample; |
GerhardBerman | 31:880ebd2e8145 | 335 | float motorValue1Small = DerTotalError1/MotorGain; |
GerhardBerman | 31:880ebd2e8145 | 336 | float motorValue2Small = DerTotalError2/MotorGain; |
GerhardBerman | 25:596255732b65 | 337 | */ |
GerhardBerman | 25:596255732b65 | 338 | |
GerhardBerman | 31:880ebd2e8145 | 339 | float PseudoDerError1 = TotalError1/t_sample; |
GerhardBerman | 31:880ebd2e8145 | 340 | float PseudoDerError2 = TotalError2/t_sample; |
GerhardBerman | 31:880ebd2e8145 | 341 | float motorValue1Small = PseudoDerError1/MotorGain; //~motorValue1Small = TotalError1*11.90 |
GerhardBerman | 31:880ebd2e8145 | 342 | float motorValue2Small = PseudoDerError2/MotorGain; |
GerhardBerman | 31:880ebd2e8145 | 343 | /* |
GerhardBerman | 30:676ecd59467a | 344 | float motorValue1Small = TotalError1/MotorGain; |
GerhardBerman | 30:676ecd59467a | 345 | float motorValue2Small = TotalError2/MotorGain; |
GerhardBerman | 31:880ebd2e8145 | 346 | */ |
GerhardBerman | 31:880ebd2e8145 | 347 | |
GerhardBerman | 31:880ebd2e8145 | 348 | motorValue1Out = motorValue1Small/MotorSpeedGain; |
GerhardBerman | 31:880ebd2e8145 | 349 | motorValue2Out = motorValue2Small/MotorSpeedGain; |
11i | 21:dd51d78c0732 | 350 | |
GerhardBerman | 29:caf3dd699617 | 351 | TotalError1_prev = TotalError1; |
GerhardBerman | 29:caf3dd699617 | 352 | TotalError2_prev = TotalError2; |
GerhardBerman | 29:caf3dd699617 | 353 | |
GerhardBerman | 31:880ebd2e8145 | 354 | /* |
GerhardBerman | 29:caf3dd699617 | 355 | scope.set(0,q1_ref); |
GerhardBerman | 29:caf3dd699617 | 356 | scope.set(1,q1); |
GerhardBerman | 29:caf3dd699617 | 357 | scope.set(2,q2_ref); |
GerhardBerman | 29:caf3dd699617 | 358 | scope.set(3,q2); |
GerhardBerman | 29:caf3dd699617 | 359 | scope.set(4,motorValue1Out); |
GerhardBerman | 29:caf3dd699617 | 360 | scope.set(5,motorValue2Out); |
GerhardBerman | 29:caf3dd699617 | 361 | scope.send(); |
GerhardBerman | 31:880ebd2e8145 | 362 | */ |
GerhardBerman | 31:880ebd2e8145 | 363 | //pc.printf(" error1: %f ", q1_error ); |
GerhardBerman | 31:880ebd2e8145 | 364 | //pc.printf("IntError1: %f \r\n", q1IntError); |
GerhardBerman | 31:880ebd2e8145 | 365 | //pc.printf("DerError1: %f \r\n", q1DerivativeError); |
GerhardBerman | 31:880ebd2e8145 | 366 | //pc.printf(" error2: %f ", q2_error); |
GerhardBerman | 31:880ebd2e8145 | 367 | //pc.printf("IntError2: %f \r\n", q2IntError); |
GerhardBerman | 31:880ebd2e8145 | 368 | //pc.printf("DerError2: %f \r\n", q2DerivativeError); |
GerhardBerman | 31:880ebd2e8145 | 369 | pc.printf("E1 %f ", TotalError1); |
GerhardBerman | 31:880ebd2e8145 | 370 | pc.printf("E2 %f ", TotalError2); |
GerhardBerman | 31:880ebd2e8145 | 371 | pc.printf("M1 %f ", motorValue1Out); |
GerhardBerman | 31:880ebd2e8145 | 372 | pc.printf("M2 %f\r\n", motorValue2Out); |
GerhardBerman | 25:596255732b65 | 373 | /* |
GerhardBerman | 19:cba54636bd62 | 374 | q1_error_prev = q1_error; |
GerhardBerman | 19:cba54636bd62 | 375 | q2_error_prev = q2_error; |
GerhardBerman | 25:596255732b65 | 376 | TotalError1_prev = TotalError1; |
GerhardBerman | 25:596255732b65 | 377 | TotalError1_prev = TotalError1; |
GerhardBerman | 25:596255732b65 | 378 | */ |
GerhardBerman | 25:596255732b65 | 379 | |
GerhardBerman | 12:05e5964b69a4 | 380 | /* |
GerhardBerman | 12:05e5964b69a4 | 381 | scope.set(0,q1); |
GerhardBerman | 12:05e5964b69a4 | 382 | scope.set(1,q2); |
GerhardBerman | 18:d2cfd07ae88a | 383 | scope.set(2,biceps_l); |
GerhardBerman | 18:d2cfd07ae88a | 384 | scope.set(3,biceps_r); |
GerhardBerman | 11:773b3532d50f | 385 | scope.send(); |
11i | 20:201175fa8a2a | 386 | */ |
GerhardBerman | 3:8caef4872b0c | 387 | } |
GerhardBerman | 3:8caef4872b0c | 388 | |
GerhardBerman | 9:e4c34f5665a0 | 389 | void SetMotor1(float motorValue1, float motorValue2) |
GerhardBerman | 3:8caef4872b0c | 390 | { |
GerhardBerman | 3:8caef4872b0c | 391 | // Given -1<=motorValue<=1, this sets the PWM and direction |
GerhardBerman | 3:8caef4872b0c | 392 | // bits for motor 1. Positive value makes motor rotating |
GerhardBerman | 3:8caef4872b0c | 393 | // clockwise. motorValues outside range are truncated to |
GerhardBerman | 3:8caef4872b0c | 394 | // within range |
GerhardBerman | 9:e4c34f5665a0 | 395 | //control motor 1 |
GerhardBerman | 15:9061cf7db23e | 396 | if (motorValue1 >=0) //clockwise rotation |
GerhardBerman | 19:cba54636bd62 | 397 | {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower |
GerhardBerman | 9:e4c34f5665a0 | 398 | //led1=1; |
GerhardBerman | 9:e4c34f5665a0 | 399 | //led2=0; |
GerhardBerman | 3:8caef4872b0c | 400 | } |
GerhardBerman | 15:9061cf7db23e | 401 | else //counterclockwise rotation |
GerhardBerman | 19:cba54636bd62 | 402 | {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower |
GerhardBerman | 9:e4c34f5665a0 | 403 | //led1=0; |
GerhardBerman | 9:e4c34f5665a0 | 404 | //led2=1; |
GerhardBerman | 3:8caef4872b0c | 405 | } |
GerhardBerman | 29:caf3dd699617 | 406 | if (fabs(motorValue1)>1){ |
GerhardBerman | 29:caf3dd699617 | 407 | motor1MagnitudePin = 1; |
GerhardBerman | 29:caf3dd699617 | 408 | } |
GerhardBerman | 29:caf3dd699617 | 409 | else{ |
GerhardBerman | 29:caf3dd699617 | 410 | motor1MagnitudePin = fabs(motorValue1); //fabs(motorValue1); |
GerhardBerman | 29:caf3dd699617 | 411 | } |
GerhardBerman | 29:caf3dd699617 | 412 | |
GerhardBerman | 9:e4c34f5665a0 | 413 | //control motor 2 |
GerhardBerman | 15:9061cf7db23e | 414 | if (motorValue2 >=0) //clockwise rotation |
GerhardBerman | 14:725a608b6709 | 415 | {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted) |
GerhardBerman | 10:45473f650198 | 416 | //led1=1; |
GerhardBerman | 10:45473f650198 | 417 | //led2=0; |
GerhardBerman | 7:2f74dfd1d411 | 418 | } |
GerhardBerman | 15:9061cf7db23e | 419 | else //counterclockwise rotation |
GerhardBerman | 15:9061cf7db23e | 420 | {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted) |
GerhardBerman | 10:45473f650198 | 421 | //led1=0; |
GerhardBerman | 10:45473f650198 | 422 | //led2=1; |
GerhardBerman | 7:2f74dfd1d411 | 423 | } |
GerhardBerman | 29:caf3dd699617 | 424 | if (fabs(motorValue2)>1){ |
GerhardBerman | 29:caf3dd699617 | 425 | motor2MagnitudePin = 1; |
GerhardBerman | 29:caf3dd699617 | 426 | } |
GerhardBerman | 29:caf3dd699617 | 427 | else{ |
GerhardBerman | 29:caf3dd699617 | 428 | motor2MagnitudePin = fabs(motorValue2); |
GerhardBerman | 29:caf3dd699617 | 429 | } |
GerhardBerman | 19:cba54636bd62 | 430 | float ReadMagn1 = motor1MagnitudePin.read(); |
GerhardBerman | 19:cba54636bd62 | 431 | float ReadMagn2 = motor2MagnitudePin.read(); |
GerhardBerman | 31:880ebd2e8145 | 432 | //pc.printf("motor1Magnitude: %f \r\n", ReadMagn1); |
GerhardBerman | 31:880ebd2e8145 | 433 | //pc.printf("motor2Magnitude: %f \r\n", ReadMagn2); |
GerhardBerman | 3:8caef4872b0c | 434 | } |
GerhardBerman | 3:8caef4872b0c | 435 | |
GerhardBerman | 3:8caef4872b0c | 436 | void MeasureAndControl() |
GerhardBerman | 3:8caef4872b0c | 437 | { |
GerhardBerman | 9:e4c34f5665a0 | 438 | // This function measures the EMG of both arms, calculates via IK what |
GerhardBerman | 24:393da1cc1fa8 | 439 | // the joint positions should be, and controls the motor with |
GerhardBerman | 24:393da1cc1fa8 | 440 | // a Feedback controller. This is called from a Ticker. |
GerhardBerman | 19:cba54636bd62 | 441 | GetReferenceKinematics1(q1, q2, q1_ref, q2_ref); |
GerhardBerman | 24:393da1cc1fa8 | 442 | FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2); |
GerhardBerman | 9:e4c34f5665a0 | 443 | SetMotor1(motorValue1, motorValue2); |
GerhardBerman | 3:8caef4872b0c | 444 | } |
GerhardBerman | 3:8caef4872b0c | 445 | |
GerhardBerman | 3:8caef4872b0c | 446 | void TimeTrackerF(){ |
GerhardBerman | 3:8caef4872b0c | 447 | //wait(1); |
GerhardBerman | 3:8caef4872b0c | 448 | //float Potmeter1 = potMeter1.read(); |
GerhardBerman | 7:2f74dfd1d411 | 449 | //float referencePosition1 = GetReferencePosition(); |
GerhardBerman | 7:2f74dfd1d411 | 450 | //pc.printf("TTReference Position: %d rad \r\n", referencePosition1); |
GerhardBerman | 3:8caef4872b0c | 451 | //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1); |
GerhardBerman | 3:8caef4872b0c | 452 | //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2); |
GerhardBerman | 7:2f74dfd1d411 | 453 | //pc.printf("TTCounts: %i \r\n", counts1); |
GerhardBerman | 3:8caef4872b0c | 454 | } |
GerhardBerman | 7:2f74dfd1d411 | 455 | |
GerhardBerman | 3:8caef4872b0c | 456 | /* |
GerhardBerman | 3:8caef4872b0c | 457 | void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts |
GerhardBerman | 3:8caef4872b0c | 458 | //double in=DerivativeCounts(); |
GerhardBerman | 3:8caef4872b0c | 459 | bqcDerivativeCounts=bqc.step(DerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 460 | //return(bqcDerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 461 | } |
GerhardBerman | 9:e4c34f5665a0 | 462 | */ |
GerhardBerman | 6:3c4f3f2ce54f | 463 | |
GerhardBerman | 0:43160ef59f9f | 464 | int main() |
GerhardBerman | 0:43160ef59f9f | 465 | { |
GerhardBerman | 0:43160ef59f9f | 466 | //Initialize |
GerhardBerman | 15:9061cf7db23e | 467 | //int led1val = led1.read(); |
GerhardBerman | 15:9061cf7db23e | 468 | //int led2val = led2.read(); |
GerhardBerman | 9:e4c34f5665a0 | 469 | pc.baud(115200); |
GerhardBerman | 31:880ebd2e8145 | 470 | //pc.printf("Test putty IK"); |
GerhardBerman | 31:880ebd2e8145 | 471 | float TickerFreq = 1/t_sample; //0.01f |
GerhardBerman | 31:880ebd2e8145 | 472 | MeasureTicker.attach(&MeasureTicker_act, 0.01f); |
GerhardBerman | 10:45473f650198 | 473 | bqc.add(&bq1).add(&bq2); |
GerhardBerman | 10:45473f650198 | 474 | |
GerhardBerman | 0:43160ef59f9f | 475 | while(1) |
GerhardBerman | 0:43160ef59f9f | 476 | { |
GerhardBerman | 3:8caef4872b0c | 477 | if (MeasureTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 478 | MeasureTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 479 | MeasureAndControl(); |
GerhardBerman | 15:9061cf7db23e | 480 | counts1 = Encoder1.getPulses(); // gives position of encoder |
GerhardBerman | 15:9061cf7db23e | 481 | counts2 = Encoder2.getPulses(); // gives position of encoder |
GerhardBerman | 31:880ebd2e8145 | 482 | //pc.printf("counts1: %i \r\n", counts1); |
GerhardBerman | 31:880ebd2e8145 | 483 | //pc.printf("counts2: %i \r\n", counts2); |
GerhardBerman | 3:8caef4872b0c | 484 | } |
GerhardBerman | 10:45473f650198 | 485 | /* |
GerhardBerman | 3:8caef4872b0c | 486 | if (BiQuadTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 487 | BiQuadTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 488 | BiQuadFilter(); |
GerhardBerman | 3:8caef4872b0c | 489 | } |
GerhardBerman | 10:45473f650198 | 490 | */ |
GerhardBerman | 0:43160ef59f9f | 491 | } |
GerhardBerman | 0:43160ef59f9f | 492 | } |