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@66:fa7171cf3f67, 2019-10-16 (annotated)

Committer:
JornD
Date:
Wed Oct 16 13:39:19 2019 +0000
Branch:
Branch2
Revision:
66:fa7171cf3f67
Parent:
40:82addb417220 
WORKING - Implemented Jordan's motor control

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
JordanO 30:161f748a1cce 4
JornD 66:fa7171cf3f67 5
JordanO 21:0a37e3280bbd 6 #include "mbed.h"
JordanO 18:266f1ffdc9c4 7 #include "FastPWM.h"
JordanO 18:266f1ffdc9c4 8 #include "QEI.h"
JornD 66:fa7171cf3f67 9 #include "MODSERIAL.h"
JornD 66:fa7171cf3f67 10 //#include "stdio.h"
JornD 66:fa7171cf3f67 11
JornD 66:fa7171cf3f67 12 #include "global.h"
JornD 40:82addb417220 13 #include "structures.h"
JornD 66:fa7171cf3f67 14 //#include "functions.h"
JornD 40:82addb417220 15
JornD 66:fa7171cf3f67 16 //Serial pc(USBTX,USBRX);
JornD 66:fa7171cf3f67 17
JordanO 18:266f1ffdc9c4 18
JordanO 18:266f1ffdc9c4 19 //Objects
JordanO 18:266f1ffdc9c4 20 //Motors
JordanO 19:07706535ff7b 21 FastPWM motor1(D6); //Motor1 PWM output pin
JordanO 19:07706535ff7b 22 DigitalOut motor1Dir(D7); //Motor1 directional pin
JordanO 19:07706535ff7b 23 FastPWM motor2(D5); //Motor2 PWM output pin
JordanO 19:07706535ff7b 24 DigitalOut motor2Dir(D4); //Motor2 directional pin
JornD 66:fa7171cf3f67 25 //FastPWM motor3(); //Motor3 PWM output pin
JornD 66:fa7171cf3f67 26 //DigitalOut motor3Dir(); //Motor3 directional pin
JordanO 18:266f1ffdc9c4 27 //Encoders
JordanO 18:266f1ffdc9c4 28 QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 18:266f1ffdc9c4 29 QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
JornD 66:fa7171cf3f67 30 //QEI encoderMotor3(,,NC,64,QEI::X2_ENCODING);
JordanO 27:71be6e074d0f 31
JordanO 18:266f1ffdc9c4 32 //Variables
JordanO 19:07706535ff7b 33 static int countsMotor1[2]; //Array to store motor counts for i and i-1
JordanO 18:266f1ffdc9c4 34 static int countsMotor2[2];
JornD 66:fa7171cf3f67 35 static int countsMotor3[2];
JordanO 21:0a37e3280bbd 36 static float velocityMotor1;
JordanO 21:0a37e3280bbd 37 static float velocityMotor2;
JornD 66:fa7171cf3f67 38 static float velocityMotor3;
JordanO 27:71be6e074d0f 39 static float angleMotor1;
JordanO 27:71be6e074d0f 40 static float angleMotor2;
JornD 66:fa7171cf3f67 41 static float angleMotor3;
JordanO 18:266f1ffdc9c4 42 //Constants
JordanO 18:266f1ffdc9c4 43 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
JornD 66:fa7171cf3f67 44 //const double PI = 3.14159265358979;
JordanO 18:266f1ffdc9c4 45 const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
JordanO 27:71be6e074d0f 46
JornD 66:fa7171cf3f67 47 //motorData motorReturn;
JordanO 27:71be6e074d0f 48
JornD 66:fa7171cf3f67 49 void motorAndEncoder()
JordanO 18:266f1ffdc9c4 50 {
JornD 66:fa7171cf3f67 51 float PWM1 = motorData.motor1.input.PWM;
JornD 66:fa7171cf3f67 52 float PWM2 = motorData.motor2.input.PWM;
JornD 66:fa7171cf3f67 53 float PWM3 = motorData.motor3.input.PWM;
JornD 66:fa7171cf3f67 54 float dt = motorData.dt;
JordanO 18:266f1ffdc9c4 55 //Set motors
JordanO 19:07706535ff7b 56 //Direction
JornD 66:fa7171cf3f67 57 //Motor 1
JornD 66:fa7171cf3f67 58 if (PWM1<0)
JornD 66:fa7171cf3f67 59 {
JornD 66:fa7171cf3f67 60 PWM1 *= -1; //Shorthand for *-1
JornD 66:fa7171cf3f67 61 motor1Dir.write(1); //negative direction
JornD 66:fa7171cf3f67 62 }
JornD 66:fa7171cf3f67 63 else
JornD 66:fa7171cf3f67 64 {
JornD 66:fa7171cf3f67 65 motor1Dir.write(0); //positive direction
JornD 66:fa7171cf3f67 66 }
JornD 66:fa7171cf3f67 67 //Motor 2
JornD 66:fa7171cf3f67 68 if (PWM2<0)
JornD 66:fa7171cf3f67 69 {
JornD 66:fa7171cf3f67 70 PWM2 *= -1; //Shorthand for *-1
JornD 66:fa7171cf3f67 71 motor2Dir.write(1); //negative direction
JornD 66:fa7171cf3f67 72 }
JornD 66:fa7171cf3f67 73 else
JornD 66:fa7171cf3f67 74 {
JornD 66:fa7171cf3f67 75 motor2Dir.write(0); //positive direction
JornD 66:fa7171cf3f67 76 }
JornD 66:fa7171cf3f67 77 //Motor 3
JornD 66:fa7171cf3f67 78 if (PWM3<0)
JornD 66:fa7171cf3f67 79 {
JornD 66:fa7171cf3f67 80 PWM3 *= -1; //Shorthand for *-1
JornD 66:fa7171cf3f67 81 // motor3Dir.write(1); //negative direction
JornD 66:fa7171cf3f67 82 }
JornD 66:fa7171cf3f67 83 else
JornD 66:fa7171cf3f67 84 {
JornD 66:fa7171cf3f67 85 //motor3Dir.write(0); //positive direction
JornD 66:fa7171cf3f67 86 }
JordanO 19:07706535ff7b 87 //Period and PWM
JordanO 27:71be6e074d0f 88 float periodPWM = 1/2000;
JordanO 27:71be6e074d0f 89 motor1.period(periodPWM);
JordanO 27:71be6e074d0f 90 motor1.write(PWM1);
JordanO 19:07706535ff7b 91
JordanO 27:71be6e074d0f 92 motor2.period(periodPWM);
JordanO 27:71be6e074d0f 93 motor2.write(PWM2);
JornD 66:fa7171cf3f67 94
JornD 66:fa7171cf3f67 95 //motor3.period(periodPWM);
JornD 66:fa7171cf3f67 96 //motor3.write(PWM3);
JornD 66:fa7171cf3f67 97
JordanO 18:266f1ffdc9c4 98 //Read encoders
JornD 66:fa7171cf3f67 99
JordanO 18:266f1ffdc9c4 100 //Counts
JordanO 19:07706535ff7b 101 //set the value from last loop to poisition 0 in the vector
JordanO 19:07706535ff7b 102 countsMotor1[0] = countsMotor1[1];
JordanO 19:07706535ff7b 103 countsMotor2[0] = countsMotor2[1];
JornD 66:fa7171cf3f67 104 //countsMotor3[0] = countsMotor3[1];
JordanO 19:07706535ff7b 105 //set the new number of counts to position 1 in the vector
JordanO 21:0a37e3280bbd 106 countsMotor1[1] = encoderMotor1.getPulses();
JordanO 21:0a37e3280bbd 107 countsMotor2[1] = encoderMotor2.getPulses();
JornD 66:fa7171cf3f67 108 //countsMotor3[1] = encoderMotor3.getPulses();
JordanO 27:71be6e074d0f 109 //Angle
JordanO 27:71be6e074d0f 110 angleMotor1 = countsMotor1[1]/countsToRadians;
JornD 66:fa7171cf3f67 111 angleMotor2 = countsMotor2[1]/countsToRadians;
JornD 66:fa7171cf3f67 112 //angleMotor3 = countsMotor3[1]/countsToRadians;
JordanO 18:266f1ffdc9c4 113 //Velocity calculation
JornD 66:fa7171cf3f67 114 velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //difference in counts difided by the number of counts per radians [rad/s]
JordanO 21:0a37e3280bbd 115 velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
JornD 66:fa7171cf3f67 116 //velocityMotor3 = ((countsMotor3[1]-countsMotor3[0])/countsToRadians)/dt; //rad/s
JordanO 27:71be6e074d0f 117
JornD 66:fa7171cf3f67 118 //Push variables to global structure
JornD 66:fa7171cf3f67 119 ::motorData.motor1.output.counts = countsMotor1[1]; //:: To signal the global structure
JornD 66:fa7171cf3f67 120 ::motorData.motor2.output.counts = countsMotor2[1];
JornD 66:fa7171cf3f67 121 //::motorReturn.motor3.counts = countsMotor3[1];
JornD 66:fa7171cf3f67 122
JornD 66:fa7171cf3f67 123 ::motorData.motor1.output.angle = angleMotor1;
JornD 66:fa7171cf3f67 124 ::motorData.motor2.output.angle = angleMotor2;
JornD 66:fa7171cf3f67 125 //::motorReturn.motor3.angle = angleMotor3;
JornD 66:fa7171cf3f67 126
JornD 66:fa7171cf3f67 127 ::motorData.motor1.output.velocity = velocityMotor1;
JornD 66:fa7171cf3f67 128 ::motorData.motor2.output.velocity = velocityMotor2;
JornD 66:fa7171cf3f67 129 //::motorReturn.motor3.velocity = velocityMotor3;
JornD 66:fa7171cf3f67 130
JornD 66:fa7171cf3f67 131 //debug Mode
JornD 66:fa7171cf3f67 132 /*if (motorData.debug == true)
JornD 66:fa7171cf3f67 133 {
JornD 66:fa7171cf3f67 134 pc.printf("Motor debug mode is on");
JornD 66:fa7171cf3f67 135 pc.printf("PWM motor 1 to 3: .2%f, .2%f, .2%f \r\n",motorData.motor1.input.PWM, motorData.motor2.input.PWM, motorData.motor3.input.PWM);
JornD 66:fa7171cf3f67 136 pc.printf("CalibrationCounts motor 1 to 3: %i, %i, %i \r\n",motorData.motor1.input.calibrationCounts, motorData.motor2.input.calibrationCounts, motorData.motor3.input.calibrationCounts);
JornD 66:fa7171cf3f67 137 pc.printf("Current counts motor 1 to 3: %i, %i, %i \r\n", motorData.motor1.output.counts, motorData.motor2.output.counts, motorData.motor3.output.counts);
JornD 66:fa7171cf3f67 138 //pc.printf("Current Velocity motor 1 to 3: .3%f, .3%f, .3%f \r\n",);
JornD 66:fa7171cf3f67 139 }
JornD 66:fa7171cf3f67 140 */
JornD 28:4a5671b3d88d 141 }
JornD 66:fa7171cf3f67 142