Brighton de Jong / Mbed 2 deprecated Turning_Motor_V6

Facking kut filter werkt EINDELIJK !311!!111!!1!!!!!!!!!!!!!!!!!!!

Dependencies:   HIDScope MODSERIAL Matrix QEI biquadFilter mbed

Fork of Filter by Jurriën Bos

main.cpp@8:b6b09226a421, 2018-10-31 (annotated)

Committer:
Brighton_the_robot
Date:
Wed Oct 31 13:56:41 2018 +0000
Revision:
8:b6b09226a421
Parent:
7:c5c648898881 
Facking kut filter werkt EINDELIJK1111!!!11!!! TERING

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