Diff: motorAndSensorControl.cpp

Revision:

18:266f1ffdc9c4

Child:

19:07706535ff7b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motorAndSensorControl.cpp	Thu Oct 03 17:50:52 2019 +0000
@@ -0,0 +1,46 @@
+//voltage in PWM (between 0 and 1)
+//period in seconds
+//dt the time between measurements, i.o.w. Ticker timing
+//directionMotor either 1 or 0
+
+#include "FastPWM.h"
+#include "QEI.h"
+
+//Objects
+    //Motors
+        FastPWM motor1(D6);
+        DigitalOut motor1Dir(D7);
+        FastPWM motor2(D5);
+        DigitalOut motor2Dir(D4);
+    //Encoders
+         QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
+         QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
+    //Variables
+        static int countsMotor1[2];
+        static int countsMotor2[2];
+        static double velocityMotor1;
+        static double velocityMotor2;
+    //Constants
+        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
+        const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
+
+double motorAndEncoder(voltage1, periodMotor1, voltage2, periodMotor2, directionMotor1, directionMotor2, dt)
+{
+    //Set motors
+        motor1.period(periodMotor1);
+        motor1.write(voltage1);
+    
+        motor2.period(periodMotor2);
+        motor2.write(voltage2);
+    //Read encoders
+        //Counts
+            countsMotor1[0] = countsMotor1[1];
+            countsMotor2[0] = countsMotor2[1];
+            countsMotor1[1] = encoderMotor1.gePulses();
+            countsMotor2[1] = encoderMotor2.gePulses();
+        //Velocity calculation
+            velocityMotor1 = ((countsMotor1[1]-countsMotor1[2])/countsToRadians)/dt; //rad/s
+            velocityMotor2 = ((countsMotor2[1]-countsMotor2[2])/countsToRadians)/dt; //rad/s
+            
+    return (velocityMotor1, velocityMotor2, countsMotor1[1], countsMotor2[0]);
+}
\ No newline at end of file