M9 Biorobotics Group 8 / Mbed 2 deprecated Encoder-Motor-Control-EMG

Afgesplitste versie van motor control waarbij we ook iets met EMG gaan doen

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@4:28d71b0a29aa, 2019-10-04 (annotated)

Committer:
Jellehierck
Date:
Fri Oct 04 12:58:04 2019 +0000
Revision:
4:28d71b0a29aa
Parent:
3:2d45e3d0b0f0
Child:
5:54ce02ad7a50
Encoder angle and angular velocity can be read

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 4:28d71b0a29aa 26 Ticker encoderTicker;
Jellehierck 3:2d45e3d0b0f0 27
Jellehierck 3:2d45e3d0b0f0 28 const float maxVelocity = 7.958701; // 76 RPM to rad/s
Jellehierck 4:28d71b0a29aa 29 const float PWM_period = 1e-6;
Jellehierck 2:6f5f300f0569 30
Jellehierck 4:28d71b0a29aa 31 volatile int counts; // Encoder counts
Jellehierck 4:28d71b0a29aa 32 volatile int countsPrev = 0;
Jellehierck 4:28d71b0a29aa 33 volatile int deltaCounts;
Jellehierck 4:28d71b0a29aa 34
Jellehierck 4:28d71b0a29aa 35 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
Jellehierck 4:28d71b0a29aa 36 float gearratio = 131.25; // Gear ratio of gearbox
Jellehierck 2:6f5f300f0569 37
Jellehierck 3:2d45e3d0b0f0 38 float getRefVelocity()
Jellehierck 3:2d45e3d0b0f0 39 {
Jellehierck 3:2d45e3d0b0f0 40 float refVelocity;
Jellehierck 3:2d45e3d0b0f0 41
Jellehierck 3:2d45e3d0b0f0 42 if (button1) {
Jellehierck 3:2d45e3d0b0f0 43 refVelocity = potmeter.read() * maxVelocity;
Jellehierck 3:2d45e3d0b0f0 44 } else {
Jellehierck 3:2d45e3d0b0f0 45 refVelocity = potmeter.read() * maxVelocity * -1;
Jellehierck 3:2d45e3d0b0f0 46 }
Jellehierck 3:2d45e3d0b0f0 47 return refVelocity;
Jellehierck 3:2d45e3d0b0f0 48 }
Jellehierck 3:2d45e3d0b0f0 49
Jellehierck 3:2d45e3d0b0f0 50 void motorControl()
Jellehierck 3:2d45e3d0b0f0 51 {
Jellehierck 3:2d45e3d0b0f0 52 float potValue = potmeter.read();
Jellehierck 4:28d71b0a29aa 53 motor1Power.pulsewidth(potValue * PWM_period);
Jellehierck 4:28d71b0a29aa 54 }
Jellehierck 4:28d71b0a29aa 55
Jellehierck 4:28d71b0a29aa 56 void readEncoder()
Jellehierck 4:28d71b0a29aa 57 {
Jellehierck 4:28d71b0a29aa 58 counts = encoder.getPulses();
Jellehierck 4:28d71b0a29aa 59 deltaCounts = counts - countsPrev;
Jellehierck 4:28d71b0a29aa 60
Jellehierck 4:28d71b0a29aa 61 countsPrev = counts;
Jellehierck 3:2d45e3d0b0f0 62 }
Jellehierck 3:2d45e3d0b0f0 63
freek100 0:1843eec2b552 64 int main()
freek100 0:1843eec2b552 65 {
freek100 0:1843eec2b552 66 pc.baud(115200);
freek100 0:1843eec2b552 67 pc.printf("\r\nStarting...\r\n\r\n");
Jellehierck 3:2d45e3d0b0f0 68
Jellehierck 4:28d71b0a29aa 69 motor1Power.period(PWM_period);
Jellehierck 3:2d45e3d0b0f0 70
Jellehierck 3:2d45e3d0b0f0 71 motorTicker.attach(motorControl, 0.01);
Jellehierck 4:28d71b0a29aa 72
Jellehierck 4:28d71b0a29aa 73 float T_encoder = 0.1;
Jellehierck 4:28d71b0a29aa 74 encoderTicker.attach(readEncoder, T_encoder);
Jellehierck 3:2d45e3d0b0f0 75
Jellehierck 3:2d45e3d0b0f0 76 motor1Direction = 0;
Jellehierck 3:2d45e3d0b0f0 77
freek100 0:1843eec2b552 78 while (true) {
Jellehierck 3:2d45e3d0b0f0 79
Jellehierck 2:6f5f300f0569 80 float potValue = potmeter.read(); // Read potmeter
Jellehierck 4:28d71b0a29aa 81
Jellehierck 4:28d71b0a29aa 82 float angle = counts * factorin / gearratio; // Angle of motor shaft in rad
Jellehierck 4:28d71b0a29aa 83 float omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s
Jellehierck 2:6f5f300f0569 84
Jellehierck 4:28d71b0a29aa 85 pc.printf("Potmeter: %f \r\n", potValue);
Jellehierck 4:28d71b0a29aa 86 pc.printf("Counts: %i DeltaCounts: %i\r\n", counts, deltaCounts);
Jellehierck 4:28d71b0a29aa 87 pc.printf("Angle: %f Omega: %f\r\n", angle, omega);
Jellehierck 4:28d71b0a29aa 88
Jellehierck 2:6f5f300f0569 89 wait(0.5);
freek100 0:1843eec2b552 90 }
freek100 0:1843eec2b552 91 }