Jorn Dokter / Mbed 2 deprecated TEB_branch2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

motorAndSensorControl.cpp@18:266f1ffdc9c4, 2019-10-03 (annotated)

Committer:
JordanO
Date:
Thu Oct 03 17:50:52 2019 +0000
Revision:
18:266f1ffdc9c4
Child:
19:07706535ff7b
First version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JordanO 18:266f1ffdc9c4 1 //voltage in PWM (between 0 and 1)
JordanO 18:266f1ffdc9c4 2 //period in seconds
JordanO 18:266f1ffdc9c4 3 //dt the time between measurements, i.o.w. Ticker timing
JordanO 18:266f1ffdc9c4 4 //directionMotor either 1 or 0
JordanO 18:266f1ffdc9c4 5
JordanO 18:266f1ffdc9c4 6 #include "FastPWM.h"
JordanO 18:266f1ffdc9c4 7 #include "QEI.h"
JordanO 18:266f1ffdc9c4 8
JordanO 18:266f1ffdc9c4 9 //Objects
JordanO 18:266f1ffdc9c4 10 //Motors
JordanO 18:266f1ffdc9c4 11 FastPWM motor1(D6);
JordanO 18:266f1ffdc9c4 12 DigitalOut motor1Dir(D7);
JordanO 18:266f1ffdc9c4 13 FastPWM motor2(D5);
JordanO 18:266f1ffdc9c4 14 DigitalOut motor2Dir(D4);
JordanO 18:266f1ffdc9c4 15 //Encoders
JordanO 18:266f1ffdc9c4 16 QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 18:266f1ffdc9c4 17 QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 18:266f1ffdc9c4 18 //Variables
JordanO 18:266f1ffdc9c4 19 static int countsMotor1[2];
JordanO 18:266f1ffdc9c4 20 static int countsMotor2[2];
JordanO 18:266f1ffdc9c4 21 static double velocityMotor1;
JordanO 18:266f1ffdc9c4 22 static double velocityMotor2;
JordanO 18:266f1ffdc9c4 23 //Constants
JordanO 18:266f1ffdc9c4 24 const int uniqueMeasurementPoints = 4200; //From Canvas X4 = 8400, X1 = 2100, so X2 = 4200 https://canvas.utwente.nl/courses/4023/pages/project-materials?module_item_id=91422
JordanO 18:266f1ffdc9c4 25 const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
JordanO 18:266f1ffdc9c4 26
JordanO 18:266f1ffdc9c4 27 double motorAndEncoder(voltage1, periodMotor1, voltage2, periodMotor2, directionMotor1, directionMotor2, dt)
JordanO 18:266f1ffdc9c4 28 {
JordanO 18:266f1ffdc9c4 29 //Set motors
JordanO 18:266f1ffdc9c4 30 motor1.period(periodMotor1);
JordanO 18:266f1ffdc9c4 31 motor1.write(voltage1);
JordanO 18:266f1ffdc9c4 32
JordanO 18:266f1ffdc9c4 33 motor2.period(periodMotor2);
JordanO 18:266f1ffdc9c4 34 motor2.write(voltage2);
JordanO 18:266f1ffdc9c4 35 //Read encoders
JordanO 18:266f1ffdc9c4 36 //Counts
JordanO 18:266f1ffdc9c4 37 countsMotor1[0] = countsMotor1[1];
JordanO 18:266f1ffdc9c4 38 countsMotor2[0] = countsMotor2[1];
JordanO 18:266f1ffdc9c4 39 countsMotor1[1] = encoderMotor1.gePulses();
JordanO 18:266f1ffdc9c4 40 countsMotor2[1] = encoderMotor2.gePulses();
JordanO 18:266f1ffdc9c4 41 //Velocity calculation
JordanO 18:266f1ffdc9c4 42 velocityMotor1 = ((countsMotor1[1]-countsMotor1[2])/countsToRadians)/dt; //rad/s
JordanO 18:266f1ffdc9c4 43 velocityMotor2 = ((countsMotor2[1]-countsMotor2[2])/countsToRadians)/dt; //rad/s
JordanO 18:266f1ffdc9c4 44
JordanO 18:266f1ffdc9c4 45 return (velocityMotor1, velocityMotor2, countsMotor1[1], countsMotor2[0]);
JordanO 18:266f1ffdc9c4 46 }