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@39:8d8ff9a30cfc, 2019-10-10 (annotated)

Committer:
JordanO
Date:
Thu Oct 10 15:49:25 2019 +0000
Revision:
39:8d8ff9a30cfc
Parent:
37:6602655a80f4 
Start on debug mode

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