Gerhard Berman / Mbed 2 deprecated prog_BioRobotics_Group9_StampRobot_EMGThreshold

Version for EMG Threshold finding

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_BioRobotics_Group9_StampRobot by Gerhard Berman

main.cpp@40:9ecaab27acde, 2016-11-03 (annotated)

Committer:
GerhardBerman
Date:
Thu Nov 03 16:12:23 2016 +0000
Revision:
40:9ecaab27acde
Parent:
39:cc7754330c26
Child:
41:68b170829965
Robot working good, not perfectly tracking reference positions!

Who changed what in which revision?

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