Diff: motorAndSensorControl.cpp

Revision:

19:07706535ff7b

Parent:

18:266f1ffdc9c4

Child:

21:0a37e3280bbd

diff -r 266f1ffdc9c4 -r 07706535ff7b motorAndSensorControl.cpp
--- a/motorAndSensorControl.cpp	Thu Oct 03 17:50:52 2019 +0000
+++ b/motorAndSensorControl.cpp	Fri Oct 04 09:41:14 2019 +0000
@@ -1,22 +1,21 @@
-//voltage in PWM (between 0 and 1)
+//voltage in PWM (between -1 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);
+        FastPWM motor1(D6); //Motor1 PWM output pin
+        DigitalOut motor1Dir(D7); //Motor1 directional pin
+        FastPWM motor2(D5); //Motor2 PWM output pin
+        DigitalOut motor2Dir(D4); //Motor2 directional pin
     //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 countsMotor1[2]; //Array to store motor counts for i and i-1
         static int countsMotor2[2];
         static double velocityMotor1;
         static double velocityMotor2;
@@ -24,20 +23,42 @@
         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)
+double motorAndEncoder(voltage1, periodMotor1, voltage2, periodMotor2, dt)
 {
     //Set motors
-        motor1.period(periodMotor1);
-        motor1.write(voltage1);
-    
-        motor2.period(periodMotor2);
-        motor2.write(voltage2);
+        //Direction
+            if (voltage1<0)
+            {
+                voltage1 *= -1 //Shorthand for *-1
+                motor1Dir.write(1); //negative direction
+            }
+            else
+            {
+                motor1Dir.write(0); //positive direction    
+            }
+            if (voltage2<0)
+            {
+                voltage2 *= -1 //Shorthand for *-1
+                motor2Dir.write(1); //negative direction
+            }
+            else
+            {
+                motor2Dir.write(0); //positive direction    
+            }
+        //Period and PWM
+            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();
+            //set the value from last loop to poisition 0 in the vector
+                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();
         //Velocity calculation
             velocityMotor1 = ((countsMotor1[1]-countsMotor1[2])/countsToRadians)/dt; //rad/s
             velocityMotor2 = ((countsMotor2[1]-countsMotor2[2])/countsToRadians)/dt; //rad/s