Gerhard Berman / Mbed 2 deprecated prog_inversekin

First trial for Inverse Kinematics Feedforward implementation. No errors, not yet tested with board

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_pract3_3_PI_controller by Gerhard Berman

main.cpp@10:45473f650198, 2016-10-19 (annotated)

Committer:
GerhardBerman
Date:
Wed Oct 19 16:14:25 2016 +0000
Revision:
10:45473f650198
Parent:
9:e4c34f5665a0
Child:
11:773b3532d50f
NO errors, not yet tested at 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 10:45473f650198 97 led1 = 1;
GerhardBerman 10:45473f650198 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 10:45473f650198 106 led1 = 1;
GerhardBerman 10:45473f650198 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 10:45473f650198 113 led1 = 0;
GerhardBerman 10:45473f650198 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 10:45473f650198 119 led1 = 0;
GerhardBerman 10:45473f650198 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 10:45473f650198 191 //led1=1;
GerhardBerman 10:45473f650198 192 //led2=0;
GerhardBerman 7:2f74dfd1d411 193 }
GerhardBerman 7:2f74dfd1d411 194 else {motor2DirectionPin=ccw;
GerhardBerman 10:45473f650198 195 //led1=0;
GerhardBerman 10:45473f650198 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 10:45473f650198 233 int led1val = led1.read();
GerhardBerman 10:45473f650198 234 int led2val = led2.read();
GerhardBerman 10:45473f650198 235
GerhardBerman 10:45473f650198 236 /*
GerhardBerman 9:e4c34f5665a0 237 pc.baud(115200);
GerhardBerman 10:45473f650198 238 pc.printf("Test putty ledvals IK");
GerhardBerman 10:45473f650198 239 while (true) {
GerhardBerman 10:45473f650198 240 wait(0.2f);
GerhardBerman 10:45473f650198 241 pc.printf("Led1: %i \r\n", led1val);
GerhardBerman 10:45473f650198 242 pc.printf("Led2: %i \r\n", led2val);
GerhardBerman 10:45473f650198 243 }
GerhardBerman 10:45473f650198 244 */
GerhardBerman 9:e4c34f5665a0 245 MeasureTicker.attach(&MeasureTicker_act, 1.0f);
GerhardBerman 10:45473f650198 246 bqc.add(&bq1).add(&bq2);
GerhardBerman 10:45473f650198 247
GerhardBerman 0:43160ef59f9f 248 while(1)
GerhardBerman 0:43160ef59f9f 249 {
GerhardBerman 3:8caef4872b0c 250 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 251 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 252 MeasureAndControl();
GerhardBerman 7:2f74dfd1d411 253 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 7:2f74dfd1d411 254 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 10:45473f650198 255 //pc.printf("Resolution: %f pulses/rad \r\n",resolution);
GerhardBerman 3:8caef4872b0c 256 }
GerhardBerman 10:45473f650198 257 /*
GerhardBerman 3:8caef4872b0c 258 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 259 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 260 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 261 }
GerhardBerman 10:45473f650198 262 */
GerhardBerman 0:43160ef59f9f 263 }
GerhardBerman 0:43160ef59f9f 264 }