Diff: motorAndSensorControl.cpp

Revision:

27:71be6e074d0f

Parent:

22:4197629a300f

Child:

28:4a5671b3d88d

--- a/motorAndSensorControl.cpp	Wed Oct 09 07:11:45 2019 +0000
+++ b/motorAndSensorControl.cpp	Wed Oct 09 12:56:35 2019 +0000
@@ -14,32 +14,49 @@
     //Encoders
          QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
          QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
+
     //Variables
         static int countsMotor1[2]; //Array to store motor counts for i and i-1
         static int countsMotor2[2];
         static float velocityMotor1;
         static float velocityMotor2;
+        static float angleMotor1;
+        static float angleMotor2;
     //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
+        
+struct motorReturnSub
+{
+    int counts;
+    float angle;
+    float velocity;
+};
 
-void motorAndEncoder(float voltage1, float periodMotor1, float voltage2, float periodMotor2, float dt, float &countsMotor1Return, float &countsMotor2Return, float &velocityMotor1Return, float &velocityMotor2Return)
+struct motorReturn
+{
+    motorReturnSub motor1;
+    motorReturnSub motor2;
+    motorReturnSub motor3;
+} motorReturn;
+
+motorReturn motorAndEncoder(float PWM1, float PWM2, float dt)
 {
     //Set motors
         //Direction
-            if (voltage1<0)
+            if (PWM1<0)
             {
-                voltage1 *= -1; //Shorthand for *-1
+                PWM1 *= -1; //Shorthand for *-1
                 motor1Dir.write(1); //negative direction
             }
             else
             {
                 motor1Dir.write(0); //positive direction    
             }
-            if (voltage2<0)
+            if (PWM2<0)
             {
-                voltage2 *= -1; //Shorthand for *-1
+                PWM2 *= -1; //Shorthand for *-1
                 motor2Dir.write(1); //negative direction
             }
             else
@@ -47,11 +64,12 @@
                 motor2Dir.write(0); //positive direction    
             }
         //Period and PWM
-            motor1.period(periodMotor1);
-            motor1.write(voltage1);
+            float periodPWM = 1/2000;
+            motor1.period(periodPWM);
+            motor1.write(PWM1);
         
-            motor2.period(periodMotor2);
-            motor2.write(voltage2);
+            motor2.period(periodPWM);
+            motor2.write(PWM2);
     //Read encoders
         //Counts
             //set the value from last loop to poisition 0 in the vector
@@ -60,12 +78,26 @@
             //set the new number of counts to position 1 in the vector
                 countsMotor1[1] = encoderMotor1.getPulses();
                 countsMotor2[1] = encoderMotor2.getPulses();
+        //Angle
+            angleMotor1 = countsMotor1[1]/countsToRadians;
+            angleMotor2 = countsMotor2[1]/countsToRadians;        
         //Velocity calculation
             velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
             velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
-               
+              /* 
     countsMotor1Return = countsMotor1[1];
     countsMotor2Return = countsMotor2[1];
     velocityMotor1Return = velocityMotor1;
     velocityMotor2Return = velocityMotor2;
+    */
+    motorReturn.motor1.counts = countsMotor1[1];
+    motorReturn.motor2.counts = countsMotor2[1];
+    
+    motorReturn.motor1.angle = angleMotor1;
+    motorReturn.motor2.angle = angleMotor2;
+    
+    motorReturn.motor1.velocity = velocityMotor1;
+    motorReturn.motor2.velocity = velocityMotor2;
+    
+    return motorReturn
 }
\ No newline at end of file