Elfi Hofmeijer / Mbed 2 deprecated prog_forwardkin_feedback_copy

With added boundaries, without errors, not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_forwardkin_feedback_copy by Gerhard Berman

main.cpp@18:d2cfd07ae88a, 2016-10-24 (annotated)

Committer:
GerhardBerman
Date:
Mon Oct 24 09:07:07 2016 +0000
Revision:
18:d2cfd07ae88a
Parent:
17:91d20d362e72
Child:
19:cba54636bd62
All working, but Position_x and Position_y don't start at value 0, which causes spontaneous motor action.

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