First trial for Inverse Kinematics Feedforward implementation. No errors, not yet tested with board
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_pract3_3_PI_controller by
main.cpp@16:9b7651fdf5a0, 2016-10-20 (annotated)
- Committer:
- GerhardBerman
- Date:
- Thu Oct 20 13:14:03 2016 +0000
- Revision:
- 16:9b7651fdf5a0
- Parent:
- 15:9061cf7db23e
- Child:
- 17:91d20d362e72
All working, motorValues scaled down to 1/10 for first testing.
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 | 0:43160ef59f9f | 8 | //set pins |
GerhardBerman | 0:43160ef59f9f | 9 | DigitalIn encoder1A (D13); //Channel A van Encoder 1 |
GerhardBerman | 0:43160ef59f9f | 10 | DigitalIn encoder1B (D12); //Channel B van Encoder 1 |
GerhardBerman | 14:725a608b6709 | 11 | DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15 |
GerhardBerman | 14:725a608b6709 | 12 | DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14 |
GerhardBerman | 14:725a608b6709 | 13 | //DigitalOut led1 (D11); |
GerhardBerman | 14:725a608b6709 | 14 | //DigitalOut led2 (D10); |
GerhardBerman | 3:8caef4872b0c | 15 | AnalogIn potMeter1(A2); |
GerhardBerman | 0:43160ef59f9f | 16 | AnalogIn potMeter2(A1); |
GerhardBerman | 0:43160ef59f9f | 17 | DigitalOut motor1DirectionPin(D7); |
GerhardBerman | 0:43160ef59f9f | 18 | PwmOut motor1MagnitudePin(D6); |
GerhardBerman | 7:2f74dfd1d411 | 19 | DigitalOut motor2DirectionPin(D4); |
GerhardBerman | 7:2f74dfd1d411 | 20 | PwmOut motor2MagnitudePin(D5); |
GerhardBerman | 9:e4c34f5665a0 | 21 | DigitalIn button1(D3); |
GerhardBerman | 7:2f74dfd1d411 | 22 | DigitalIn button2(D9); |
GerhardBerman | 0:43160ef59f9f | 23 | |
GerhardBerman | 7:2f74dfd1d411 | 24 | //library settings |
GerhardBerman | 0:43160ef59f9f | 25 | Serial pc(USBTX,USBRX); |
GerhardBerman | 3:8caef4872b0c | 26 | Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT; |
GerhardBerman | 12:05e5964b69a4 | 27 | //HIDScope scope(4); |
GerhardBerman | 0:43160ef59f9f | 28 | |
GerhardBerman | 7:2f74dfd1d411 | 29 | //set initial conditions |
GerhardBerman | 7:2f74dfd1d411 | 30 | float error1_prev = 0; |
GerhardBerman | 7:2f74dfd1d411 | 31 | float error2_prev = 0; |
GerhardBerman | 7:2f74dfd1d411 | 32 | float IntError1 = 0; |
GerhardBerman | 7:2f74dfd1d411 | 33 | float IntError2 = 0; |
GerhardBerman | 7:2f74dfd1d411 | 34 | float q1 = 0; |
GerhardBerman | 7:2f74dfd1d411 | 35 | float q2 = 0; |
GerhardBerman | 9:e4c34f5665a0 | 36 | //set initial conditions for function references |
GerhardBerman | 9:e4c34f5665a0 | 37 | float q1_dot = 0.0; |
GerhardBerman | 9:e4c34f5665a0 | 38 | float q2_dot = 0.0; |
GerhardBerman | 9:e4c34f5665a0 | 39 | float motorValue1 = 0.0; |
GerhardBerman | 9:e4c34f5665a0 | 40 | float motorValue2 = 0.0; |
GerhardBerman | 9:e4c34f5665a0 | 41 | |
GerhardBerman | 7:2f74dfd1d411 | 42 | //set constant or variable values |
GerhardBerman | 15:9061cf7db23e | 43 | int counts1 = 0; |
GerhardBerman | 15:9061cf7db23e | 44 | int counts2 = 0; |
GerhardBerman | 7:2f74dfd1d411 | 45 | int counts1Prev = 0; |
GerhardBerman | 7:2f74dfd1d411 | 46 | int counts2Prev = 0; |
GerhardBerman | 7:2f74dfd1d411 | 47 | double DerivativeCounts; |
GerhardBerman | 7:2f74dfd1d411 | 48 | float x0 = 1.0; |
GerhardBerman | 7:2f74dfd1d411 | 49 | float L0 = 1.0; |
GerhardBerman | 7:2f74dfd1d411 | 50 | float L1 = 1.0; |
GerhardBerman | 7:2f74dfd1d411 | 51 | float dx; |
GerhardBerman | 7:2f74dfd1d411 | 52 | float dy; |
GerhardBerman | 7:2f74dfd1d411 | 53 | float dy_stampdown = 0.05; //5 cm movement downward to stamp |
GerhardBerman | 7:2f74dfd1d411 | 54 | |
GerhardBerman | 4:19e376d31380 | 55 | float t_sample = 0.01; //seconds |
GerhardBerman | 0:43160ef59f9f | 56 | float referenceVelocity = 0; |
GerhardBerman | 3:8caef4872b0c | 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 | 0:43160ef59f9f | 62 | //set BiQuad |
GerhardBerman | 0:43160ef59f9f | 63 | BiQuadChain bqc; |
GerhardBerman | 0:43160ef59f9f | 64 | BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB |
GerhardBerman | 0:43160ef59f9f | 65 | BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762); |
GerhardBerman | 0:43160ef59f9f | 66 | |
GerhardBerman | 0:43160ef59f9f | 67 | //set go-Ticker settings |
GerhardBerman | 3:8caef4872b0c | 68 | volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false; |
GerhardBerman | 3:8caef4872b0c | 69 | void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags |
GerhardBerman | 3:8caef4872b0c | 70 | void BiQuadTicker_act(){BiQuadTicker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 71 | void FeedbackTicker_act(){FeedbackTicker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 72 | void TimeTracker_act(){TimeTracker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 73 | //void sampleT_act(){sampleT_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 74 | |
GerhardBerman | 3:8caef4872b0c | 75 | //define encoder counts and degrees |
GerhardBerman | 7:2f74dfd1d411 | 76 | QEI Encoder1(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 15:9061cf7db23e | 77 | QEI Encoder2(D10, D11, NC, 32); // turns on encoder |
GerhardBerman | 14:725a608b6709 | 78 | |
GerhardBerman | 4:19e376d31380 | 79 | const int counts_per_revolution = 4200; //counts per motor axis revolution |
GerhardBerman | 4:19e376d31380 | 80 | const int inverse_gear_ratio = 131; |
GerhardBerman | 4:19e376d31380 | 81 | //const float motor_axial_resolution = counts_per_revolution/(2*PI); |
GerhardBerman | 4:19e376d31380 | 82 | const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis |
GerhardBerman | 3:8caef4872b0c | 83 | |
GerhardBerman | 12:05e5964b69a4 | 84 | void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_dotOut, float &q2_dotOut){ |
GerhardBerman | 7:2f74dfd1d411 | 85 | |
GerhardBerman | 7:2f74dfd1d411 | 86 | //get joint positions q from encoder |
GerhardBerman | 7:2f74dfd1d411 | 87 | float Encoder1Position = counts1/resolution; //position in radians, encoder axis |
GerhardBerman | 9:e4c34f5665a0 | 88 | float Encoder2Position = counts2/resolution; |
GerhardBerman | 7:2f74dfd1d411 | 89 | |
GerhardBerman | 12:05e5964b69a4 | 90 | float Motor1Position = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis |
GerhardBerman | 12:05e5964b69a4 | 91 | float Motor2Position = Encoder2Position*inverse_gear_ratio; |
GerhardBerman | 9:e4c34f5665a0 | 92 | |
GerhardBerman | 7:2f74dfd1d411 | 93 | //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG |
GerhardBerman | 11:773b3532d50f | 94 | float biceps1 = !button1.read(); |
GerhardBerman | 11:773b3532d50f | 95 | float biceps2 = !button2.read(); |
GerhardBerman | 8:935abf8ecc27 | 96 | if (biceps1 > 0 && biceps2 > 0){ |
GerhardBerman | 8:935abf8ecc27 | 97 | //both arms activated: stamp moves down |
GerhardBerman | 15:9061cf7db23e | 98 | //led1 = 1; |
GerhardBerman | 15:9061cf7db23e | 99 | //led2 = 1; |
GerhardBerman | 8:935abf8ecc27 | 100 | dx = 0; |
GerhardBerman | 12:05e5964b69a4 | 101 | dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action |
GerhardBerman | 12:05e5964b69a4 | 102 | 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 | 103 | 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 | 104 | |
GerhardBerman | 12:05e5964b69a4 | 105 | /* |
GerhardBerman | 8:935abf8ecc27 | 106 | wait(1); |
GerhardBerman | 8:935abf8ecc27 | 107 | dy = -(dy_stampdown); //reset vertical position |
GerhardBerman | 12:05e5964b69a4 | 108 | 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 | 109 | 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 | 110 | */ |
GerhardBerman | 8:935abf8ecc27 | 111 | } |
GerhardBerman | 8:935abf8ecc27 | 112 | else if (biceps1 > 0 && biceps2 <= 0){ |
GerhardBerman | 8:935abf8ecc27 | 113 | //arm 1 activated, move left |
GerhardBerman | 15:9061cf7db23e | 114 | //led1 = 1; |
GerhardBerman | 15:9061cf7db23e | 115 | //led2 = 0; |
GerhardBerman | 12:05e5964b69a4 | 116 | dx = 1; //-biceps1; |
GerhardBerman | 8:935abf8ecc27 | 117 | dy = 0; |
GerhardBerman | 12:05e5964b69a4 | 118 | 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 | 119 | 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 | 8:935abf8ecc27 | 120 | } |
GerhardBerman | 8:935abf8ecc27 | 121 | else if (biceps1 <= 0 && biceps2 > 0){ |
GerhardBerman | 8:935abf8ecc27 | 122 | //arm 1 activated, move left |
GerhardBerman | 15:9061cf7db23e | 123 | //led1 = 0; |
GerhardBerman | 15:9061cf7db23e | 124 | //led2 = 1; |
GerhardBerman | 12:05e5964b69a4 | 125 | dx = 1; //biceps2; |
GerhardBerman | 8:935abf8ecc27 | 126 | dy = 0; |
GerhardBerman | 12:05e5964b69a4 | 127 | 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 | 128 | 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 | 129 | |
GerhardBerman | 8:935abf8ecc27 | 130 | } |
GerhardBerman | 8:935abf8ecc27 | 131 | else{ |
GerhardBerman | 15:9061cf7db23e | 132 | //led1 = 0; |
GerhardBerman | 15:9061cf7db23e | 133 | //led2 = 0; |
GerhardBerman | 8:935abf8ecc27 | 134 | dx=0; |
GerhardBerman | 8:935abf8ecc27 | 135 | dy=0; |
GerhardBerman | 12:05e5964b69a4 | 136 | q1_dotOut = 0; |
GerhardBerman | 12:05e5964b69a4 | 137 | q2_dotOut = 0; |
GerhardBerman | 8:935abf8ecc27 | 138 | } |
GerhardBerman | 8:935abf8ecc27 | 139 | |
GerhardBerman | 7:2f74dfd1d411 | 140 | //get joint angles change q_dot = Jpseudo * TwistEndEff (Matlab) |
GerhardBerman | 12:05e5964b69a4 | 141 | |
GerhardBerman | 7:2f74dfd1d411 | 142 | //update joint angles |
GerhardBerman | 12:05e5964b69a4 | 143 | q1Out = q1Out + q1_dotOut; //in radians |
GerhardBerman | 12:05e5964b69a4 | 144 | q2Out = q2Out + q2_dotOut; |
GerhardBerman | 12:05e5964b69a4 | 145 | |
GerhardBerman | 12:05e5964b69a4 | 146 | pc.baud(115200); |
GerhardBerman | 12:05e5964b69a4 | 147 | pc.printf("dx: %f \r\n", dx); |
GerhardBerman | 12:05e5964b69a4 | 148 | pc.printf("dy: %f \r\n", dy); |
GerhardBerman | 12:05e5964b69a4 | 149 | pc.printf("q1: %f \r\n", q1Out); |
GerhardBerman | 12:05e5964b69a4 | 150 | pc.printf("q1_dot: %f \r\n", q1_dotOut); |
GerhardBerman | 12:05e5964b69a4 | 151 | pc.printf("q2: %f \r\n", q2Out); |
GerhardBerman | 12:05e5964b69a4 | 152 | pc.printf("q2_dot: %f \r\n", q2_dotOut); |
GerhardBerman | 12:05e5964b69a4 | 153 | |
GerhardBerman | 14:725a608b6709 | 154 | pc.printf("Counts1: %f \r\n", counts1); |
GerhardBerman | 14:725a608b6709 | 155 | pc.printf("Encoder1: %f \r\n", Encoder1Position); |
GerhardBerman | 14:725a608b6709 | 156 | pc.printf("Motor1: %f \r\n", Motor1Position); |
GerhardBerman | 14:725a608b6709 | 157 | pc.printf("Counts2: %f \r\n", counts2); |
GerhardBerman | 14:725a608b6709 | 158 | pc.printf("Encoder2: %f \r\n", Encoder2Position); |
GerhardBerman | 14:725a608b6709 | 159 | pc.printf("Motor2: %f \r\n", Motor2Position); |
GerhardBerman | 7:2f74dfd1d411 | 160 | } |
GerhardBerman | 7:2f74dfd1d411 | 161 | |
GerhardBerman | 9:e4c34f5665a0 | 162 | void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){ |
GerhardBerman | 7:2f74dfd1d411 | 163 | //float Encoder1Position = counts1/resolution; //position in radians, encoder axis |
GerhardBerman | 7:2f74dfd1d411 | 164 | //float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis |
GerhardBerman | 7:2f74dfd1d411 | 165 | |
GerhardBerman | 7:2f74dfd1d411 | 166 | // linear feedback control |
GerhardBerman | 7:2f74dfd1d411 | 167 | float error1 = q1_dot; //referencePosition1 - Position1; // proportional error in radians |
GerhardBerman | 9:e4c34f5665a0 | 168 | float error2 = q2_dot; //referencePosition1 - Position1; // proportional error in radians |
GerhardBerman | 7:2f74dfd1d411 | 169 | float Kp = 1; //potMeter2.read(); |
GerhardBerman | 7:2f74dfd1d411 | 170 | |
GerhardBerman | 7:2f74dfd1d411 | 171 | float IntError1 = IntError1 + error1*t_sample; // integrated error in radians |
GerhardBerman | 9:e4c34f5665a0 | 172 | float IntError2 = IntError2 + error2*t_sample; // integrated error in radians |
GerhardBerman | 7:2f74dfd1d411 | 173 | //float maxKi = 0.2; |
GerhardBerman | 7:2f74dfd1d411 | 174 | float Ki = 0.1; //potMeter2.read(); |
GerhardBerman | 7:2f74dfd1d411 | 175 | |
GerhardBerman | 7:2f74dfd1d411 | 176 | float DerivativeError1 = (error1_prev + error1)/t_sample; // derivative of error in radians |
GerhardBerman | 9:e4c34f5665a0 | 177 | float DerivativeError2 = (error2_prev + error2)/t_sample; // derivative of error in radians |
GerhardBerman | 7:2f74dfd1d411 | 178 | //float maxKd = 0.2; |
GerhardBerman | 7:2f74dfd1d411 | 179 | float Kd = 0.0; //potMeter2.read(); |
GerhardBerman | 7:2f74dfd1d411 | 180 | |
GerhardBerman | 7:2f74dfd1d411 | 181 | //scope.set(0,referencePosition1); |
GerhardBerman | 7:2f74dfd1d411 | 182 | //scope.set(1,Position1); |
GerhardBerman | 7:2f74dfd1d411 | 183 | //scope.set(2,Ki); |
GerhardBerman | 7:2f74dfd1d411 | 184 | //scope.send(); |
GerhardBerman | 7:2f74dfd1d411 | 185 | |
GerhardBerman | 9:e4c34f5665a0 | 186 | motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd; //total controller output = motor input |
GerhardBerman | 9:e4c34f5665a0 | 187 | motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd; //total controller output = motor input |
GerhardBerman | 7:2f74dfd1d411 | 188 | //pc.printf("Motor Axis Position: %f rad \r\n", Position1); |
GerhardBerman | 7:2f74dfd1d411 | 189 | //pc.printf("Counts encoder1: %i rad \r\n", counts1); |
GerhardBerman | 7:2f74dfd1d411 | 190 | //pc.printf("Kp: %f \r\n", Kp); |
GerhardBerman | 7:2f74dfd1d411 | 191 | //pc.printf("MotorValue: %f \r\n", motorValue1); |
GerhardBerman | 7:2f74dfd1d411 | 192 | |
GerhardBerman | 12:05e5964b69a4 | 193 | pc.printf("error1: %f \r\n", error1); |
GerhardBerman | 12:05e5964b69a4 | 194 | pc.printf("IntError1: %f \r\n", IntError1); |
GerhardBerman | 12:05e5964b69a4 | 195 | pc.printf("DerError1: %f \r\n", DerivativeError1); |
GerhardBerman | 12:05e5964b69a4 | 196 | pc.printf("error2: %f \r\n", error2); |
GerhardBerman | 12:05e5964b69a4 | 197 | pc.printf("IntError2: %f \r\n", IntError2); |
GerhardBerman | 12:05e5964b69a4 | 198 | pc.printf("DerError2: %f \r\n", DerivativeError2); |
GerhardBerman | 12:05e5964b69a4 | 199 | |
GerhardBerman | 7:2f74dfd1d411 | 200 | error1_prev = error1; |
GerhardBerman | 9:e4c34f5665a0 | 201 | error2_prev = error1; |
GerhardBerman | 11:773b3532d50f | 202 | float biceps1 = !button1.read(); |
GerhardBerman | 11:773b3532d50f | 203 | float biceps2 = !button2.read(); |
GerhardBerman | 11:773b3532d50f | 204 | |
GerhardBerman | 12:05e5964b69a4 | 205 | /* |
GerhardBerman | 12:05e5964b69a4 | 206 | scope.set(0,q1); |
GerhardBerman | 12:05e5964b69a4 | 207 | scope.set(1,q2); |
GerhardBerman | 11:773b3532d50f | 208 | scope.set(2,biceps1); |
GerhardBerman | 11:773b3532d50f | 209 | scope.set(3,biceps2); |
GerhardBerman | 11:773b3532d50f | 210 | scope.send(); |
GerhardBerman | 12:05e5964b69a4 | 211 | */ |
GerhardBerman | 3:8caef4872b0c | 212 | } |
GerhardBerman | 3:8caef4872b0c | 213 | |
GerhardBerman | 9:e4c34f5665a0 | 214 | void SetMotor1(float motorValue1, float motorValue2) |
GerhardBerman | 3:8caef4872b0c | 215 | { |
GerhardBerman | 3:8caef4872b0c | 216 | // Given -1<=motorValue<=1, this sets the PWM and direction |
GerhardBerman | 3:8caef4872b0c | 217 | // bits for motor 1. Positive value makes motor rotating |
GerhardBerman | 3:8caef4872b0c | 218 | // clockwise. motorValues outside range are truncated to |
GerhardBerman | 3:8caef4872b0c | 219 | // within range |
GerhardBerman | 9:e4c34f5665a0 | 220 | //control motor 1 |
GerhardBerman | 15:9061cf7db23e | 221 | if (motorValue1 >=0) //clockwise rotation |
GerhardBerman | 15:9061cf7db23e | 222 | {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower |
GerhardBerman | 9:e4c34f5665a0 | 223 | //led1=1; |
GerhardBerman | 9:e4c34f5665a0 | 224 | //led2=0; |
GerhardBerman | 3:8caef4872b0c | 225 | } |
GerhardBerman | 15:9061cf7db23e | 226 | else //counterclockwise rotation |
GerhardBerman | 15:9061cf7db23e | 227 | {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower |
GerhardBerman | 9:e4c34f5665a0 | 228 | //led1=0; |
GerhardBerman | 9:e4c34f5665a0 | 229 | //led2=1; |
GerhardBerman | 3:8caef4872b0c | 230 | } |
GerhardBerman | 7:2f74dfd1d411 | 231 | if (fabs(motorValue1)>1) motor1MagnitudePin = 1; |
GerhardBerman | 16:9b7651fdf5a0 | 232 | else motor1MagnitudePin = 0.1*fabs(motorValue1); //fabs(motorValue1); |
GerhardBerman | 9:e4c34f5665a0 | 233 | //control motor 2 |
GerhardBerman | 15:9061cf7db23e | 234 | if (motorValue2 >=0) //clockwise rotation |
GerhardBerman | 14:725a608b6709 | 235 | {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted) |
GerhardBerman | 10:45473f650198 | 236 | //led1=1; |
GerhardBerman | 10:45473f650198 | 237 | //led2=0; |
GerhardBerman | 7:2f74dfd1d411 | 238 | } |
GerhardBerman | 15:9061cf7db23e | 239 | else //counterclockwise rotation |
GerhardBerman | 15:9061cf7db23e | 240 | {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted) |
GerhardBerman | 10:45473f650198 | 241 | //led1=0; |
GerhardBerman | 10:45473f650198 | 242 | //led2=1; |
GerhardBerman | 7:2f74dfd1d411 | 243 | } |
GerhardBerman | 7:2f74dfd1d411 | 244 | if (fabs(motorValue2)>1) motor2MagnitudePin = 1; |
GerhardBerman | 16:9b7651fdf5a0 | 245 | else motor2MagnitudePin = 0.1*fabs(motorValue2); //fabs(motorValue1); |
GerhardBerman | 3:8caef4872b0c | 246 | } |
GerhardBerman | 3:8caef4872b0c | 247 | |
GerhardBerman | 3:8caef4872b0c | 248 | void MeasureAndControl() |
GerhardBerman | 3:8caef4872b0c | 249 | { |
GerhardBerman | 9:e4c34f5665a0 | 250 | // This function measures the EMG of both arms, calculates via IK what |
GerhardBerman | 9:e4c34f5665a0 | 251 | // the joint speeds should be, and controls the motor with |
GerhardBerman | 9:e4c34f5665a0 | 252 | // a Feedforward controller. This is called from a Ticker. |
GerhardBerman | 12:05e5964b69a4 | 253 | GetReferenceKinematics1(q1, q2, q1_dot, q2_dot); |
GerhardBerman | 9:e4c34f5665a0 | 254 | FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2); |
GerhardBerman | 9:e4c34f5665a0 | 255 | SetMotor1(motorValue1, motorValue2); |
GerhardBerman | 3:8caef4872b0c | 256 | } |
GerhardBerman | 3:8caef4872b0c | 257 | |
GerhardBerman | 3:8caef4872b0c | 258 | void TimeTrackerF(){ |
GerhardBerman | 3:8caef4872b0c | 259 | //wait(1); |
GerhardBerman | 3:8caef4872b0c | 260 | //float Potmeter1 = potMeter1.read(); |
GerhardBerman | 7:2f74dfd1d411 | 261 | //float referencePosition1 = GetReferencePosition(); |
GerhardBerman | 7:2f74dfd1d411 | 262 | //pc.printf("TTReference Position: %d rad \r\n", referencePosition1); |
GerhardBerman | 3:8caef4872b0c | 263 | //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1); |
GerhardBerman | 3:8caef4872b0c | 264 | //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2); |
GerhardBerman | 7:2f74dfd1d411 | 265 | //pc.printf("TTCounts: %i \r\n", counts1); |
GerhardBerman | 3:8caef4872b0c | 266 | } |
GerhardBerman | 7:2f74dfd1d411 | 267 | |
GerhardBerman | 3:8caef4872b0c | 268 | /* |
GerhardBerman | 3:8caef4872b0c | 269 | void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts |
GerhardBerman | 3:8caef4872b0c | 270 | //double in=DerivativeCounts(); |
GerhardBerman | 3:8caef4872b0c | 271 | bqcDerivativeCounts=bqc.step(DerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 272 | //return(bqcDerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 273 | } |
GerhardBerman | 9:e4c34f5665a0 | 274 | */ |
GerhardBerman | 6:3c4f3f2ce54f | 275 | |
GerhardBerman | 0:43160ef59f9f | 276 | int main() |
GerhardBerman | 0:43160ef59f9f | 277 | { |
GerhardBerman | 0:43160ef59f9f | 278 | //Initialize |
GerhardBerman | 15:9061cf7db23e | 279 | //int led1val = led1.read(); |
GerhardBerman | 15:9061cf7db23e | 280 | //int led2val = led2.read(); |
GerhardBerman | 9:e4c34f5665a0 | 281 | pc.baud(115200); |
GerhardBerman | 12:05e5964b69a4 | 282 | pc.printf("Test putty IK"); |
GerhardBerman | 9:e4c34f5665a0 | 283 | MeasureTicker.attach(&MeasureTicker_act, 1.0f); |
GerhardBerman | 10:45473f650198 | 284 | bqc.add(&bq1).add(&bq2); |
GerhardBerman | 10:45473f650198 | 285 | |
GerhardBerman | 0:43160ef59f9f | 286 | while(1) |
GerhardBerman | 0:43160ef59f9f | 287 | { |
GerhardBerman | 3:8caef4872b0c | 288 | if (MeasureTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 289 | MeasureTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 290 | MeasureAndControl(); |
GerhardBerman | 15:9061cf7db23e | 291 | counts1 = Encoder1.getPulses(); // gives position of encoder |
GerhardBerman | 15:9061cf7db23e | 292 | counts2 = Encoder2.getPulses(); // gives position of encoder |
GerhardBerman | 3:8caef4872b0c | 293 | } |
GerhardBerman | 10:45473f650198 | 294 | /* |
GerhardBerman | 3:8caef4872b0c | 295 | if (BiQuadTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 296 | BiQuadTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 297 | BiQuadFilter(); |
GerhardBerman | 3:8caef4872b0c | 298 | } |
GerhardBerman | 10:45473f650198 | 299 | */ |
GerhardBerman | 0:43160ef59f9f | 300 | } |
GerhardBerman | 0:43160ef59f9f | 301 | } |