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@28:4a5671b3d88d, 2019-10-09 (annotated)

Committer:
JornD
Date:
Wed Oct 09 13:20:19 2019 +0000
Revision:
28:4a5671b3d88d
Parent:
27:71be6e074d0f
Child:
30:161f748a1cce
Fixed global variable and structure distribution;

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
JornD 28:4a5671b3d88d 3 //dt the time between measurements, i.o.w. Ticker timing
JornD 28:4a5671b3d88d 4 /*
JordanO 21:0a37e3280bbd 5 #include "mbed.h"
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 19:07706535ff7b 11 FastPWM motor1(D6); //Motor1 PWM output pin
JordanO 19:07706535ff7b 12 DigitalOut motor1Dir(D7); //Motor1 directional pin
JordanO 19:07706535ff7b 13 FastPWM motor2(D5); //Motor2 PWM output pin
JordanO 19:07706535ff7b 14 DigitalOut motor2Dir(D4); //Motor2 directional pin
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 27:71be6e074d0f 18
JordanO 18:266f1ffdc9c4 19 //Variables
JordanO 19:07706535ff7b 20 static int countsMotor1[2]; //Array to store motor counts for i and i-1
JordanO 18:266f1ffdc9c4 21 static int countsMotor2[2];
JordanO 21:0a37e3280bbd 22 static float velocityMotor1;
JordanO 21:0a37e3280bbd 23 static float velocityMotor2;
JordanO 27:71be6e074d0f 24 static float angleMotor1;
JordanO 27:71be6e074d0f 25 static float angleMotor2;
JordanO 18:266f1ffdc9c4 26 //Constants
JordanO 18:266f1ffdc9c4 27 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 28 const double PI = 3.14159265358979;
JordanO 18:266f1ffdc9c4 29 const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
JordanO 27:71be6e074d0f 30
JordanO 27:71be6e074d0f 31 struct motorReturnSub
JordanO 27:71be6e074d0f 32 {
JordanO 27:71be6e074d0f 33 int counts;
JordanO 27:71be6e074d0f 34 float angle;
JordanO 27:71be6e074d0f 35 float velocity;
JordanO 27:71be6e074d0f 36 };
JordanO 18:266f1ffdc9c4 37
JordanO 27:71be6e074d0f 38 struct motorReturn
JordanO 27:71be6e074d0f 39 {
JordanO 27:71be6e074d0f 40 motorReturnSub motor1;
JordanO 27:71be6e074d0f 41 motorReturnSub motor2;
JordanO 27:71be6e074d0f 42 motorReturnSub motor3;
JordanO 27:71be6e074d0f 43 } motorReturn;
JordanO 27:71be6e074d0f 44
JordanO 27:71be6e074d0f 45 motorReturn motorAndEncoder(float PWM1, float PWM2, float dt)
JordanO 18:266f1ffdc9c4 46 {
JordanO 18:266f1ffdc9c4 47 //Set motors
JordanO 19:07706535ff7b 48 //Direction
JordanO 27:71be6e074d0f 49 if (PWM1<0)
JordanO 19:07706535ff7b 50 {
JordanO 27:71be6e074d0f 51 PWM1 *= -1; //Shorthand for *-1
JordanO 19:07706535ff7b 52 motor1Dir.write(1); //negative direction
JordanO 19:07706535ff7b 53 }
JordanO 19:07706535ff7b 54 else
JordanO 19:07706535ff7b 55 {
JordanO 19:07706535ff7b 56 motor1Dir.write(0); //positive direction
JordanO 19:07706535ff7b 57 }
JordanO 27:71be6e074d0f 58 if (PWM2<0)
JordanO 19:07706535ff7b 59 {
JordanO 27:71be6e074d0f 60 PWM2 *= -1; //Shorthand for *-1
JordanO 19:07706535ff7b 61 motor2Dir.write(1); //negative direction
JordanO 19:07706535ff7b 62 }
JordanO 19:07706535ff7b 63 else
JordanO 19:07706535ff7b 64 {
JordanO 19:07706535ff7b 65 motor2Dir.write(0); //positive direction
JordanO 19:07706535ff7b 66 }
JordanO 19:07706535ff7b 67 //Period and PWM
JordanO 27:71be6e074d0f 68 float periodPWM = 1/2000;
JordanO 27:71be6e074d0f 69 motor1.period(periodPWM);
JordanO 27:71be6e074d0f 70 motor1.write(PWM1);
JordanO 19:07706535ff7b 71
JordanO 27:71be6e074d0f 72 motor2.period(periodPWM);
JordanO 27:71be6e074d0f 73 motor2.write(PWM2);
JordanO 18:266f1ffdc9c4 74 //Read encoders
JordanO 18:266f1ffdc9c4 75 //Counts
JordanO 19:07706535ff7b 76 //set the value from last loop to poisition 0 in the vector
JordanO 19:07706535ff7b 77 countsMotor1[0] = countsMotor1[1];
JordanO 19:07706535ff7b 78 countsMotor2[0] = countsMotor2[1];
JordanO 19:07706535ff7b 79 //set the new number of counts to position 1 in the vector
JordanO 21:0a37e3280bbd 80 countsMotor1[1] = encoderMotor1.getPulses();
JordanO 21:0a37e3280bbd 81 countsMotor2[1] = encoderMotor2.getPulses();
JordanO 27:71be6e074d0f 82 //Angle
JordanO 27:71be6e074d0f 83 angleMotor1 = countsMotor1[1]/countsToRadians;
JordanO 27:71be6e074d0f 84 angleMotor2 = countsMotor2[1]/countsToRadians;
JordanO 18:266f1ffdc9c4 85 //Velocity calculation
JordanO 21:0a37e3280bbd 86 velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
JordanO 21:0a37e3280bbd 87 velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
JordanO 27:71be6e074d0f 88 /*
JordanO 22:4197629a300f 89 countsMotor1Return = countsMotor1[1];
JordanO 22:4197629a300f 90 countsMotor2Return = countsMotor2[1];
JordanO 22:4197629a300f 91 velocityMotor1Return = velocityMotor1;
JordanO 22:4197629a300f 92 velocityMotor2Return = velocityMotor2;
JordanO 27:71be6e074d0f 93 */
JornD 28:4a5671b3d88d 94 /*
JordanO 27:71be6e074d0f 95 motorReturn.motor1.counts = countsMotor1[1];
JordanO 27:71be6e074d0f 96 motorReturn.motor2.counts = countsMotor2[1];
JordanO 27:71be6e074d0f 97
JordanO 27:71be6e074d0f 98 motorReturn.motor1.angle = angleMotor1;
JordanO 27:71be6e074d0f 99 motorReturn.motor2.angle = angleMotor2;
JordanO 27:71be6e074d0f 100
JordanO 27:71be6e074d0f 101 motorReturn.motor1.velocity = velocityMotor1;
JordanO 27:71be6e074d0f 102 motorReturn.motor2.velocity = velocityMotor2;
JordanO 27:71be6e074d0f 103
JordanO 27:71be6e074d0f 104 return motorReturn
JornD 28:4a5671b3d88d 105 }
JornD 28:4a5671b3d88d 106 */