Jurriën Bos / Mbed 2 deprecated Filter

Filter werkt eindelijk, echter zijn alle kanalen hetzelfde

Dependencies:   HIDScope MODSERIAL Matrix QEI biquadFilter mbed

Fork of Turning_Motor_V3 by Thom Kuenen

main.cpp@7:c5c648898881, 2018-10-30 (annotated)

Committer:
JurrienBos
Date:
Tue Oct 30 15:41:58 2018 +0000
Revision:
7:c5c648898881
Parent:
6:056ad27636ff 
Filter werkt, echter niet op alle kanalen.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JurrienBos 0:4591ba678a39 1 #include "mbed.h"
JurrienBos 0:4591ba678a39 2 #include "MODSERIAL.h"
ThomBMT 2:dc9766657afb 3 #include "HIDScope.h"
ThomBMT 2:dc9766657afb 4 #include "QEI.h"
JurrienBos 7:c5c648898881 5 #include "BiQuad.h"
ThomBMT 2:dc9766657afb 6
JurrienBos 0:4591ba678a39 7 MODSERIAL pc(USBTX, USBRX);
JurrienBos 0:4591ba678a39 8 DigitalOut DirectionPin1(D4);
JurrienBos 1:4bf64d003f3a 9 DigitalOut DirectionPin2(D7);
JurrienBos 0:4591ba678a39 10 PwmOut PwmPin1(D5);
JurrienBos 1:4bf64d003f3a 11 PwmOut PwmPin2(D6);
JurrienBos 0:4591ba678a39 12 DigitalIn Knop1(D2);
ThomBMT 4:8f67b8327300 13 DigitalIn Knop2(D3);
JurrienBos 0:4591ba678a39 14 AnalogIn pot1 (A5);
JurrienBos 1:4bf64d003f3a 15 AnalogIn pot2 (A4);
ThomBMT 2:dc9766657afb 16 AnalogIn emg0( A0 );
ThomBMT 2:dc9766657afb 17 AnalogIn emg1( A1 );
ThomBMT 2:dc9766657afb 18 AnalogIn emg2( A2 );
ThomBMT 2:dc9766657afb 19 AnalogIn emg3( A3 );
JurrienBos 0:4591ba678a39 20
JurrienBos 7:c5c648898881 21 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
ThomBMT 5:312186a0604d 22 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
ThomBMT 5:312186a0604d 23
ThomBMT 5:312186a0604d 24 //DigitalOut LED(LED_RED);
ThomBMT 5:312186a0604d 25
ThomBMT 4:8f67b8327300 26 Ticker StateTicker;
ThomBMT 2:dc9766657afb 27 Ticker printTicker;
ThomBMT 5:312186a0604d 28
JurrienBos 7:c5c648898881 29 HIDScope scope(4);
JurrienBos 7:c5c648898881 30
JurrienBos 7:c5c648898881 31 BiQuadChain bqc1;
JurrienBos 7:c5c648898881 32 BiQuadChain bqc2;
JurrienBos 7:c5c648898881 33 BiQuadChain bqc3;
JurrienBos 7:c5c648898881 34 BiQuadChain bqc4;
JurrienBos 7:c5c648898881 35 BiQuadChain bqc5;
JurrienBos 7:c5c648898881 36 BiQuadChain bqc6;
JurrienBos 7:c5c648898881 37 BiQuadChain bqc7;
JurrienBos 7:c5c648898881 38 BiQuadChain bqc8;
JurrienBos 7:c5c648898881 39 BiQuad BqNotch1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
JurrienBos 7:c5c648898881 40 BiQuad BqNotch2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
JurrienBos 7:c5c648898881 41 BiQuad BqHP( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
JurrienBos 7:c5c648898881 42 BiQuad BqLP( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
JurrienBos 0:4591ba678a39 43
ThomBMT 3:c8f0fc045505 44 volatile float Bicep_Right = 0.0;
ThomBMT 3:c8f0fc045505 45 volatile float Bicep_Left = 0.0;
ThomBMT 3:c8f0fc045505 46 volatile float Tricep_Right = 0.0;
ThomBMT 3:c8f0fc045505 47 volatile float Tricep_Left = 0.0;
ThomBMT 3:c8f0fc045505 48 volatile const float maxVelocity = 8.4; // in rad/s
ThomBMT 4:8f67b8327300 49 volatile const double pi = 3.14159265358979;
ThomBMT 3:c8f0fc045505 50 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
ThomBMT 3:c8f0fc045505 51 volatile float referenceVelocity2 = 0.5;
ThomBMT 2:dc9766657afb 52
ThomBMT 4:8f67b8327300 53 enum states{Calibration, Homing, Function};
ThomBMT 4:8f67b8327300 54
ThomBMT 4:8f67b8327300 55 volatile states Active_State = Calibration;
ThomBMT 4:8f67b8327300 56
ThomBMT 5:312186a0604d 57 volatile int counts1 ;
ThomBMT 5:312186a0604d 58 volatile int counts2 ;
ThomBMT 5:312186a0604d 59 volatile float rad_m1;
ThomBMT 5:312186a0604d 60 volatile float rad_m2;
JurrienBos 7:c5c648898881 61 volatile float q_1;
JurrienBos 7:c5c648898881 62 volatile float q_2;
JurrienBos 7:c5c648898881 63 volatile float r_1;
JurrienBos 7:c5c648898881 64 volatile float r_2;
JurrienBos 7:c5c648898881 65 volatile const float r_3 = 2.0; //
JurrienBos 7:c5c648898881 66
JurrienBos 7:c5c648898881 67 volatile float FilterHP_Bi_R;
JurrienBos 7:c5c648898881 68 volatile float Filterabs_Bi_R;
JurrienBos 7:c5c648898881 69 volatile float Filtered_Bi_R;
JurrienBos 7:c5c648898881 70 volatile float FilterHP_Bi_L;
JurrienBos 7:c5c648898881 71 volatile float Filterabs_Bi_L;
JurrienBos 7:c5c648898881 72 volatile float Filtered_Bi_L;
JurrienBos 7:c5c648898881 73 volatile float FilterHP_Tri_R;
JurrienBos 7:c5c648898881 74 volatile float Filterabs_Tri_R;
JurrienBos 7:c5c648898881 75 volatile float Filtered_Tri_R;
JurrienBos 7:c5c648898881 76 volatile float FilterHP_Tri_L;
JurrienBos 7:c5c648898881 77 volatile float Filterabs_Tri_L;
JurrienBos 7:c5c648898881 78 volatile float Filtered_Tri_L;
ThomBMT 4:8f67b8327300 79
JurrienBos 7:c5c648898881 80 void filter()
JurrienBos 7:c5c648898881 81 {
JurrienBos 7:c5c648898881 82 FilterHP_Bi_R = bqc1.step( emg0.read() );
JurrienBos 7:c5c648898881 83 Filterabs_Bi_R = fabs(FilterHP_Bi_R);
JurrienBos 7:c5c648898881 84 Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
JurrienBos 7:c5c648898881 85
JurrienBos 7:c5c648898881 86 FilterHP_Bi_L = bqc3.step( emg1.read() );
JurrienBos 7:c5c648898881 87 Filterabs_Bi_L = fabs(FilterHP_Bi_L);
JurrienBos 7:c5c648898881 88 Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
JurrienBos 7:c5c648898881 89
JurrienBos 7:c5c648898881 90 FilterHP_Tri_R = bqc5.step( emg2.read() );
JurrienBos 7:c5c648898881 91 Filterabs_Tri_R = fabs(FilterHP_Tri_R);
JurrienBos 7:c5c648898881 92 Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
JurrienBos 7:c5c648898881 93
JurrienBos 7:c5c648898881 94 FilterHP_Tri_L = bqc7.step( emg3.read() );
JurrienBos 7:c5c648898881 95 Filterabs_Tri_L = fabs(FilterHP_Tri_L);
JurrienBos 7:c5c648898881 96 Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
JurrienBos 7:c5c648898881 97 }
JurrienBos 7:c5c648898881 98
ThomBMT 3:c8f0fc045505 99 void Encoding()
ThomBMT 3:c8f0fc045505 100 {
ThomBMT 5:312186a0604d 101
ThomBMT 3:c8f0fc045505 102 counts1 = Encoder1.getPulses();
ThomBMT 3:c8f0fc045505 103 counts2 = Encoder2.getPulses();
ThomBMT 5:312186a0604d 104 // Hier gaat iets fout waardoor het 0 wordt!!!
JurrienBos 7:c5c648898881 105 rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
JurrienBos 7:c5c648898881 106 rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
ThomBMT 4:8f67b8327300 107
ThomBMT 4:8f67b8327300 108 // pc.printf("%f & %f ....\n",rad_m1, rad_m2);
ThomBMT 3:c8f0fc045505 109 }
ThomBMT 2:dc9766657afb 110
ThomBMT 2:dc9766657afb 111 void EMG_Read()
ThomBMT 2:dc9766657afb 112 {
ThomBMT 3:c8f0fc045505 113 Bicep_Right = emg0.read();
ThomBMT 3:c8f0fc045505 114 Bicep_Left = emg1.read();
ThomBMT 3:c8f0fc045505 115 Tricep_Right = emg2.read();
ThomBMT 3:c8f0fc045505 116 Tricep_Left = emg3.read();
ThomBMT 2:dc9766657afb 117 }
ThomBMT 2:dc9766657afb 118
ThomBMT 2:dc9766657afb 119 void sample()
ThomBMT 2:dc9766657afb 120 {
ThomBMT 2:dc9766657afb 121
JurrienBos 7:c5c648898881 122 scope.set(0, Filtered_Bi_R );
JurrienBos 7:c5c648898881 123 scope.set(1, Filtered_Bi_L );
JurrienBos 7:c5c648898881 124 scope.set(2, Filtered_Tri_R );
JurrienBos 7:c5c648898881 125 scope.set(3, Filtered_Tri_L );
ThomBMT 2:dc9766657afb 126
ThomBMT 2:dc9766657afb 127 scope.send();
ThomBMT 2:dc9766657afb 128 }
ThomBMT 2:dc9766657afb 129
JurrienBos 1:4bf64d003f3a 130
JurrienBos 1:4bf64d003f3a 131 void velocity1()
JurrienBos 0:4591ba678a39 132 {
ThomBMT 3:c8f0fc045505 133 if (pot1.read()>0.5f)
JurrienBos 0:4591ba678a39 134 {
JurrienBos 0:4591ba678a39 135 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 136 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 137 }
JurrienBos 0:4591ba678a39 138
ThomBMT 5:312186a0604d 139 else if (pot1.read() == 0.5f)
JurrienBos 0:4591ba678a39 140 {
JurrienBos 1:4bf64d003f3a 141 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 142 }
JurrienBos 0:4591ba678a39 143
JurrienBos 0:4591ba678a39 144 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 145 {
JurrienBos 0:4591ba678a39 146 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 147 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 148 }
JurrienBos 0:4591ba678a39 149 }
JurrienBos 0:4591ba678a39 150
JurrienBos 1:4bf64d003f3a 151 void velocity2()
JurrienBos 1:4bf64d003f3a 152 {
JurrienBos 1:4bf64d003f3a 153 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 154 {
JurrienBos 1:4bf64d003f3a 155 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 156 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 157 }
JurrienBos 1:4bf64d003f3a 158
JurrienBos 1:4bf64d003f3a 159 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 160 {
JurrienBos 1:4bf64d003f3a 161 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 162 }
JurrienBos 1:4bf64d003f3a 163
JurrienBos 1:4bf64d003f3a 164 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 165 {
JurrienBos 1:4bf64d003f3a 166 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 167 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 168 }
JurrienBos 1:4bf64d003f3a 169 }
JurrienBos 1:4bf64d003f3a 170
JurrienBos 1:4bf64d003f3a 171 void motor1()
JurrienBos 0:4591ba678a39 172 {
JurrienBos 1:4bf64d003f3a 173 float u = referenceVelocity1;
JurrienBos 0:4591ba678a39 174 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 175 PwmPin1 = fabs(u);
JurrienBos 0:4591ba678a39 176 }
JurrienBos 0:4591ba678a39 177
JurrienBos 1:4bf64d003f3a 178 void motor2()
JurrienBos 1:4bf64d003f3a 179 {
JurrienBos 1:4bf64d003f3a 180 float u = referenceVelocity2;
JurrienBos 1:4bf64d003f3a 181 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 182 PwmPin2 = fabs(u);
JurrienBos 1:4bf64d003f3a 183 }
JurrienBos 0:4591ba678a39 184
ThomBMT 2:dc9766657afb 185 void Printing()
ThomBMT 2:dc9766657afb 186 {
ThomBMT 2:dc9766657afb 187 float v1 = fabs(referenceVelocity1) * maxVelocity;
ThomBMT 2:dc9766657afb 188 float v2 = fabs(referenceVelocity2) * maxVelocity;
ThomBMT 2:dc9766657afb 189
ThomBMT 2:dc9766657afb 190 //eventueel nog counts -> rad/s
ThomBMT 2:dc9766657afb 191
ThomBMT 2:dc9766657afb 192 //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
ThomBMT 5:312186a0604d 193 pc.printf("%i %i \n",counts1,counts2);
ThomBMT 2:dc9766657afb 194 }
ThomBMT 2:dc9766657afb 195
JurrienBos 7:c5c648898881 196 void inverse_kinematics()
JurrienBos 7:c5c648898881 197 {
JurrienBos 7:c5c648898881 198 float JacPs [2][2];
JurrienBos 7:c5c648898881 199 JacPs[0][0] = 2.0;
JurrienBos 7:c5c648898881 200 JacPs[0][1] = 3.0;
JurrienBos 7:c5c648898881 201 JacPs[1][0] = 4.0;
JurrienBos 7:c5c648898881 202 JacPs[1][1] = 5.0;
JurrienBos 7:c5c648898881 203 pc.printf("%f ", JacPs[0][0]);
JurrienBos 7:c5c648898881 204 }
JurrienBos 7:c5c648898881 205
JurrienBos 7:c5c648898881 206
JurrienBos 7:c5c648898881 207
ThomBMT 4:8f67b8327300 208 void StateMachine()
ThomBMT 4:8f67b8327300 209 {
ThomBMT 4:8f67b8327300 210 switch (Active_State)
ThomBMT 4:8f67b8327300 211 {
ThomBMT 4:8f67b8327300 212 case Calibration:
ThomBMT 5:312186a0604d 213 //calibration actions
ThomBMT 5:312186a0604d 214 //pc.printf("Calibration State");
ThomBMT 5:312186a0604d 215 if (Knop1==false)
ThomBMT 5:312186a0604d 216 {
ThomBMT 5:312186a0604d 217 pc.printf("Entering Homing state \n");
ThomBMT 5:312186a0604d 218 Active_State = Homing;
ThomBMT 5:312186a0604d 219 }
JurrienBos 7:c5c648898881 220 filter();
ThomBMT 5:312186a0604d 221 sample();
JurrienBos 7:c5c648898881 222 //EMG_Read();
JurrienBos 7:c5c648898881 223 Encoding();
ThomBMT 5:312186a0604d 224
ThomBMT 4:8f67b8327300 225 break;
ThomBMT 4:8f67b8327300 226
ThomBMT 4:8f67b8327300 227 case Homing:
ThomBMT 5:312186a0604d 228 //Homing actions
ThomBMT 5:312186a0604d 229 //pc.printf("Homing State");
ThomBMT 5:312186a0604d 230 if (Knop2==false)
ThomBMT 5:312186a0604d 231 {
ThomBMT 5:312186a0604d 232 pc.printf("Entering Funtioning State \n");
ThomBMT 5:312186a0604d 233 Active_State = Function;
ThomBMT 5:312186a0604d 234 }
JurrienBos 7:c5c648898881 235 filter();
ThomBMT 5:312186a0604d 236 sample();
JurrienBos 7:c5c648898881 237 //EMG_Read();
ThomBMT 5:312186a0604d 238 Encoding();
ThomBMT 4:8f67b8327300 239 break;
ThomBMT 4:8f67b8327300 240
ThomBMT 4:8f67b8327300 241 case Function:
ThomBMT 5:312186a0604d 242 //pc.printf("Funtioning State");
ThomBMT 5:312186a0604d 243
ThomBMT 5:312186a0604d 244 if (Knop2==false)
ThomBMT 5:312186a0604d 245 {
ThomBMT 5:312186a0604d 246 pc.printf("Re-entering Homing State \n");
ThomBMT 5:312186a0604d 247 Active_State = Homing;
ThomBMT 5:312186a0604d 248 }
ThomBMT 5:312186a0604d 249 else if (Knop1==false)
ThomBMT 5:312186a0604d 250 {
ThomBMT 5:312186a0604d 251 pc.printf("Re-entering Calibration State \n");
ThomBMT 5:312186a0604d 252 Active_State = Calibration;
ThomBMT 5:312186a0604d 253 }
ThomBMT 5:312186a0604d 254
JurrienBos 7:c5c648898881 255 filter();
ThomBMT 5:312186a0604d 256 sample();
JurrienBos 7:c5c648898881 257 //EMG_Read();
ThomBMT 5:312186a0604d 258 Encoding();
ThomBMT 5:312186a0604d 259 velocity1();
ThomBMT 5:312186a0604d 260 velocity2();
ThomBMT 5:312186a0604d 261 motor1();
ThomBMT 5:312186a0604d 262 motor2();
ThomBMT 5:312186a0604d 263
ThomBMT 4:8f67b8327300 264 break;
ThomBMT 4:8f67b8327300 265
ThomBMT 4:8f67b8327300 266 default:
ThomBMT 4:8f67b8327300 267 pc.printf("UNKNOWN COMMAND");
ThomBMT 4:8f67b8327300 268 }
ThomBMT 4:8f67b8327300 269 }
ThomBMT 4:8f67b8327300 270
JurrienBos 0:4591ba678a39 271 int main()
JurrienBos 0:4591ba678a39 272 {
JurrienBos 0:4591ba678a39 273 pc.baud(115200);
ThomBMT 4:8f67b8327300 274 PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
JurrienBos 7:c5c648898881 275 bqc1.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
JurrienBos 7:c5c648898881 276 bqc2.add(&BqLP);
JurrienBos 7:c5c648898881 277 bqc3.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
JurrienBos 7:c5c648898881 278 bqc4.add(&BqLP);
JurrienBos 7:c5c648898881 279 bqc5.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
JurrienBos 7:c5c648898881 280 bqc6.add(&BqLP);
JurrienBos 7:c5c648898881 281 bqc7.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP );
JurrienBos 7:c5c648898881 282 bqc8.add(&BqLP);
ThomBMT 4:8f67b8327300 283 StateTicker.attach(StateMachine, 0.002);
ThomBMT 4:8f67b8327300 284
ThomBMT 5:312186a0604d 285 printTicker.attach(&Printing, 2.0);
ThomBMT 2:dc9766657afb 286
JurrienBos 0:4591ba678a39 287 while(true)
ThomBMT 5:312186a0604d 288 {
JurrienBos 0:4591ba678a39 289 }
JurrienBos 1:4bf64d003f3a 290 }