Project of Biorobotics

Dependencies:   HIDScope MODSERIAL QEI mbed biquadFilter

Fork of TutorialPES by Jurriën Bos

Committer:
JurrienBos
Date:
Mon Oct 15 13:55:45 2018 +0000
Revision:
1:4bf64d003f3a
Parent:
0:4591ba678a39
Child:
2:dc9766657afb
Uitgewerkte potreader die beide motoren aansturen

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"
JurrienBos 0:4591ba678a39 3 MODSERIAL pc(USBTX, USBRX);
JurrienBos 0:4591ba678a39 4 DigitalOut DirectionPin1(D4);
JurrienBos 1:4bf64d003f3a 5 DigitalOut DirectionPin2(D7);
JurrienBos 0:4591ba678a39 6 PwmOut PwmPin1(D5);
JurrienBos 1:4bf64d003f3a 7 PwmOut PwmPin2(D6);
JurrienBos 0:4591ba678a39 8 DigitalIn Knop1(D2);
JurrienBos 0:4591ba678a39 9 AnalogIn pot1 (A5);
JurrienBos 1:4bf64d003f3a 10 AnalogIn pot2 (A4);
JurrienBos 0:4591ba678a39 11
JurrienBos 1:4bf64d003f3a 12 Ticker mycontrollerTicker1;
JurrienBos 1:4bf64d003f3a 13 Ticker mycontrollerTicker2;
JurrienBos 1:4bf64d003f3a 14 Ticker Velo1;
JurrienBos 1:4bf64d003f3a 15 Ticker Velo2;
JurrienBos 0:4591ba678a39 16
JurrienBos 1:4bf64d003f3a 17 //const float maxVelocity=8.4; // in rad/s
JurrienBos 1:4bf64d003f3a 18 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
JurrienBos 1:4bf64d003f3a 19 volatile float referenceVelocity2 = 0.5;
JurrienBos 1:4bf64d003f3a 20
JurrienBos 1:4bf64d003f3a 21 void velocity1()
JurrienBos 0:4591ba678a39 22 {
JurrienBos 1:4bf64d003f3a 23 if ((pot1.read()>0.5f) || (Knop1 == true))//gezeik met die knop doet het niet ff uitzoeken nog
JurrienBos 0:4591ba678a39 24 {
JurrienBos 0:4591ba678a39 25 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 26 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 27 }
JurrienBos 0:4591ba678a39 28
JurrienBos 1:4bf64d003f3a 29 else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f))
JurrienBos 0:4591ba678a39 30 {
JurrienBos 1:4bf64d003f3a 31 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 32 }
JurrienBos 0:4591ba678a39 33
JurrienBos 0:4591ba678a39 34 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 35 {
JurrienBos 0:4591ba678a39 36 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 37 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 38 }
JurrienBos 0:4591ba678a39 39 }
JurrienBos 0:4591ba678a39 40
JurrienBos 1:4bf64d003f3a 41 void velocity2()
JurrienBos 1:4bf64d003f3a 42 {
JurrienBos 1:4bf64d003f3a 43 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 44 {
JurrienBos 1:4bf64d003f3a 45 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 46 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 47 }
JurrienBos 1:4bf64d003f3a 48
JurrienBos 1:4bf64d003f3a 49 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 50 {
JurrienBos 1:4bf64d003f3a 51 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 52 }
JurrienBos 1:4bf64d003f3a 53
JurrienBos 1:4bf64d003f3a 54 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 55 {
JurrienBos 1:4bf64d003f3a 56 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 57 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 58 }
JurrienBos 1:4bf64d003f3a 59 }
JurrienBos 1:4bf64d003f3a 60
JurrienBos 1:4bf64d003f3a 61 void motor1()
JurrienBos 0:4591ba678a39 62 {
JurrienBos 1:4bf64d003f3a 63 float u = referenceVelocity1;
JurrienBos 0:4591ba678a39 64 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 65 PwmPin1 = fabs(u);
JurrienBos 0:4591ba678a39 66 }
JurrienBos 0:4591ba678a39 67
JurrienBos 1:4bf64d003f3a 68 void motor2()
JurrienBos 1:4bf64d003f3a 69 {
JurrienBos 1:4bf64d003f3a 70 float u = referenceVelocity2;
JurrienBos 1:4bf64d003f3a 71 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 72 PwmPin2 = fabs(u);
JurrienBos 1:4bf64d003f3a 73 }
JurrienBos 0:4591ba678a39 74
JurrienBos 0:4591ba678a39 75 int main()
JurrienBos 0:4591ba678a39 76 {
JurrienBos 0:4591ba678a39 77 pc.baud(115200);
JurrienBos 0:4591ba678a39 78 PwmPin1.period_us(120); //60 microseconds pwm period, 16.7 kHz
JurrienBos 1:4bf64d003f3a 79 mycontrollerTicker1.attach(motor1, 0.002);//500Hz
JurrienBos 1:4bf64d003f3a 80 Velo1.attach(velocity1, 0.002);
JurrienBos 1:4bf64d003f3a 81 mycontrollerTicker2.attach(motor2, 0.002);
JurrienBos 1:4bf64d003f3a 82 Velo2.attach(velocity2, 0.002);
JurrienBos 1:4bf64d003f3a 83
JurrienBos 0:4591ba678a39 84 while(true)
JurrienBos 0:4591ba678a39 85 {
JurrienBos 0:4591ba678a39 86 }
JurrienBos 1:4bf64d003f3a 87 }