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@3:c8f0fc045505, 2018-10-22 (annotated)

Committer:
ThomBMT
Date:
Mon Oct 22 14:51:46 2018 +0000
Revision:
3:c8f0fc045505
Parent:
2:dc9766657afb
Child:
4:8f67b8327300
De encoders werken nog niet en ik snap niet waarom;

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"
ThomBMT 2:dc9766657afb 5
JurrienBos 0:4591ba678a39 6 MODSERIAL pc(USBTX, USBRX);
JurrienBos 0:4591ba678a39 7 DigitalOut DirectionPin1(D4);
JurrienBos 1:4bf64d003f3a 8 DigitalOut DirectionPin2(D7);
JurrienBos 0:4591ba678a39 9 PwmOut PwmPin1(D5);
JurrienBos 1:4bf64d003f3a 10 PwmOut PwmPin2(D6);
JurrienBos 0:4591ba678a39 11 DigitalIn Knop1(D2);
JurrienBos 0:4591ba678a39 12 AnalogIn pot1 (A5);
JurrienBos 1:4bf64d003f3a 13 AnalogIn pot2 (A4);
ThomBMT 2:dc9766657afb 14 AnalogIn emg0( A0 );
ThomBMT 2:dc9766657afb 15 AnalogIn emg1( A1 );
ThomBMT 2:dc9766657afb 16 AnalogIn emg2( A2 );
ThomBMT 2:dc9766657afb 17 AnalogIn emg3( A3 );
JurrienBos 0:4591ba678a39 18
ThomBMT 3:c8f0fc045505 19 DigitalIn a1( D10);
ThomBMT 3:c8f0fc045505 20 DigitalIn b1( D11);
ThomBMT 3:c8f0fc045505 21 DigitalIn a2( D12);
ThomBMT 3:c8f0fc045505 22 DigitalIn b2( D13);
ThomBMT 3:c8f0fc045505 23
ThomBMT 3:c8f0fc045505 24
ThomBMT 3:c8f0fc045505 25 Ticker EncodingTicker;
ThomBMT 2:dc9766657afb 26 Ticker printTicker;
ThomBMT 2:dc9766657afb 27 Ticker mycontrollerTicker1;
ThomBMT 2:dc9766657afb 28 Ticker mycontrollerTicker2;
ThomBMT 2:dc9766657afb 29 Ticker Velo1;
ThomBMT 2:dc9766657afb 30 Ticker Velo2;
ThomBMT 2:dc9766657afb 31 Ticker EMG_Read_Ticker;
ThomBMT 2:dc9766657afb 32 Ticker sample_timer;
ThomBMT 2:dc9766657afb 33 HIDScope scope( 4 );
JurrienBos 0:4591ba678a39 34
ThomBMT 3:c8f0fc045505 35 volatile float Bicep_Right = 0.0;
ThomBMT 3:c8f0fc045505 36 volatile float Bicep_Left = 0.0;
ThomBMT 3:c8f0fc045505 37 volatile float Tricep_Right = 0.0;
ThomBMT 3:c8f0fc045505 38 volatile float Tricep_Left = 0.0;
ThomBMT 3:c8f0fc045505 39 volatile const float maxVelocity = 8.4; // in rad/s
ThomBMT 3:c8f0fc045505 40 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
ThomBMT 3:c8f0fc045505 41 volatile float referenceVelocity2 = 0.5;
ThomBMT 2:dc9766657afb 42
ThomBMT 2:dc9766657afb 43 volatile int counts1;
ThomBMT 2:dc9766657afb 44 volatile int counts2;
ThomBMT 3:c8f0fc045505 45
ThomBMT 3:c8f0fc045505 46 void Encoding()
ThomBMT 3:c8f0fc045505 47 {
ThomBMT 3:c8f0fc045505 48
ThomBMT 3:c8f0fc045505 49 QEI Encoder1(D12,D13,NC,32);
ThomBMT 3:c8f0fc045505 50 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
ThomBMT 3:c8f0fc045505 51 counts1 = Encoder1.getPulses();
ThomBMT 3:c8f0fc045505 52 counts2 = Encoder2.getPulses();
ThomBMT 3:c8f0fc045505 53
ThomBMT 3:c8f0fc045505 54 }
ThomBMT 2:dc9766657afb 55
ThomBMT 2:dc9766657afb 56 void EMG_Read()
ThomBMT 2:dc9766657afb 57 {
ThomBMT 3:c8f0fc045505 58 Bicep_Right = emg0.read();
ThomBMT 3:c8f0fc045505 59 Bicep_Left = emg1.read();
ThomBMT 3:c8f0fc045505 60 Tricep_Right = emg2.read();
ThomBMT 3:c8f0fc045505 61 Tricep_Left = emg3.read();
ThomBMT 2:dc9766657afb 62 }
ThomBMT 2:dc9766657afb 63
ThomBMT 2:dc9766657afb 64 void sample()
ThomBMT 2:dc9766657afb 65 {
ThomBMT 2:dc9766657afb 66
ThomBMT 2:dc9766657afb 67 scope.set(0, emg0.read() );
ThomBMT 2:dc9766657afb 68 scope.set(1, emg1.read() );
ThomBMT 2:dc9766657afb 69 scope.set(2, emg2.read() );
ThomBMT 2:dc9766657afb 70 scope.set(3, emg3.read() );
ThomBMT 2:dc9766657afb 71
ThomBMT 2:dc9766657afb 72 scope.send();
ThomBMT 2:dc9766657afb 73 }
ThomBMT 2:dc9766657afb 74
JurrienBos 1:4bf64d003f3a 75
JurrienBos 1:4bf64d003f3a 76 void velocity1()
JurrienBos 0:4591ba678a39 77 {
ThomBMT 3:c8f0fc045505 78 if (pot1.read()>0.5f)
JurrienBos 0:4591ba678a39 79 {
JurrienBos 0:4591ba678a39 80 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 81 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 82 }
JurrienBos 0:4591ba678a39 83
JurrienBos 1:4bf64d003f3a 84 else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f))
JurrienBos 0:4591ba678a39 85 {
JurrienBos 1:4bf64d003f3a 86 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 87 }
JurrienBos 0:4591ba678a39 88
JurrienBos 0:4591ba678a39 89 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 90 {
JurrienBos 0:4591ba678a39 91 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 92 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 93 }
JurrienBos 0:4591ba678a39 94 }
JurrienBos 0:4591ba678a39 95
JurrienBos 1:4bf64d003f3a 96 void velocity2()
JurrienBos 1:4bf64d003f3a 97 {
JurrienBos 1:4bf64d003f3a 98 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 99 {
JurrienBos 1:4bf64d003f3a 100 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 101 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 102 }
JurrienBos 1:4bf64d003f3a 103
JurrienBos 1:4bf64d003f3a 104 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 105 {
JurrienBos 1:4bf64d003f3a 106 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 107 }
JurrienBos 1:4bf64d003f3a 108
JurrienBos 1:4bf64d003f3a 109 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 110 {
JurrienBos 1:4bf64d003f3a 111 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 112 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 113 }
JurrienBos 1:4bf64d003f3a 114 }
JurrienBos 1:4bf64d003f3a 115
JurrienBos 1:4bf64d003f3a 116 void motor1()
JurrienBos 0:4591ba678a39 117 {
JurrienBos 1:4bf64d003f3a 118 float u = referenceVelocity1;
JurrienBos 0:4591ba678a39 119 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 120 PwmPin1 = fabs(u);
JurrienBos 0:4591ba678a39 121 }
JurrienBos 0:4591ba678a39 122
JurrienBos 1:4bf64d003f3a 123 void motor2()
JurrienBos 1:4bf64d003f3a 124 {
JurrienBos 1:4bf64d003f3a 125 float u = referenceVelocity2;
JurrienBos 1:4bf64d003f3a 126 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 127 PwmPin2 = fabs(u);
JurrienBos 1:4bf64d003f3a 128 }
JurrienBos 0:4591ba678a39 129
ThomBMT 2:dc9766657afb 130 void Printing()
ThomBMT 2:dc9766657afb 131 {
ThomBMT 2:dc9766657afb 132 float v1 = fabs(referenceVelocity1) * maxVelocity;
ThomBMT 2:dc9766657afb 133 float v2 = fabs(referenceVelocity2) * maxVelocity;
ThomBMT 2:dc9766657afb 134
ThomBMT 2:dc9766657afb 135 //eventueel nog counts -> rad/s
ThomBMT 2:dc9766657afb 136
ThomBMT 2:dc9766657afb 137 //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
ThomBMT 3:c8f0fc045505 138 pc.printf("%f ", counts1);
ThomBMT 2:dc9766657afb 139
ThomBMT 2:dc9766657afb 140
ThomBMT 2:dc9766657afb 141 }
ThomBMT 2:dc9766657afb 142
JurrienBos 0:4591ba678a39 143 int main()
JurrienBos 0:4591ba678a39 144 {
JurrienBos 0:4591ba678a39 145 pc.baud(115200);
ThomBMT 3:c8f0fc045505 146 PwmPin1.period_us(40); //60 microseconds pwm period, 16.7 kHz
ThomBMT 2:dc9766657afb 147
ThomBMT 3:c8f0fc045505 148 EncodingTicker.attach(&Encoding, 0.002);
ThomBMT 2:dc9766657afb 149
ThomBMT 2:dc9766657afb 150 sample_timer.attach(&sample, 0.002);
ThomBMT 2:dc9766657afb 151 EMG_Read_Ticker.attach(&EMG_Read, 0.002);
ThomBMT 2:dc9766657afb 152
JurrienBos 1:4bf64d003f3a 153 mycontrollerTicker1.attach(motor1, 0.002);//500Hz
JurrienBos 1:4bf64d003f3a 154 Velo1.attach(velocity1, 0.002);
JurrienBos 1:4bf64d003f3a 155 mycontrollerTicker2.attach(motor2, 0.002);
JurrienBos 1:4bf64d003f3a 156 Velo2.attach(velocity2, 0.002);
JurrienBos 1:4bf64d003f3a 157
ThomBMT 2:dc9766657afb 158 printTicker.attach(&Printing, 2.0);
ThomBMT 2:dc9766657afb 159
JurrienBos 0:4591ba678a39 160 while(true)
JurrienBos 0:4591ba678a39 161 {
JurrienBos 0:4591ba678a39 162 }
JurrienBos 1:4bf64d003f3a 163 }