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@21:0a37e3280bbd, 2019-10-04 (annotated)

Committer:
JordanO
Date:
Fri Oct 04 10:07:27 2019 +0000
Revision:
21:0a37e3280bbd
Parent:
19:07706535ff7b
Child:
22:4197629a300f
Compiled

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JordanO 19:07706535ff7b 1 //voltage in PWM (between -1 and 1)
JordanO 18:266f1ffdc9c4 2 //period in seconds
JordanO 18:266f1ffdc9c4 3 //dt the time between measurements, i.o.w. Ticker timing
JordanO 21:0a37e3280bbd 4 #include "mbed.h"
JordanO 18:266f1ffdc9c4 5 #include "FastPWM.h"
JordanO 18:266f1ffdc9c4 6 #include "QEI.h"
JordanO 18:266f1ffdc9c4 7
JordanO 18:266f1ffdc9c4 8 //Objects
JordanO 18:266f1ffdc9c4 9 //Motors
JordanO 19:07706535ff7b 10 FastPWM motor1(D6); //Motor1 PWM output pin
JordanO 19:07706535ff7b 11 DigitalOut motor1Dir(D7); //Motor1 directional pin
JordanO 19:07706535ff7b 12 FastPWM motor2(D5); //Motor2 PWM output pin
JordanO 19:07706535ff7b 13 DigitalOut motor2Dir(D4); //Motor2 directional pin
JordanO 18:266f1ffdc9c4 14 //Encoders
JordanO 18:266f1ffdc9c4 15 QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 18:266f1ffdc9c4 16 QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 18:266f1ffdc9c4 17 //Variables
JordanO 19:07706535ff7b 18 static int countsMotor1[2]; //Array to store motor counts for i and i-1
JordanO 18:266f1ffdc9c4 19 static int countsMotor2[2];
JordanO 21:0a37e3280bbd 20 static float velocityMotor1;
JordanO 21:0a37e3280bbd 21 static float velocityMotor2;
JordanO 18:266f1ffdc9c4 22 //Constants
JordanO 18:266f1ffdc9c4 23 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 21:0a37e3280bbd 24 const double PI = 3.14159265358979;
JordanO 18:266f1ffdc9c4 25 const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
JordanO 18:266f1ffdc9c4 26
JordanO 21:0a37e3280bbd 27 float motorAndEncoder(float voltage1, float periodMotor1, float voltage2, float periodMotor2, float dt)
JordanO 18:266f1ffdc9c4 28 {
JordanO 18:266f1ffdc9c4 29 //Set motors
JordanO 19:07706535ff7b 30 //Direction
JordanO 19:07706535ff7b 31 if (voltage1<0)
JordanO 19:07706535ff7b 32 {
JordanO 21:0a37e3280bbd 33 voltage1 *= -1; //Shorthand for *-1
JordanO 19:07706535ff7b 34 motor1Dir.write(1); //negative direction
JordanO 19:07706535ff7b 35 }
JordanO 19:07706535ff7b 36 else
JordanO 19:07706535ff7b 37 {
JordanO 19:07706535ff7b 38 motor1Dir.write(0); //positive direction
JordanO 19:07706535ff7b 39 }
JordanO 19:07706535ff7b 40 if (voltage2<0)
JordanO 19:07706535ff7b 41 {
JordanO 21:0a37e3280bbd 42 voltage2 *= -1; //Shorthand for *-1
JordanO 19:07706535ff7b 43 motor2Dir.write(1); //negative direction
JordanO 19:07706535ff7b 44 }
JordanO 19:07706535ff7b 45 else
JordanO 19:07706535ff7b 46 {
JordanO 19:07706535ff7b 47 motor2Dir.write(0); //positive direction
JordanO 19:07706535ff7b 48 }
JordanO 19:07706535ff7b 49 //Period and PWM
JordanO 19:07706535ff7b 50 motor1.period(periodMotor1);
JordanO 19:07706535ff7b 51 motor1.write(voltage1);
JordanO 19:07706535ff7b 52
JordanO 19:07706535ff7b 53 motor2.period(periodMotor2);
JordanO 19:07706535ff7b 54 motor2.write(voltage2);
JordanO 18:266f1ffdc9c4 55 //Read encoders
JordanO 18:266f1ffdc9c4 56 //Counts
JordanO 19:07706535ff7b 57 //set the value from last loop to poisition 0 in the vector
JordanO 19:07706535ff7b 58 countsMotor1[0] = countsMotor1[1];
JordanO 19:07706535ff7b 59 countsMotor2[0] = countsMotor2[1];
JordanO 19:07706535ff7b 60 //set the new number of counts to position 1 in the vector
JordanO 21:0a37e3280bbd 61 countsMotor1[1] = encoderMotor1.getPulses();
JordanO 21:0a37e3280bbd 62 countsMotor2[1] = encoderMotor2.getPulses();
JordanO 18:266f1ffdc9c4 63 //Velocity calculation
JordanO 21:0a37e3280bbd 64 velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
JordanO 21:0a37e3280bbd 65 velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
JordanO 21:0a37e3280bbd 66
JordanO 21:0a37e3280bbd 67 return (velocityMotor1, velocityMotor2, countsMotor1[1], countsMotor2[1]);
JordanO 18:266f1ffdc9c4 68 }