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

Committer:
ThomBMT
Date:
Mon Oct 22 08:15:41 2018 +0000
Revision:
2:dc9766657afb
Parent:
1:4bf64d003f3a
Child:
3:c8f0fc045505
Counts encoder toegevoegd voor beide motoren

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 2:dc9766657afb 19 Ticker printTicker;
ThomBMT 2:dc9766657afb 20 Ticker mycontrollerTicker1;
ThomBMT 2:dc9766657afb 21 Ticker mycontrollerTicker2;
ThomBMT 2:dc9766657afb 22 Ticker Velo1;
ThomBMT 2:dc9766657afb 23 Ticker Velo2;
ThomBMT 2:dc9766657afb 24 Ticker EMG_Read_Ticker;
ThomBMT 2:dc9766657afb 25 Ticker sample_timer;
ThomBMT 2:dc9766657afb 26 HIDScope scope( 4 );
JurrienBos 0:4591ba678a39 27
ThomBMT 2:dc9766657afb 28 volatile float Bicep_Right = 0.0;
ThomBMT 2:dc9766657afb 29 volatile const float maxVelocity=8.4; // in rad/s
JurrienBos 1:4bf64d003f3a 30 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
JurrienBos 1:4bf64d003f3a 31 volatile float referenceVelocity2 = 0.5;
ThomBMT 2:dc9766657afb 32
ThomBMT 2:dc9766657afb 33 volatile int counts1;
ThomBMT 2:dc9766657afb 34 volatile int counts2;
ThomBMT 2:dc9766657afb 35 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
ThomBMT 2:dc9766657afb 36 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
ThomBMT 2:dc9766657afb 37
ThomBMT 2:dc9766657afb 38 void EMG_Read()
ThomBMT 2:dc9766657afb 39 {
ThomBMT 2:dc9766657afb 40 Bicep_Right = emg0.read();
ThomBMT 2:dc9766657afb 41 }
ThomBMT 2:dc9766657afb 42
ThomBMT 2:dc9766657afb 43 void sample()
ThomBMT 2:dc9766657afb 44 {
ThomBMT 2:dc9766657afb 45
ThomBMT 2:dc9766657afb 46 scope.set(0, emg0.read() );
ThomBMT 2:dc9766657afb 47 scope.set(1, emg1.read() );
ThomBMT 2:dc9766657afb 48 scope.set(2, emg2.read() );
ThomBMT 2:dc9766657afb 49 scope.set(3, emg3.read() );
ThomBMT 2:dc9766657afb 50
ThomBMT 2:dc9766657afb 51 scope.send();
ThomBMT 2:dc9766657afb 52 }
ThomBMT 2:dc9766657afb 53
JurrienBos 1:4bf64d003f3a 54
JurrienBos 1:4bf64d003f3a 55 void velocity1()
JurrienBos 0:4591ba678a39 56 {
JurrienBos 1:4bf64d003f3a 57 if ((pot1.read()>0.5f) || (Knop1 == true))//gezeik met die knop doet het niet ff uitzoeken nog
JurrienBos 0:4591ba678a39 58 {
JurrienBos 0:4591ba678a39 59 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 60 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 61 }
JurrienBos 0:4591ba678a39 62
JurrienBos 1:4bf64d003f3a 63 else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f))
JurrienBos 0:4591ba678a39 64 {
JurrienBos 1:4bf64d003f3a 65 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 66 }
JurrienBos 0:4591ba678a39 67
JurrienBos 0:4591ba678a39 68 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 69 {
JurrienBos 0:4591ba678a39 70 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 71 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 72 }
JurrienBos 0:4591ba678a39 73 }
JurrienBos 0:4591ba678a39 74
JurrienBos 1:4bf64d003f3a 75 void velocity2()
JurrienBos 1:4bf64d003f3a 76 {
JurrienBos 1:4bf64d003f3a 77 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 78 {
JurrienBos 1:4bf64d003f3a 79 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 80 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 81 }
JurrienBos 1:4bf64d003f3a 82
JurrienBos 1:4bf64d003f3a 83 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 84 {
JurrienBos 1:4bf64d003f3a 85 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 86 }
JurrienBos 1:4bf64d003f3a 87
JurrienBos 1:4bf64d003f3a 88 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 89 {
JurrienBos 1:4bf64d003f3a 90 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 91 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 92 }
JurrienBos 1:4bf64d003f3a 93 }
JurrienBos 1:4bf64d003f3a 94
JurrienBos 1:4bf64d003f3a 95 void motor1()
JurrienBos 0:4591ba678a39 96 {
JurrienBos 1:4bf64d003f3a 97 float u = referenceVelocity1;
JurrienBos 0:4591ba678a39 98 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 99 PwmPin1 = fabs(u);
JurrienBos 0:4591ba678a39 100 }
JurrienBos 0:4591ba678a39 101
JurrienBos 1:4bf64d003f3a 102 void motor2()
JurrienBos 1:4bf64d003f3a 103 {
JurrienBos 1:4bf64d003f3a 104 float u = referenceVelocity2;
JurrienBos 1:4bf64d003f3a 105 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 106 PwmPin2 = fabs(u);
JurrienBos 1:4bf64d003f3a 107 }
JurrienBos 0:4591ba678a39 108
ThomBMT 2:dc9766657afb 109 void Printing()
ThomBMT 2:dc9766657afb 110 {
ThomBMT 2:dc9766657afb 111 float v1 = fabs(referenceVelocity1) * maxVelocity;
ThomBMT 2:dc9766657afb 112 float v2 = fabs(referenceVelocity2) * maxVelocity;
ThomBMT 2:dc9766657afb 113
ThomBMT 2:dc9766657afb 114 //eventueel nog counts -> rad/s
ThomBMT 2:dc9766657afb 115
ThomBMT 2:dc9766657afb 116 //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
ThomBMT 2:dc9766657afb 117 pc.printf("%i counts van m1, %i counts van m2", counts1, counts2);
ThomBMT 2:dc9766657afb 118
ThomBMT 2:dc9766657afb 119
ThomBMT 2:dc9766657afb 120 }
ThomBMT 2:dc9766657afb 121
JurrienBos 0:4591ba678a39 122 int main()
JurrienBos 0:4591ba678a39 123 {
JurrienBos 0:4591ba678a39 124 pc.baud(115200);
JurrienBos 0:4591ba678a39 125 PwmPin1.period_us(120); //60 microseconds pwm period, 16.7 kHz
ThomBMT 2:dc9766657afb 126
ThomBMT 2:dc9766657afb 127 counts1 = Encoder1.getPulses();
ThomBMT 2:dc9766657afb 128 counts2 = Encoder2.getPulses();
ThomBMT 2:dc9766657afb 129
ThomBMT 2:dc9766657afb 130 sample_timer.attach(&sample, 0.002);
ThomBMT 2:dc9766657afb 131 EMG_Read_Ticker.attach(&EMG_Read, 0.002);
ThomBMT 2:dc9766657afb 132
JurrienBos 1:4bf64d003f3a 133 mycontrollerTicker1.attach(motor1, 0.002);//500Hz
JurrienBos 1:4bf64d003f3a 134 Velo1.attach(velocity1, 0.002);
JurrienBos 1:4bf64d003f3a 135 mycontrollerTicker2.attach(motor2, 0.002);
JurrienBos 1:4bf64d003f3a 136 Velo2.attach(velocity2, 0.002);
JurrienBos 1:4bf64d003f3a 137
ThomBMT 2:dc9766657afb 138 printTicker.attach(&Printing, 2.0);
ThomBMT 2:dc9766657afb 139
JurrienBos 0:4591ba678a39 140 while(true)
JurrienBos 0:4591ba678a39 141 {
JurrienBos 0:4591ba678a39 142 }
JurrienBos 1:4bf64d003f3a 143 }