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@3:2d45e3d0b0f0, 2019-10-04 (annotated)

Committer:
Jellehierck
Date:
Fri Oct 04 12:21:53 2019 +0000
Revision:
3:2d45e3d0b0f0
Parent:
2:6f5f300f0569
Child:
4:28d71b0a29aa
Motor works, potmeter can change motor velocity

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
freek100 0:1843eec2b552 8 DigitalOut ledr(LED_RED);
freek100 0:1843eec2b552 9 DigitalOut ledg(LED_GREEN);
freek100 0:1843eec2b552 10 DigitalOut ledb(LED_BLUE);
freek100 0:1843eec2b552 11 PwmOut led1(D10);
Jellehierck 3:2d45e3d0b0f0 12 DigitalIn button1(D11);
Jellehierck 2:6f5f300f0569 13 AnalogIn potmeter(A0);
freek100 0:1843eec2b552 14 DigitalIn sw(SW2);
freek100 0:1843eec2b552 15 MODSERIAL pc(USBTX, USBRX);
freek100 0:1843eec2b552 16 DigitalIn encA(D13);
freek100 0:1843eec2b552 17 DigitalIn encB(D12);
freek100 0:1843eec2b552 18 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
freek100 0:1843eec2b552 19
Jellehierck 3:2d45e3d0b0f0 20 DigitalOut motor2Direction(D4);
Jellehierck 3:2d45e3d0b0f0 21 FastPWM motor2Power(D5);
Jellehierck 3:2d45e3d0b0f0 22 DigitalOut motor1Direction(D7);
Jellehierck 3:2d45e3d0b0f0 23 FastPWM motor1Power(D6);
Jellehierck 3:2d45e3d0b0f0 24
Jellehierck 3:2d45e3d0b0f0 25 Ticker motorTicker;
Jellehierck 3:2d45e3d0b0f0 26
Jellehierck 3:2d45e3d0b0f0 27 const float maxVelocity = 7.958701; // 76 RPM to rad/s
Jellehierck 3:2d45e3d0b0f0 28 const double PWM_freq = 1e-6;
Jellehierck 2:6f5f300f0569 29
Jellehierck 2:6f5f300f0569 30 int counts; // Encoder counts
Jellehierck 2:6f5f300f0569 31 float degreein; // Angle of DC motor shaft input (before gearbox)
Jellehierck 2:6f5f300f0569 32 float factorin= 360/64; // Convert encoder counts to angle in degrees
Jellehierck 2:6f5f300f0569 33 float degreeout; // Angle of motor shaft output (after gearbox)
Jellehierck 2:6f5f300f0569 34 float gearratio= 131.25; // Gear ratio of gearbox
Jellehierck 2:6f5f300f0569 35
Jellehierck 3:2d45e3d0b0f0 36 float getRefVelocity()
Jellehierck 3:2d45e3d0b0f0 37 {
Jellehierck 3:2d45e3d0b0f0 38 float refVelocity;
Jellehierck 3:2d45e3d0b0f0 39
Jellehierck 3:2d45e3d0b0f0 40 if (button1) {
Jellehierck 3:2d45e3d0b0f0 41 refVelocity = potmeter.read() * maxVelocity;
Jellehierck 3:2d45e3d0b0f0 42 } else {
Jellehierck 3:2d45e3d0b0f0 43 refVelocity = potmeter.read() * maxVelocity * -1;
Jellehierck 3:2d45e3d0b0f0 44 }
Jellehierck 3:2d45e3d0b0f0 45 return refVelocity;
Jellehierck 3:2d45e3d0b0f0 46 }
Jellehierck 3:2d45e3d0b0f0 47
Jellehierck 3:2d45e3d0b0f0 48 void motorControl()
Jellehierck 3:2d45e3d0b0f0 49 {
Jellehierck 3:2d45e3d0b0f0 50 float potValue = potmeter.read();
Jellehierck 3:2d45e3d0b0f0 51 motor1Power.pulsewidth(potValue * PWM_freq);
Jellehierck 3:2d45e3d0b0f0 52 }
Jellehierck 3:2d45e3d0b0f0 53
freek100 0:1843eec2b552 54 int main()
freek100 0:1843eec2b552 55 {
freek100 0:1843eec2b552 56 pc.baud(115200);
freek100 0:1843eec2b552 57 pc.printf("\r\nStarting...\r\n\r\n");
Jellehierck 3:2d45e3d0b0f0 58
Jellehierck 3:2d45e3d0b0f0 59 motor1Power.period(PWM_freq);
Jellehierck 3:2d45e3d0b0f0 60
Jellehierck 3:2d45e3d0b0f0 61 motorTicker.attach(motorControl, 0.01);
Jellehierck 3:2d45e3d0b0f0 62
Jellehierck 3:2d45e3d0b0f0 63 motor1Direction = 0;
Jellehierck 3:2d45e3d0b0f0 64
freek100 0:1843eec2b552 65 while (true) {
Jellehierck 2:6f5f300f0569 66 counts = encoder.getPulses(); // Get encoder pulses
Jellehierck 2:6f5f300f0569 67 degreein = counts*factorin; // Convert encoder data to angle
Jellehierck 2:6f5f300f0569 68 degreeout = degreein/gearratio; // Derived output angle
freek100 1:68f74b2ceb7d 69 //pc.printf("%f \r\n", degreein); //draaigraden van inputgear en zo ook encoder.
Jellehierck 2:6f5f300f0569 70 pc.printf("%f \r\n", degreeout); // Angle of output
Jellehierck 3:2d45e3d0b0f0 71
Jellehierck 2:6f5f300f0569 72 float potValue = potmeter.read(); // Read potmeter
Jellehierck 2:6f5f300f0569 73 pc.printf("Potmeter: %f \r\n", potValue);
Jellehierck 2:6f5f300f0569 74
Jellehierck 3:2d45e3d0b0f0 75 // motor1Power.pulsewidth(potValue * PWM_freq);
Jellehierck 3:2d45e3d0b0f0 76
Jellehierck 2:6f5f300f0569 77 wait(0.5);
freek100 0:1843eec2b552 78 }
freek100 0:1843eec2b552 79 }