Gerhard Berman / Mbed 2 deprecated prog_forwardkin_feedback_copy

Working, but boundaries not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_inversekin_feedback by Gerhard Berman

main.cpp@19:cba54636bd62, 2016-10-25 (annotated)

Committer:
GerhardBerman
Date:
Tue Oct 25 13:37:22 2016 +0000
Revision:
19:cba54636bd62
Parent:
18:d2cfd07ae88a 
Working, but boundaries not yet tested

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 19:cba54636bd62 19 - Add tower height to ReferencePosition_y and Position_y
GerhardBerman 17:91d20d362e72 20 */
GerhardBerman 17:91d20d362e72 21
GerhardBerman 0:43160ef59f9f 22 //set pins
GerhardBerman 0:43160ef59f9f 23 DigitalIn encoder1A (D13); //Channel A van Encoder 1
GerhardBerman 0:43160ef59f9f 24 DigitalIn encoder1B (D12); //Channel B van Encoder 1
GerhardBerman 14:725a608b6709 25 DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
GerhardBerman 14:725a608b6709 26 DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
GerhardBerman 14:725a608b6709 27 //DigitalOut led1 (D11);
GerhardBerman 14:725a608b6709 28 //DigitalOut led2 (D10);
GerhardBerman 19:cba54636bd62 29 //AnalogIn potMeter1(A2);
GerhardBerman 19:cba54636bd62 30 //AnalogIn potMeter2(A1);
GerhardBerman 0:43160ef59f9f 31 DigitalOut motor1DirectionPin(D7);
GerhardBerman 0:43160ef59f9f 32 PwmOut motor1MagnitudePin(D6);
GerhardBerman 7:2f74dfd1d411 33 DigitalOut motor2DirectionPin(D4);
GerhardBerman 7:2f74dfd1d411 34 PwmOut motor2MagnitudePin(D5);
GerhardBerman 9:e4c34f5665a0 35 DigitalIn button1(D3);
GerhardBerman 7:2f74dfd1d411 36 DigitalIn button2(D9);
GerhardBerman 0:43160ef59f9f 37
GerhardBerman 7:2f74dfd1d411 38 //library settings
GerhardBerman 0:43160ef59f9f 39 Serial pc(USBTX,USBRX);
GerhardBerman 3:8caef4872b0c 40 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
GerhardBerman 12:05e5964b69a4 41 //HIDScope scope(4);
GerhardBerman 0:43160ef59f9f 42
GerhardBerman 18:d2cfd07ae88a 43 //set constant or variable values
GerhardBerman 18:d2cfd07ae88a 44 float EMGgain = 1.0;
GerhardBerman 7:2f74dfd1d411 45 double DerivativeCounts;
GerhardBerman 7:2f74dfd1d411 46 float x0 = 1.0;
GerhardBerman 7:2f74dfd1d411 47 float L0 = 1.0;
GerhardBerman 7:2f74dfd1d411 48 float L1 = 1.0;
GerhardBerman 18:d2cfd07ae88a 49 float L2 = 1.0;
GerhardBerman 7:2f74dfd1d411 50 float dx;
GerhardBerman 7:2f74dfd1d411 51 float dy;
GerhardBerman 19:cba54636bd62 52 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
GerhardBerman 19:cba54636bd62 53 float motorMaxGain1 = 0.1;
GerhardBerman 19:cba54636bd62 54 float motorMaxGain2 = 0.1;
GerhardBerman 4:19e376d31380 55 float t_sample = 0.01; //seconds
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 19:cba54636bd62 61 const float maxStampDistance = 1.5;
GerhardBerman 0:43160ef59f9f 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
GerhardBerman 19:cba54636bd62 67 float ReferencePosition_x = L2;
GerhardBerman 19:cba54636bd62 68 float ReferencePosition_y = L1;
GerhardBerman 19:cba54636bd62 69 float ReferencePosition_xnew = 0;
GerhardBerman 19:cba54636bd62 70 float ReferencePosition_ynew = 0;
GerhardBerman 19:cba54636bd62 71 float Position_x = 0.0;
GerhardBerman 19:cba54636bd62 72 float Position_y = 0.0;
GerhardBerman 19:cba54636bd62 73 float q1 = 0;
GerhardBerman 19:cba54636bd62 74 float q2 = 0;
GerhardBerman 19:cba54636bd62 75 float q1_ref = 0;
GerhardBerman 19:cba54636bd62 76 float q2_ref = 0;
GerhardBerman 19:cba54636bd62 77 float q1start = 0;
GerhardBerman 19:cba54636bd62 78 float q2start = PI/2;
GerhardBerman 19:cba54636bd62 79
GerhardBerman 19:cba54636bd62 80 float q1_refOutNew = 0;
GerhardBerman 19:cba54636bd62 81 float q1_refOutMin = 0; //WRONG values
GerhardBerman 19:cba54636bd62 82 float q1_refOutMax = PI; //WRONG values
GerhardBerman 19:cba54636bd62 83 float q2_refOutNew = 0;
GerhardBerman 19:cba54636bd62 84 float q2_refOutMin = 0; //WRONG values
GerhardBerman 19:cba54636bd62 85 float q2_refOutMax = PI; //WRONG values
GerhardBerman 19:cba54636bd62 86
GerhardBerman 19:cba54636bd62 87 float q1_error_prev = 0;
GerhardBerman 19:cba54636bd62 88 float q2_error_prev = 0;
GerhardBerman 19:cba54636bd62 89 float q1IntError = 0;
GerhardBerman 19:cba54636bd62 90 float q2IntError = 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 19:cba54636bd62 98
GerhardBerman 0:43160ef59f9f 99 //set BiQuad
GerhardBerman 0:43160ef59f9f 100 BiQuadChain bqc;
GerhardBerman 0:43160ef59f9f 101 BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
GerhardBerman 0:43160ef59f9f 102 BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
GerhardBerman 0:43160ef59f9f 103
GerhardBerman 0:43160ef59f9f 104 //set go-Ticker settings
GerhardBerman 3:8caef4872b0c 105 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
GerhardBerman 3:8caef4872b0c 106 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
GerhardBerman 3:8caef4872b0c 107 void BiQuadTicker_act(){BiQuadTicker_go=true;};
GerhardBerman 3:8caef4872b0c 108 void FeedbackTicker_act(){FeedbackTicker_go=true;};
GerhardBerman 3:8caef4872b0c 109 void TimeTracker_act(){TimeTracker_go=true;};
GerhardBerman 3:8caef4872b0c 110 //void sampleT_act(){sampleT_go=true;};
GerhardBerman 3:8caef4872b0c 111
GerhardBerman 3:8caef4872b0c 112 //define encoder counts and degrees
GerhardBerman 7:2f74dfd1d411 113 QEI Encoder1(D12, D13, NC, 32); // turns on encoder
GerhardBerman 15:9061cf7db23e 114 QEI Encoder2(D10, D11, NC, 32); // turns on encoder
GerhardBerman 14:725a608b6709 115
GerhardBerman 4:19e376d31380 116 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 117 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 118 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 119
GerhardBerman 19:cba54636bd62 120 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
GerhardBerman 7:2f74dfd1d411 121
GerhardBerman 19:cba54636bd62 122 //get joint positions q feedback from encoder
GerhardBerman 19:cba54636bd62 123 float Encoder1Position = counts1/resolution; //angular position in radians, encoder axis
GerhardBerman 9:e4c34f5665a0 124 float Encoder2Position = counts2/resolution;
GerhardBerman 19:cba54636bd62 125
GerhardBerman 19:cba54636bd62 126 q1Out = q1start + Encoder1Position*inverse_gear_ratio; //angular position in radians, motor axis
GerhardBerman 19:cba54636bd62 127 q2Out = q2start + Encoder2Position*inverse_gear_ratio;
GerhardBerman 7:2f74dfd1d411 128
GerhardBerman 19:cba54636bd62 129 /*
GerhardBerman 19:cba54636bd62 130 //get end effector position feedback with Brockett
GerhardBerman 18:d2cfd07ae88a 131 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 132 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 133 */
GerhardBerman 19:cba54636bd62 134 //get end effector position feedback with trigonometry
GerhardBerman 19:cba54636bd62 135 Position_x = (L1*sin(q1) + L2*sin(q1+q2));
GerhardBerman 19:cba54636bd62 136 Position_y = (L1*cos(q1) + L2*cos(q1+q2));
GerhardBerman 19:cba54636bd62 137 //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
GerhardBerman 9:e4c34f5665a0 138
GerhardBerman 7:2f74dfd1d411 139 //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
GerhardBerman 19:cba54636bd62 140 biceps_l = !button1.read() * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
GerhardBerman 19:cba54636bd62 141 biceps_r = !button2.read() * EMGgain; //emg1.read();
GerhardBerman 18:d2cfd07ae88a 142 if (biceps_l > 0 && biceps_r > 0){
GerhardBerman 8:935abf8ecc27 143 //both arms activated: stamp moves down
GerhardBerman 15:9061cf7db23e 144 //led1 = 1;
GerhardBerman 15:9061cf7db23e 145 //led2 = 1;
GerhardBerman 19:cba54636bd62 146 ReferencePosition_xnew = ReferencePosition_x;
GerhardBerman 19:cba54636bd62 147 ReferencePosition_ynew = ReferencePosition_y - dy_stampdown; //into stamping vertical position ~the stamp down action
GerhardBerman 18:d2cfd07ae88a 148
GerhardBerman 18:d2cfd07ae88a 149 /*
GerhardBerman 18:d2cfd07ae88a 150 wait(1); //lift stamp after stamping
GerhardBerman 18:d2cfd07ae88a 151 ReferencePosition_y = ReferencePosition_y - dy_stampdown;
GerhardBerman 18:d2cfd07ae88a 152 PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 153 dy = PositionError_y;
GerhardBerman 12:05e5964b69a4 154 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 155 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 156 */
GerhardBerman 18:d2cfd07ae88a 157 }
GerhardBerman 18:d2cfd07ae88a 158 else if (biceps_l > 0 && biceps_r <= 0){
GerhardBerman 18:d2cfd07ae88a 159 //arm 1 activated, move left
GerhardBerman 18:d2cfd07ae88a 160 //led1 = 1;
GerhardBerman 18:d2cfd07ae88a 161 //led2 = 0;
GerhardBerman 19:cba54636bd62 162 ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l;
GerhardBerman 19:cba54636bd62 163 ReferencePosition_ynew = ReferencePosition_y;
GerhardBerman 12:05e5964b69a4 164 /*
GerhardBerman 18:d2cfd07ae88a 165 PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 166 PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 167
GerhardBerman 18:d2cfd07ae88a 168 dx = PositionError_x;
GerhardBerman 18:d2cfd07ae88a 169 dy = PositionError_y;
GerhardBerman 12:05e5964b69a4 170 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 171 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 172 */
GerhardBerman 8:935abf8ecc27 173 }
GerhardBerman 18:d2cfd07ae88a 174 else if (biceps_l <= 0 && biceps_r > 0){
GerhardBerman 18:d2cfd07ae88a 175 //arm 1 activated, move right
GerhardBerman 18:d2cfd07ae88a 176 //led1 = 0;
GerhardBerman 18:d2cfd07ae88a 177 //led2 = 1;
GerhardBerman 19:cba54636bd62 178 ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r;
GerhardBerman 19:cba54636bd62 179 ReferencePosition_ynew = ReferencePosition_y;
GerhardBerman 18:d2cfd07ae88a 180 /*PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 181 PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
GerhardBerman 18:d2cfd07ae88a 182
GerhardBerman 18:d2cfd07ae88a 183 dx = PositionError_x;
GerhardBerman 18:d2cfd07ae88a 184 dy = PositionError_y;
GerhardBerman 12:05e5964b69a4 185 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 186 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 187 */
GerhardBerman 8:935abf8ecc27 188 }
GerhardBerman 8:935abf8ecc27 189 else{
GerhardBerman 15:9061cf7db23e 190 //led1 = 0;
GerhardBerman 15:9061cf7db23e 191 //led2 = 0;
GerhardBerman 19:cba54636bd62 192 ReferencePosition_xnew = ReferencePosition_x;
GerhardBerman 19:cba54636bd62 193 ReferencePosition_ynew = ReferencePosition_y;
GerhardBerman 19:cba54636bd62 194 }
GerhardBerman 19:cba54636bd62 195
GerhardBerman 19:cba54636bd62 196 float ReferenceVector = sqrt(pow(ReferencePosition_x,2)+pow(ReferencePosition_y,2));
GerhardBerman 19:cba54636bd62 197
GerhardBerman 19:cba54636bd62 198 //check position boundaries
GerhardBerman 19:cba54636bd62 199 if (ReferenceVector > maxStampDistance){
GerhardBerman 18:d2cfd07ae88a 200 ReferencePosition_x = ReferencePosition_x;
GerhardBerman 18:d2cfd07ae88a 201 ReferencePosition_y = ReferencePosition_y;
GerhardBerman 8:935abf8ecc27 202 }
GerhardBerman 19:cba54636bd62 203 else if (ReferencePosition_ynew < 0){
GerhardBerman 19:cba54636bd62 204 ReferencePosition_y = 0; //could also be little negative value to get more pressure on table
GerhardBerman 19:cba54636bd62 205 }
GerhardBerman 19:cba54636bd62 206 else {
GerhardBerman 19:cba54636bd62 207 ReferencePosition_x = ReferencePosition_xnew;
GerhardBerman 19:cba54636bd62 208 ReferencePosition_y = ReferencePosition_ynew;
GerhardBerman 19:cba54636bd62 209
GerhardBerman 19:cba54636bd62 210 //calculate reference joint angles for the new reference position
GerhardBerman 19:cba54636bd62 211 float alpha = atan(ReferencePosition_y/ReferencePosition_x);
GerhardBerman 19:cba54636bd62 212 float beta = acos((L2*L2-L1*L1-pow(ReferenceVector,2))/(-2*L1*ReferenceVector));
GerhardBerman 19:cba54636bd62 213 q1_refOutNew = PI/2 - (alpha+beta);
GerhardBerman 19:cba54636bd62 214 q2_refOutNew = PI - asin(ReferenceVector*sin(beta)/L2);
GerhardBerman 19:cba54636bd62 215
GerhardBerman 19:cba54636bd62 216 //check angle boundaries
GerhardBerman 19:cba54636bd62 217 if (q1_refOutNew > q1_refOutMin && q1_refOutMax < q1_refOutMax){
GerhardBerman 19:cba54636bd62 218 q1_refOut = q1_refOutNew;
GerhardBerman 19:cba54636bd62 219 }
GerhardBerman 19:cba54636bd62 220 else {
GerhardBerman 19:cba54636bd62 221 q1_refOut = q1_refOut;
GerhardBerman 19:cba54636bd62 222 }
GerhardBerman 19:cba54636bd62 223 if (q2_refOutNew > q2_refOutMin && q2_refOutMax < q2_refOutMax){
GerhardBerman 19:cba54636bd62 224 q2_refOut = q2_refOutNew;
GerhardBerman 19:cba54636bd62 225 }
GerhardBerman 19:cba54636bd62 226 else {
GerhardBerman 19:cba54636bd62 227 q2_refOut = q2_refOut;
GerhardBerman 19:cba54636bd62 228 }
GerhardBerman 18:d2cfd07ae88a 229
GerhardBerman 12:05e5964b69a4 230
GerhardBerman 7:2f74dfd1d411 231 //update joint angles
GerhardBerman 18:d2cfd07ae88a 232 //q1Out = q1Out + q1_dotOut; //in radians
GerhardBerman 18:d2cfd07ae88a 233 //q2Out = q2Out + q2_dotOut;
GerhardBerman 12:05e5964b69a4 234
GerhardBerman 12:05e5964b69a4 235 pc.baud(115200);
GerhardBerman 19:cba54636bd62 236 pc.printf("biceps_l: %f \r\n", biceps_l);
GerhardBerman 19:cba54636bd62 237 pc.printf("biceps_r: %f \r\n", biceps_r);
GerhardBerman 19:cba54636bd62 238 pc.printf("Position_x: %f m\r\n", Position_x);
GerhardBerman 19:cba54636bd62 239 pc.printf("q1Out: %f rad \r\n", q1Out);
GerhardBerman 19:cba54636bd62 240 pc.printf("Reference_x: %f m \r\n", ReferencePosition_x);
GerhardBerman 19:cba54636bd62 241 pc.printf("q1refOut: %f rad\r\n", q1_refOut);
GerhardBerman 19:cba54636bd62 242 pc.printf("Position_y: %f m\r\n", Position_y);
GerhardBerman 19:cba54636bd62 243 pc.printf("q2Out: %f rad \r\n", q2Out);
GerhardBerman 19:cba54636bd62 244 pc.printf("Reference_y: %f m \r\n", ReferencePosition_y);
GerhardBerman 19:cba54636bd62 245 pc.printf("q2refOut: %f rad\r\n", q2_refOut);
GerhardBerman 19:cba54636bd62 246 pc.printf("alpha: %f rad \r\n", alpha);
GerhardBerman 19:cba54636bd62 247 pc.printf("beta: %f rad\r\n", beta);
GerhardBerman 18:d2cfd07ae88a 248
GerhardBerman 19:cba54636bd62 249
GerhardBerman 18:d2cfd07ae88a 250 /*
GerhardBerman 12:05e5964b69a4 251 pc.printf("dx: %f \r\n", dx);
GerhardBerman 12:05e5964b69a4 252 pc.printf("dy: %f \r\n", dy);
GerhardBerman 12:05e5964b69a4 253 pc.printf("q1: %f \r\n", q1Out);
GerhardBerman 12:05e5964b69a4 254 pc.printf("q1_dot: %f \r\n", q1_dotOut);
GerhardBerman 12:05e5964b69a4 255 pc.printf("q2: %f \r\n", q2Out);
GerhardBerman 12:05e5964b69a4 256 pc.printf("q2_dot: %f \r\n", q2_dotOut);
GerhardBerman 12:05e5964b69a4 257
GerhardBerman 14:725a608b6709 258 pc.printf("Counts1: %f \r\n", counts1);
GerhardBerman 14:725a608b6709 259 pc.printf("Encoder1: %f \r\n", Encoder1Position);
GerhardBerman 18:d2cfd07ae88a 260 pc.printf("Motor1: %f \r\n", q1Out);
GerhardBerman 14:725a608b6709 261 pc.printf("Counts2: %f \r\n", counts2);
GerhardBerman 14:725a608b6709 262 pc.printf("Encoder2: %f \r\n", Encoder2Position);
GerhardBerman 18:d2cfd07ae88a 263 pc.printf("Motor2: %f \r\n", q2Out);
GerhardBerman 18:d2cfd07ae88a 264 */
GerhardBerman 7:2f74dfd1d411 265 }
GerhardBerman 18:d2cfd07ae88a 266
GerhardBerman 7:2f74dfd1d411 267
GerhardBerman 19:cba54636bd62 268 void FeedForwardControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
GerhardBerman 7:2f74dfd1d411 269 //float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 270 //float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 271
GerhardBerman 7:2f74dfd1d411 272 // linear feedback control
GerhardBerman 19:cba54636bd62 273 float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians
GerhardBerman 19:cba54636bd62 274 float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians
GerhardBerman 7:2f74dfd1d411 275 float Kp = 1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 276
GerhardBerman 19:cba54636bd62 277 float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians
GerhardBerman 19:cba54636bd62 278 float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 279 //float maxKi = 0.2;
GerhardBerman 7:2f74dfd1d411 280 float Ki = 0.1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 281
GerhardBerman 19:cba54636bd62 282 float q1DerivativeError = (q1_error_prev + q1_error)/t_sample; // derivative of error in radians
GerhardBerman 19:cba54636bd62 283 float q2DerivativeError = (q2_error_prev + q2_error)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 284 //float maxKd = 0.2;
GerhardBerman 7:2f74dfd1d411 285 float Kd = 0.0; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 286
GerhardBerman 7:2f74dfd1d411 287 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 288 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 289 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 290 //scope.send();
GerhardBerman 7:2f74dfd1d411 291
GerhardBerman 19:cba54636bd62 292 motorValue1Out = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output = motor input
GerhardBerman 19:cba54636bd62 293 motorValue2Out = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output = motor input
GerhardBerman 7:2f74dfd1d411 294 //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
GerhardBerman 7:2f74dfd1d411 295 //pc.printf("Counts encoder1: %i rad \r\n", counts1);
GerhardBerman 7:2f74dfd1d411 296 //pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 7:2f74dfd1d411 297 //pc.printf("MotorValue: %f \r\n", motorValue1);
GerhardBerman 7:2f74dfd1d411 298
GerhardBerman 19:cba54636bd62 299 pc.printf("error1: %f \r\n", q1_error);
GerhardBerman 19:cba54636bd62 300 pc.printf("IntError1: %f \r\n", q1IntError);
GerhardBerman 19:cba54636bd62 301 pc.printf("DerError1: %f \r\n", q1DerivativeError);
GerhardBerman 19:cba54636bd62 302 pc.printf("error2: %f \r\n", q2_error);
GerhardBerman 19:cba54636bd62 303 pc.printf("IntError2: %f \r\n", q2IntError);
GerhardBerman 19:cba54636bd62 304 pc.printf("DerError2: %f \r\n", q2DerivativeError);
GerhardBerman 12:05e5964b69a4 305
GerhardBerman 19:cba54636bd62 306 q1_error_prev = q1_error;
GerhardBerman 19:cba54636bd62 307 q2_error_prev = q2_error;
GerhardBerman 18:d2cfd07ae88a 308 //float biceps_l = !button1.read();
GerhardBerman 18:d2cfd07ae88a 309 //float biceps_r = !button2.read();
GerhardBerman 11:773b3532d50f 310
GerhardBerman 12:05e5964b69a4 311 /*
GerhardBerman 12:05e5964b69a4 312 scope.set(0,q1);
GerhardBerman 12:05e5964b69a4 313 scope.set(1,q2);
GerhardBerman 18:d2cfd07ae88a 314 scope.set(2,biceps_l);
GerhardBerman 18:d2cfd07ae88a 315 scope.set(3,biceps_r);
GerhardBerman 11:773b3532d50f 316 scope.send();
GerhardBerman 12:05e5964b69a4 317 */
GerhardBerman 3:8caef4872b0c 318 }
GerhardBerman 3:8caef4872b0c 319
GerhardBerman 9:e4c34f5665a0 320 void SetMotor1(float motorValue1, float motorValue2)
GerhardBerman 3:8caef4872b0c 321 {
GerhardBerman 3:8caef4872b0c 322 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 323 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 324 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 325 // within range
GerhardBerman 9:e4c34f5665a0 326 //control motor 1
GerhardBerman 15:9061cf7db23e 327 if (motorValue1 >=0) //clockwise rotation
GerhardBerman 19:cba54636bd62 328 {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 329 //led1=1;
GerhardBerman 9:e4c34f5665a0 330 //led2=0;
GerhardBerman 3:8caef4872b0c 331 }
GerhardBerman 15:9061cf7db23e 332 else //counterclockwise rotation
GerhardBerman 19:cba54636bd62 333 {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 334 //led1=0;
GerhardBerman 9:e4c34f5665a0 335 //led2=1;
GerhardBerman 3:8caef4872b0c 336 }
GerhardBerman 19:cba54636bd62 337 if (fabs(motorValue1)>1) motor1MagnitudePin = motorMaxGain1;
GerhardBerman 19:cba54636bd62 338 else motor1MagnitudePin = motorMaxGain1*fabs(motorValue1); //fabs(motorValue1);
GerhardBerman 9:e4c34f5665a0 339 //control motor 2
GerhardBerman 15:9061cf7db23e 340 if (motorValue2 >=0) //clockwise rotation
GerhardBerman 14:725a608b6709 341 {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 342 //led1=1;
GerhardBerman 10:45473f650198 343 //led2=0;
GerhardBerman 7:2f74dfd1d411 344 }
GerhardBerman 15:9061cf7db23e 345 else //counterclockwise rotation
GerhardBerman 15:9061cf7db23e 346 {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 347 //led1=0;
GerhardBerman 10:45473f650198 348 //led2=1;
GerhardBerman 7:2f74dfd1d411 349 }
GerhardBerman 19:cba54636bd62 350 if (fabs(motorValue2)>1) motor2MagnitudePin = motorMaxGain2;
GerhardBerman 19:cba54636bd62 351 else motor2MagnitudePin = motorMaxGain2*fabs(motorValue2); //fabs(motorValue1);
GerhardBerman 19:cba54636bd62 352 float ReadMagn1 = motor1MagnitudePin.read();
GerhardBerman 19:cba54636bd62 353 float ReadMagn2 = motor2MagnitudePin.read();
GerhardBerman 19:cba54636bd62 354 pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);
GerhardBerman 19:cba54636bd62 355 pc.printf("motor2Magnitude: %f \r\n", ReadMagn2);
GerhardBerman 3:8caef4872b0c 356 }
GerhardBerman 3:8caef4872b0c 357
GerhardBerman 3:8caef4872b0c 358 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 359 {
GerhardBerman 9:e4c34f5665a0 360 // This function measures the EMG of both arms, calculates via IK what
GerhardBerman 9:e4c34f5665a0 361 // the joint speeds should be, and controls the motor with
GerhardBerman 9:e4c34f5665a0 362 // a Feedforward controller. This is called from a Ticker.
GerhardBerman 19:cba54636bd62 363 GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
GerhardBerman 19:cba54636bd62 364 FeedForwardControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
GerhardBerman 9:e4c34f5665a0 365 SetMotor1(motorValue1, motorValue2);
GerhardBerman 3:8caef4872b0c 366 }
GerhardBerman 3:8caef4872b0c 367
GerhardBerman 3:8caef4872b0c 368 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 369 //wait(1);
GerhardBerman 3:8caef4872b0c 370 //float Potmeter1 = potMeter1.read();
GerhardBerman 7:2f74dfd1d411 371 //float referencePosition1 = GetReferencePosition();
GerhardBerman 7:2f74dfd1d411 372 //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
GerhardBerman 3:8caef4872b0c 373 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 374 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 7:2f74dfd1d411 375 //pc.printf("TTCounts: %i \r\n", counts1);
GerhardBerman 3:8caef4872b0c 376 }
GerhardBerman 7:2f74dfd1d411 377
GerhardBerman 3:8caef4872b0c 378 /*
GerhardBerman 3:8caef4872b0c 379 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 380 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 381 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 382 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 383 }
GerhardBerman 9:e4c34f5665a0 384 */
GerhardBerman 6:3c4f3f2ce54f 385
GerhardBerman 0:43160ef59f9f 386 int main()
GerhardBerman 0:43160ef59f9f 387 {
GerhardBerman 0:43160ef59f9f 388 //Initialize
GerhardBerman 15:9061cf7db23e 389 //int led1val = led1.read();
GerhardBerman 15:9061cf7db23e 390 //int led2val = led2.read();
GerhardBerman 9:e4c34f5665a0 391 pc.baud(115200);
GerhardBerman 12:05e5964b69a4 392 pc.printf("Test putty IK");
GerhardBerman 18:d2cfd07ae88a 393 MeasureTicker.attach(&MeasureTicker_act, 0.1f);
GerhardBerman 10:45473f650198 394 bqc.add(&bq1).add(&bq2);
GerhardBerman 10:45473f650198 395
GerhardBerman 0:43160ef59f9f 396 while(1)
GerhardBerman 0:43160ef59f9f 397 {
GerhardBerman 3:8caef4872b0c 398 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 399 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 400 MeasureAndControl();
GerhardBerman 15:9061cf7db23e 401 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 15:9061cf7db23e 402 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 18:d2cfd07ae88a 403 pc.printf("counts1: %i \r\n", counts1);
GerhardBerman 18:d2cfd07ae88a 404 pc.printf("counts2: %i \r\n", counts2);
GerhardBerman 3:8caef4872b0c 405 }
GerhardBerman 10:45473f650198 406 /*
GerhardBerman 3:8caef4872b0c 407 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 408 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 409 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 410 }
GerhardBerman 10:45473f650198 411 */
GerhardBerman 0:43160ef59f9f 412 }
GerhardBerman 0:43160ef59f9f 413 }