Diff: motorAndSensorControl.cpp

Revision:

21:0a37e3280bbd

Parent:

19:07706535ff7b

Child:

22:4197629a300f

diff -r 19f7b0d5ec7b -r 0a37e3280bbd motorAndSensorControl.cpp
--- a/motorAndSensorControl.cpp	Fri Oct 04 09:41:56 2019 +0000
+++ b/motorAndSensorControl.cpp	Fri Oct 04 10:07:27 2019 +0000
@@ -1,7 +1,7 @@
 //voltage in PWM (between -1 and 1)
 //period in seconds
 //dt the time between measurements, i.o.w. Ticker timing
-
+#include "mbed.h"
 #include "FastPWM.h"
 #include "QEI.h"
 
@@ -17,19 +17,20 @@
     //Variables
         static int countsMotor1[2]; //Array to store motor counts for i and i-1
         static int countsMotor2[2];
-        static double velocityMotor1;
-        static double velocityMotor2;
+        static float velocityMotor1;
+        static float 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 double PI = 3.14159265358979;
         const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
 
-double motorAndEncoder(voltage1, periodMotor1, voltage2, periodMotor2, dt)
+float motorAndEncoder(float voltage1, float periodMotor1, float voltage2, float periodMotor2, float dt)
 {
     //Set motors
         //Direction
             if (voltage1<0)
             {
-                voltage1 *= -1 //Shorthand for *-1
+                voltage1 *= -1; //Shorthand for *-1
                 motor1Dir.write(1); //negative direction
             }
             else
@@ -38,7 +39,7 @@
             }
             if (voltage2<0)
             {
-                voltage2 *= -1 //Shorthand for *-1
+                voltage2 *= -1; //Shorthand for *-1
                 motor2Dir.write(1); //negative direction
             }
             else
@@ -57,11 +58,11 @@
                 countsMotor1[0] = countsMotor1[1];
                 countsMotor2[0] = countsMotor2[1];
             //set the new number of counts to position 1 in the vector
-                countsMotor1[1] = encoderMotor1.gePulses();
-                countsMotor2[1] = encoderMotor2.gePulses();
+                countsMotor1[1] = encoderMotor1.getPulses();
+                countsMotor2[1] = encoderMotor2.getPulses();
         //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]);
+            velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
+            velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
+               
+    return (velocityMotor1, velocityMotor2, countsMotor1[1], countsMotor2[1]);
 }
\ No newline at end of file