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@5:54ce02ad7a50, 2019-10-04 (annotated)

Committer:
Jellehierck
Date:
Fri Oct 04 13:24:06 2019 +0000
Revision:
5:54ce02ad7a50
Parent:
4:28d71b0a29aa
Child:
6:c352578a95b3
Motor control buttons added: direction and toggle

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