![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
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?
User | Revision | Line number | New 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 | } |