M9 Biorobotics Group 8 / Mbed 2 deprecated Encoder + Motor Control

Poah als je je encoder in je motor wil aflezen en de beweging van je motor wilt aanpassen is dit programma echt voor jou!

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@6:c352578a95b3, 2019-10-07 (annotated)

Committer:
Jellehierck
Date:
Mon Oct 07 07:28:56 2019 +0000
Revision:
6:c352578a95b3
Parent:
5:54ce02ad7a50 
Cleaned up code, removed unnecessary bits

Who changed what in which revision?

UserRevisionLine numberNew contents of line
freek100 0:1843eec2b552 1 #include "mbed.h"
freek100 0:1843eec2b552 2 //#include "HIDScope.h"
freek100 0:1843eec2b552 3 #include "QEI.h"
freek100 0:1843eec2b552 4 #include "MODSERIAL.h"
freek100 0:1843eec2b552 5 //#include "BiQuad.h"
Jellehierck 2:6f5f300f0569 6 #include "FastPWM.h"
freek100 0:1843eec2b552 7
Jellehierck 6:c352578a95b3 8 // Button and potmeter control
Jellehierck 5:54ce02ad7a50 9 InterruptIn button1(D11);
Jellehierck 5:54ce02ad7a50 10 InterruptIn button2(D10);
Jellehierck 2:6f5f300f0569 11 AnalogIn potmeter(A0);
Jellehierck 6:c352578a95b3 12
Jellehierck 6:c352578a95b3 13 // Encoder
freek100 0:1843eec2b552 14 DigitalIn encA(D13);
freek100 0:1843eec2b552 15 DigitalIn encB(D12);
freek100 0:1843eec2b552 16 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
freek100 0:1843eec2b552 17
Jellehierck 6:c352578a95b3 18 // Motor
Jellehierck 3:2d45e3d0b0f0 19 DigitalOut motor2Direction(D4);
Jellehierck 3:2d45e3d0b0f0 20 FastPWM motor2Power(D5);
Jellehierck 3:2d45e3d0b0f0 21 DigitalOut motor1Direction(D7);
Jellehierck 3:2d45e3d0b0f0 22 FastPWM motor1Power(D6);
Jellehierck 3:2d45e3d0b0f0 23
Jellehierck 6:c352578a95b3 24 // PC connection
Jellehierck 6:c352578a95b3 25 MODSERIAL pc(USBTX, USBRX);
Jellehierck 6:c352578a95b3 26
Jellehierck 6:c352578a95b3 27 // Intializing tickers
Jellehierck 3:2d45e3d0b0f0 28 Ticker motorTicker;
Jellehierck 4:28d71b0a29aa 29 Ticker encoderTicker;
Jellehierck 3:2d45e3d0b0f0 30
Jellehierck 5:54ce02ad7a50 31 const float maxVelocity = 7.958701 * 0.75; // 76 RPM to rad/s at 75% efficiency due to 9V instead of 12V
Jellehierck 4:28d71b0a29aa 32 const float PWM_period = 1e-6;
Jellehierck 2:6f5f300f0569 33
Jellehierck 4:28d71b0a29aa 34 volatile int counts; // Encoder counts
Jellehierck 4:28d71b0a29aa 35 volatile int countsPrev = 0;
Jellehierck 4:28d71b0a29aa 36 volatile int deltaCounts;
Jellehierck 4:28d71b0a29aa 37
Jellehierck 5:54ce02ad7a50 38 // motor1Direction = 1;
Jellehierck 5:54ce02ad7a50 39 volatile int motor1Toggle = 1;
Jellehierck 5:54ce02ad7a50 40
Jellehierck 4:28d71b0a29aa 41 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
Jellehierck 4:28d71b0a29aa 42 float gearratio = 131.25; // Gear ratio of gearbox
Jellehierck 2:6f5f300f0569 43
Jellehierck 3:2d45e3d0b0f0 44 float getRefVelocity()
Jellehierck 3:2d45e3d0b0f0 45 {
Jellehierck 3:2d45e3d0b0f0 46 float refVelocity;
Jellehierck 3:2d45e3d0b0f0 47
Jellehierck 3:2d45e3d0b0f0 48 if (button1) {
Jellehierck 3:2d45e3d0b0f0 49 refVelocity = potmeter.read() * maxVelocity;
Jellehierck 3:2d45e3d0b0f0 50 } else {
Jellehierck 3:2d45e3d0b0f0 51 refVelocity = potmeter.read() * maxVelocity * -1;
Jellehierck 3:2d45e3d0b0f0 52 }
Jellehierck 3:2d45e3d0b0f0 53 return refVelocity;
Jellehierck 3:2d45e3d0b0f0 54 }
Jellehierck 3:2d45e3d0b0f0 55
Jellehierck 3:2d45e3d0b0f0 56 void motorControl()
Jellehierck 3:2d45e3d0b0f0 57 {
Jellehierck 3:2d45e3d0b0f0 58 float potValue = potmeter.read();
Jellehierck 5:54ce02ad7a50 59 motor1Power.pulsewidth(potValue * PWM_period * motor1Toggle);
Jellehierck 4:28d71b0a29aa 60 }
Jellehierck 4:28d71b0a29aa 61
Jellehierck 4:28d71b0a29aa 62 void readEncoder()
Jellehierck 4:28d71b0a29aa 63 {
Jellehierck 4:28d71b0a29aa 64 counts = encoder.getPulses();
Jellehierck 4:28d71b0a29aa 65 deltaCounts = counts - countsPrev;
Jellehierck 4:28d71b0a29aa 66
Jellehierck 4:28d71b0a29aa 67 countsPrev = counts;
Jellehierck 3:2d45e3d0b0f0 68 }
Jellehierck 3:2d45e3d0b0f0 69
Jellehierck 5:54ce02ad7a50 70 void flipDirection()
Jellehierck 5:54ce02ad7a50 71 {
Jellehierck 5:54ce02ad7a50 72 motor1Direction = !motor1Direction;
Jellehierck 5:54ce02ad7a50 73 }
Jellehierck 5:54ce02ad7a50 74
Jellehierck 5:54ce02ad7a50 75 void toggleMotor()
Jellehierck 5:54ce02ad7a50 76 {
Jellehierck 5:54ce02ad7a50 77 motor1Toggle = !motor1Toggle;
Jellehierck 5:54ce02ad7a50 78 }
Jellehierck 5:54ce02ad7a50 79
freek100 0:1843eec2b552 80 int main()
freek100 0:1843eec2b552 81 {
freek100 0:1843eec2b552 82 pc.baud(115200);
freek100 0:1843eec2b552 83 pc.printf("\r\nStarting...\r\n\r\n");
Jellehierck 3:2d45e3d0b0f0 84
Jellehierck 4:28d71b0a29aa 85 motor1Power.period(PWM_period);
Jellehierck 3:2d45e3d0b0f0 86
Jellehierck 3:2d45e3d0b0f0 87 motorTicker.attach(motorControl, 0.01);
Jellehierck 5:54ce02ad7a50 88
Jellehierck 4:28d71b0a29aa 89 float T_encoder = 0.1;
Jellehierck 4:28d71b0a29aa 90 encoderTicker.attach(readEncoder, T_encoder);
Jellehierck 3:2d45e3d0b0f0 91
Jellehierck 5:54ce02ad7a50 92 button1.fall(&flipDirection);
Jellehierck 5:54ce02ad7a50 93 button2.fall(&toggleMotor);
Jellehierck 3:2d45e3d0b0f0 94
freek100 0:1843eec2b552 95 while (true) {
Jellehierck 2:6f5f300f0569 96 float potValue = potmeter.read(); // Read potmeter
Jellehierck 4:28d71b0a29aa 97 float angle = counts * factorin / gearratio; // Angle of motor shaft in rad
Jellehierck 4:28d71b0a29aa 98 float omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s
Jellehierck 5:54ce02ad7a50 99
Jellehierck 4:28d71b0a29aa 100 pc.printf("Potmeter: %f \r\n", potValue);
Jellehierck 4:28d71b0a29aa 101 pc.printf("Counts: %i DeltaCounts: %i\r\n", counts, deltaCounts);
Jellehierck 4:28d71b0a29aa 102 pc.printf("Angle: %f Omega: %f\r\n", angle, omega);
Jellehierck 5:54ce02ad7a50 103
Jellehierck 2:6f5f300f0569 104 wait(0.5);
freek100 0:1843eec2b552 105 }
freek100 0:1843eec2b552 106 }