Brighton de Jong / Mbed 2 deprecated Turning_Motor_V1000

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

Dependencies:   HIDScope MODSERIAL Matrix QEI biquadFilter mbed

Fork of Turning_Motor_V6 by Brighton de Jong

main.cpp@9:355babe55589, 2018-10-31 (annotated)

Committer:
Brighton_the_robot
Date:
Wed Oct 31 19:53:52 2018 +0000
Revision:
9:355babe55589
Parent:
8:b6b09226a421 
Inclusief filter

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