M9 Biorobotics Group 8 / Mbed 2 deprecated MotorControlv2

Motor control

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@0:e4858e2df9c7, 2019-10-11 (annotated)

Committer:
freek100
Date:
Fri Oct 11 09:40:16 2019 +0000
Revision:
0:e4858e2df9c7
Child:
1:08e8cc33fcae
Simpel motor aansturing met potmeter (-1<u<1)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
freek100 0:e4858e2df9c7 1 #include "mbed.h"
freek100 0:e4858e2df9c7 2 //#include "HIDScope.h"
freek100 0:e4858e2df9c7 3 #include "QEI.h"
freek100 0:e4858e2df9c7 4 #include "MODSERIAL.h"
freek100 0:e4858e2df9c7 5 //#include "BiQuad.h"
freek100 0:e4858e2df9c7 6 #include "FastPWM.h"
freek100 0:e4858e2df9c7 7
freek100 0:e4858e2df9c7 8 // Button and potmeter control
freek100 0:e4858e2df9c7 9 InterruptIn button1(D11);
freek100 0:e4858e2df9c7 10 InterruptIn button2(D10);
freek100 0:e4858e2df9c7 11 AnalogIn potmeter(A0);
freek100 0:e4858e2df9c7 12
freek100 0:e4858e2df9c7 13 // Encoder
freek100 0:e4858e2df9c7 14 DigitalIn encA(D13);
freek100 0:e4858e2df9c7 15 DigitalIn encB(D12);
freek100 0:e4858e2df9c7 16 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
freek100 0:e4858e2df9c7 17
freek100 0:e4858e2df9c7 18 // Motor
freek100 0:e4858e2df9c7 19 DigitalOut motor2Direction(D4);
freek100 0:e4858e2df9c7 20 FastPWM motor2Power(D5);
freek100 0:e4858e2df9c7 21 DigitalOut motor1Direction(D7);
freek100 0:e4858e2df9c7 22 FastPWM motor1Power(D6);
freek100 0:e4858e2df9c7 23
freek100 0:e4858e2df9c7 24 //Motorcontrol
freek100 0:e4858e2df9c7 25 bool motordir;
freek100 0:e4858e2df9c7 26 double motorpwm;
freek100 0:e4858e2df9c7 27 double u;
freek100 0:e4858e2df9c7 28 double potValue;
freek100 0:e4858e2df9c7 29
freek100 0:e4858e2df9c7 30 // PC connection
freek100 0:e4858e2df9c7 31 MODSERIAL pc(USBTX, USBRX);
freek100 0:e4858e2df9c7 32
freek100 0:e4858e2df9c7 33 // Intializing tickers
freek100 0:e4858e2df9c7 34 Ticker motorTicker;
freek100 0:e4858e2df9c7 35 Ticker controlTicker;
freek100 0:e4858e2df9c7 36 Ticker directionTicker;
freek100 0:e4858e2df9c7 37
freek100 0:e4858e2df9c7 38 const float PWM_period = 1e-6;
freek100 0:e4858e2df9c7 39
freek100 0:e4858e2df9c7 40 volatile int counts; // Encoder counts
freek100 0:e4858e2df9c7 41 volatile int countsPrev = 0;
freek100 0:e4858e2df9c7 42 volatile int deltaCounts;
freek100 0:e4858e2df9c7 43
freek100 0:e4858e2df9c7 44 // motor1Direction = 1;
freek100 0:e4858e2df9c7 45 volatile int motor1Toggle = 1;
freek100 0:e4858e2df9c7 46
freek100 0:e4858e2df9c7 47 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
freek100 0:e4858e2df9c7 48 float gearratio = 131.25; // Gear ratio of gearbox
freek100 0:e4858e2df9c7 49
freek100 0:e4858e2df9c7 50 void motorControl()
freek100 0:e4858e2df9c7 51 {
freek100 0:e4858e2df9c7 52 potValue= potmeter.read();
freek100 0:e4858e2df9c7 53 u= (potValue*2)-1;
freek100 0:e4858e2df9c7 54 motorpwm= abs(u);
freek100 0:e4858e2df9c7 55 if (u<0){
freek100 0:e4858e2df9c7 56 motordir= 0;}
freek100 0:e4858e2df9c7 57 else {
freek100 0:e4858e2df9c7 58 motordir= 1;}
freek100 0:e4858e2df9c7 59 motor1Power.pulsewidth(motorpwm * PWM_period * motor1Toggle);
freek100 0:e4858e2df9c7 60 motor1Direction= motordir;
freek100 0:e4858e2df9c7 61 }
freek100 0:e4858e2df9c7 62
freek100 0:e4858e2df9c7 63
freek100 0:e4858e2df9c7 64 int main()
freek100 0:e4858e2df9c7 65 {
freek100 0:e4858e2df9c7 66 pc.baud(115200);
freek100 0:e4858e2df9c7 67 pc.printf("\r\nStarting...\r\n\r\n");
freek100 0:e4858e2df9c7 68
freek100 0:e4858e2df9c7 69 motor1Power.period(PWM_period);
freek100 0:e4858e2df9c7 70 motorTicker.attach(motorControl, 0.01);
freek100 0:e4858e2df9c7 71
freek100 0:e4858e2df9c7 72 while (true) {
freek100 0:e4858e2df9c7 73 float potValue = potmeter.read(); // Read potmeter
freek100 0:e4858e2df9c7 74
freek100 0:e4858e2df9c7 75 pc.printf("Potmeter: %f \r\n", potValue);
freek100 0:e4858e2df9c7 76 wait(0.5);
freek100 0:e4858e2df9c7 77 }
freek100 0:e4858e2df9c7 78 }