Gerhard Berman / Mbed 2 deprecated EMG_prog_forwardkin_feedback

EMG added to main IK program. No errors, not yet tested

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_forwardkin_feedback_copy3 by Gerhard Berman

main.cpp@9:e4c34f5665a0, 2016-10-19 (annotated)

Committer:
GerhardBerman
Date:
Wed Oct 19 15:38:44 2016 +0000
Revision:
9:e4c34f5665a0
Parent:
8:935abf8ecc27
Child:
10:45473f650198
Double functions (one for motor 1, one for motor 2) replaced by single. No errors, no reaction on board

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