Biorobotics 7 / Mbed 2 deprecated PES_Tutorial_5_Assignments

Tutorial 5 Assignment --> Niet af

Dependencies:   FastPWM MODSERIAL QEI mbed

main.cpp@5:4010889f1c85, 2018-10-15 (annotated)

Committer:
MAHCSnijders
Date:
Mon Oct 15 13:06:14 2018 +0000
Revision:
5:4010889f1c85
Parent:
3:66b76df8eaef 
Versie werkt, encoder wordt uitgelezen + angle berekend (wel achterstand door ticker tussen getPulses en counts_motor)

Who changed what in which revision?

UserRevisionLine numberNew 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 5:4010889f1c85 15 QEI encoder1(D13, D12, NC, 6533, 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 5:4010889f1c85 16 QEI encoder2(D11, D10, NC, 6533, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 2 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 5:4010889f1c85 59 pc.printf("Pulses encoder1 is: %f or %i\n\r angle1 %.2f \n\r Pulses encoder2 is: %f \n\r angle2 %.2f \n\r", counts_motor1, encoder1.getPulses(),angle_motor1, counts_motor2, angle_motor2);
MAHCSnijders 5:4010889f1c85 60 wait(0.5);
MAHCSnijders 1:a1fe2023f69c 61 }
MAHCSnijders 0:fa6d20d54b4f 62 }