Diff: motorAndSensorControl.cpp

Revision:

35:51914ac2d7f4

Parent:

34:b035131eda3e

Child:

36:331907080ffb

--- a/motorAndSensorControl.cpp	Thu Oct 10 10:56:23 2019 +0000
+++ b/motorAndSensorControl.cpp	Thu Oct 10 14:36:42 2019 +0000
@@ -13,17 +13,23 @@
         DigitalOut motor1Dir(D7); //Motor1 directional pin
         FastPWM motor2(D5); //Motor2 PWM output pin
         DigitalOut motor2Dir(D4); //Motor2 directional pin
+        FastPWM motor3(); //Motor3 PWM output pin
+        DigitalOut motor3Dir(); //Motor3 directional pin
     //Encoders
          QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
          QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
+         QEI encoderMotor3(,,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 int countsMotor3[2];
         static float velocityMotor1;
         static float velocityMotor2;
+        static float velocityMotor3;
         static float angleMotor1;
         static float angleMotor2;
+        static float angleMotor3;
     //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;
@@ -31,28 +37,40 @@
         
         motorData motorReturn;
 
-void motorAndEncoder(float PWM1, float PWM2, float dt)
+void motorAndEncoder(float PWM1, float PWM2, float PWM3, float dt)
 {
     //Set motors
         //Direction
-            if (PWM1<0)
-            {
-                PWM1 *= -1; //Shorthand for *-1
-                motor1Dir.write(1); //negative direction
-            }
-            else
-            {
-                motor1Dir.write(0); //positive direction    
-            }
-            if (PWM2<0)
-            {
-                PWM2 *= -1; //Shorthand for *-1
-                motor2Dir.write(1); //negative direction
-            }
-            else
-            {
-                motor2Dir.write(0); //positive direction    
-            }
+            //Motor 1
+                if (PWM1<0)
+                {
+                    PWM1 *= -1; //Shorthand for *-1
+                    motor1Dir.write(1); //negative direction
+                }
+                else
+                {
+                    motor1Dir.write(0); //positive direction    
+                }
+            //Motor 2
+                if (PWM2<0)
+                {
+                    PWM2 *= -1; //Shorthand for *-1
+                    motor2Dir.write(1); //negative direction
+                }
+                else
+                {
+                    motor2Dir.write(0); //positive direction    
+                }
+            //Motor 3
+                if (PWM3<0)
+                {
+                    PWM3 *= -1; //Shorthand for *-1
+                    motor3Dir.write(1); //negative direction
+                }
+                else
+                {
+                    motor3Dir.write(0); //positive direction    
+                }
         //Period and PWM
             float periodPWM = 1/2000;
             motor1.period(periodPWM);
@@ -61,29 +79,40 @@
             motor2.period(periodPWM);
             motor2.write(PWM2);
             
+            motor3.period(periodPWM);
+            motor3.write(PWM3);
+            
     //Read encoders
     
         //Counts
             //set the value from last loop to poisition 0 in the vector
                 countsMotor1[0] = countsMotor1[1];
                 countsMotor2[0] = countsMotor2[1];
+                countsMotor3[0] = countsMotor3[1];
             //set the new number of counts to position 1 in the vector
                 countsMotor1[1] = encoderMotor1.getPulses();
                 countsMotor2[1] = encoderMotor2.getPulses();
+                countsMotor3[1] = encoderMotor3.getPulses();
         //Angle
             angleMotor1 = countsMotor1[1]/countsToRadians;
-            angleMotor2 = countsMotor2[1]/countsToRadians;        
+            angleMotor2 = countsMotor2[1]/countsToRadians;   
+            angleMotor3 = countsMotor3[1]/countsToRadians;       
         //Velocity calculation
             velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
             velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
-    
-    ::motorReturn.motor1.counts = countsMotor1[1];
-    ::motorReturn.motor2.counts = countsMotor2[1];
+            velocityMotor3 = ((countsMotor3[1]-countsMotor3[0])/countsToRadians)/dt; //rad/s
     
-    ::motorReturn.motor1.angle = angleMotor1;
-    ::motorReturn.motor2.angle = angleMotor2;
-    
-    ::motorReturn.motor1.velocity = velocityMotor1;
-    ::motorReturn.motor2.velocity = velocityMotor2;
+    //Push variables to global structure
+        ::motorReturn.motor1.counts = countsMotor1[1]; //:: To signal the global structure
+        ::motorReturn.motor2.counts = countsMotor2[1];
+        ::motorReturn.motor3.counts = countsMotor3[1];
+        
+        ::motorReturn.motor1.angle = angleMotor1;
+        ::motorReturn.motor2.angle = angleMotor2;
+        ::motorReturn.motor3.angle = angleMotor3;
+        
+        ::motorReturn.motor1.velocity = velocityMotor1;
+        ::motorReturn.motor2.velocity = velocityMotor2;
+        ::motorReturn.motor3.velocity = velocityMotor3;
     
 }