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@31:880ebd2e8145, 2016-10-28 (annotated)

Committer:
GerhardBerman
Date:
Fri Oct 28 14:21:46 2016 +0000
Revision:
31:880ebd2e8145
Parent:
30:676ecd59467a 
ErrorSwitch loop is introduced, not yet tested

Who changed what in which revision?

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