Tutorial 5 Assignment --> Niet af
Dependencies: FastPWM MODSERIAL QEI mbed
main.cpp@3:66b76df8eaef, 2018-10-15 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Mon Oct 15 10:11:07 2018 +0000
- Revision:
- 3:66b76df8eaef
- Parent:
- 2:63d809022839
- Child:
- 4:c4c3e284f572
- Child:
- 5:4010889f1c85
JUISTE KABELS; Stroomkabels normaal; Encoder kabels:; - Enc1_A: D12; - Enc1_B: D13; - Enc2_A: D10; - Enc2_B: D11
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MAHCSnijders | 0:fa6d20d54b4f | 1 | #include "mbed.h" |
MAHCSnijders | 1:a1fe2023f69c | 2 | #include "math.h" |
MAHCSnijders | 0:fa6d20d54b4f | 3 | #include "FastPWM.h" |
MAHCSnijders | 0:fa6d20d54b4f | 4 | #include "MODSERIAL.h" |
MAHCSnijders | 1:a1fe2023f69c | 5 | #include "QEI.h" |
MAHCSnijders | 0:fa6d20d54b4f | 6 | |
MAHCSnijders | 0:fa6d20d54b4f | 7 | Ticker motor; // Ticker function |
MAHCSnijders | 1:a1fe2023f69c | 8 | Ticker enc; // Ticker function |
MAHCSnijders | 0:fa6d20d54b4f | 9 | FastPWM pwmpin1(D5); // SPECIFIC PIN (hoeft niet aangesloten te worden) Tells you how fast the motor has to go (later: pwmpin.write will tell you the duty cycle, aka how much voltage the motor gets) |
MAHCSnijders | 0:fa6d20d54b4f | 10 | FastPWM pwmpin2(D6); // SPECIFIC PIN (hoeft niet aangesloten te worden) Tells you how fast the motor has to go (later: pwmpin.write will tell you the duty cycle, aka how much voltage the motor gets) |
MAHCSnijders | 0:fa6d20d54b4f | 11 | DigitalOut directionpin1(D4); // SPECIFIC PIN (hoeft niet aangesloten te worden) Direction value (0-1) that the mbed will give the motor: in which direction the motor must rotate |
MAHCSnijders | 0:fa6d20d54b4f | 12 | DigitalOut directionpin2(D7); // SPECIFIC PIN (hoeft niet aangesloten te worden) Direction value (0-1) that the mbed will give the motor: in which direction the motor must rotate |
MAHCSnijders | 0:fa6d20d54b4f | 13 | AnalogIn potmeter1(A4); // Analoge input van potmeter 1 -> Motor 1 |
MAHCSnijders | 0:fa6d20d54b4f | 14 | AnalogIn potmeter2(A1); // Analoge input van potmeter 2 -> Motor 2 |
MAHCSnijders | 1:a1fe2023f69c | 15 | QEI encoder1(D13, D12, NC, 1, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 1 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation) |
MAHCSnijders | 1:a1fe2023f69c | 16 | QEI encoder2(D11, D10, NC, 1, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 1 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation) |
MAHCSnijders | 1:a1fe2023f69c | 17 | Serial pc(USBTX, USBRX); // Sends encoder counts to PC |
MAHCSnijders | 0:fa6d20d54b4f | 18 | |
MAHCSnijders | 1:a1fe2023f69c | 19 | volatile float counts_motor1; |
MAHCSnijders | 1:a1fe2023f69c | 20 | volatile float counts_motor2; |
MAHCSnijders | 1:a1fe2023f69c | 21 | volatile float angle_motor1; |
MAHCSnijders | 1:a1fe2023f69c | 22 | volatile float angle_motor2; |
MAHCSnijders | 0:fa6d20d54b4f | 23 | volatile float pot1; |
MAHCSnijders | 0:fa6d20d54b4f | 24 | volatile float pot2; |
MAHCSnijders | 0:fa6d20d54b4f | 25 | volatile float pot1_scale; |
MAHCSnijders | 0:fa6d20d54b4f | 26 | volatile float pot2_scale; |
MAHCSnijders | 0:fa6d20d54b4f | 27 | volatile float u1; |
MAHCSnijders | 0:fa6d20d54b4f | 28 | volatile float u2; |
MAHCSnijders | 1:a1fe2023f69c | 29 | const float PI = 3.14159265359; |
MAHCSnijders | 0:fa6d20d54b4f | 30 | |
MAHCSnijders | 0:fa6d20d54b4f | 31 | void motorfunction() |
MAHCSnijders | 0:fa6d20d54b4f | 32 | { pot1 = potmeter1.read(); // reads out value potmeter 1 between 0-1 |
MAHCSnijders | 0:fa6d20d54b4f | 33 | pot1_scale = pot1*2 -1; // scales value potmeter from 0 - 1 to -1 - 1 |
MAHCSnijders | 0:fa6d20d54b4f | 34 | pot2 = potmeter2.read(); // reads out value potmeter 2 between 0-1 |
MAHCSnijders | 0:fa6d20d54b4f | 35 | pot2_scale = pot2*2 -1; // scales value potmeter from 0 - 1 to -1 - 1 |
MAHCSnijders | 0:fa6d20d54b4f | 36 | u1 = pot1_scale; // motor control signal |
MAHCSnijders | 0:fa6d20d54b4f | 37 | u2 = pot2_scale; // motor control signal |
MAHCSnijders | 0:fa6d20d54b4f | 38 | directionpin1 = u1 > 0.0f; // either true or false, determines direction (0 or 1) |
MAHCSnijders | 0:fa6d20d54b4f | 39 | directionpin2 = u2 > 0.0f; // either true or false, determines direction (0 or 1) |
MAHCSnijders | 0:fa6d20d54b4f | 40 | pwmpin1 = fabs(u1); // pwm duty cycle can only be positive, floating point absolute value (if value is >0, there still will be a positive value). |
MAHCSnijders | 0:fa6d20d54b4f | 41 | pwmpin2 = fabs(u2); // pwm duty cycle can only be positive, floating point absolute value (if value is >0, there still will be a positive value). |
MAHCSnijders | 0:fa6d20d54b4f | 42 | } |
MAHCSnijders | 2:63d809022839 | 43 | |
MAHCSnijders | 1:a1fe2023f69c | 44 | void encoder_counter() |
MAHCSnijders | 1:a1fe2023f69c | 45 | { |
MAHCSnijders | 1:a1fe2023f69c | 46 | counts_motor1 = (float)encoder1.getPulses(); // Gets pulses from encoder 1 |
MAHCSnijders | 1:a1fe2023f69c | 47 | counts_motor2 = (float)encoder2.getPulses(); // Gets pulses from encoder 2 |
MAHCSnijders | 1:a1fe2023f69c | 48 | angle_motor1 = (counts_motor1/6533.0f) * 2.0f*PI; // Makes angles in rad |
MAHCSnijders | 1:a1fe2023f69c | 49 | angle_motor2 = (counts_motor2/6533.0f) * 2.0f*PI; // Makes angles in rad |
MAHCSnijders | 1:a1fe2023f69c | 50 | } |
MAHCSnijders | 1:a1fe2023f69c | 51 | |
MAHCSnijders | 0:fa6d20d54b4f | 52 | int main() |
MAHCSnijders | 0:fa6d20d54b4f | 53 | { |
MAHCSnijders | 0:fa6d20d54b4f | 54 | pwmpin1.period_us(60.0); //60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once) |
MAHCSnijders | 0:fa6d20d54b4f | 55 | motor.attach(motorfunction,0.5); |
MAHCSnijders | 1:a1fe2023f69c | 56 | enc.attach(encoder_counter,0.5); |
MAHCSnijders | 1:a1fe2023f69c | 57 | while(1) |
MAHCSnijders | 1:a1fe2023f69c | 58 | { |
MAHCSnijders | 1:a1fe2023f69c | 59 | pc.printf("Pulses encoder1 is: %i \n\r Pulses encoder2 is: %i \n\r angle1 %.2f \n\r angle2 %.2f \n\r", encoder1.getPulses(), encoder2.getPulses(), angle_motor1, angle_motor2); |
MAHCSnijders | 1:a1fe2023f69c | 60 | wait(0.1); |
MAHCSnijders | 1:a1fe2023f69c | 61 | } |
MAHCSnijders | 0:fa6d20d54b4f | 62 | } |