Diff: motorAndSensorControl.cpp

Revision:

36:331907080ffb

Parent:

35:51914ac2d7f4

Child:

37:6602655a80f4

diff -r 51914ac2d7f4 -r 331907080ffb motorAndSensorControl.cpp
--- a/motorAndSensorControl.cpp	Thu Oct 10 14:36:42 2019 +0000
+++ b/motorAndSensorControl.cpp	Thu Oct 10 14:47:47 2019 +0000
@@ -13,12 +13,12 @@
         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
+        //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);
+         //QEI encoderMotor3(,,NC,64,QEI::X2_ENCODING);
 
     //Variables
         static int countsMotor1[2]; //Array to store motor counts for i and i-1
@@ -65,11 +65,11 @@
                 if (PWM3<0)
                 {
                     PWM3 *= -1; //Shorthand for *-1
-                    motor3Dir.write(1); //negative direction
+                   // motor3Dir.write(1); //negative direction
                 }
                 else
                 {
-                    motor3Dir.write(0); //positive direction    
+                    //motor3Dir.write(0); //positive direction    
                 }
         //Period and PWM
             float periodPWM = 1/2000;
@@ -79,8 +79,8 @@
             motor2.period(periodPWM);
             motor2.write(PWM2);
             
-            motor3.period(periodPWM);
-            motor3.write(PWM3);
+            //motor3.period(periodPWM);
+            //motor3.write(PWM3);
             
     //Read encoders
     
@@ -88,31 +88,31 @@
             //set the value from last loop to poisition 0 in the vector
                 countsMotor1[0] = countsMotor1[1];
                 countsMotor2[0] = countsMotor2[1];
-                countsMotor3[0] = countsMotor3[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();
+                //countsMotor3[1] = encoderMotor3.getPulses();
         //Angle
             angleMotor1 = countsMotor1[1]/countsToRadians;
             angleMotor2 = countsMotor2[1]/countsToRadians;   
-            angleMotor3 = countsMotor3[1]/countsToRadians;       
+            //angleMotor3 = countsMotor3[1]/countsToRadians;       
         //Velocity calculation
-            velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //rad/s
+            velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //difference in counts difided by the number of counts per radians [rad/s]
             velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
-            velocityMotor3 = ((countsMotor3[1]-countsMotor3[0])/countsToRadians)/dt; //rad/s
+            //velocityMotor3 = ((countsMotor3[1]-countsMotor3[0])/countsToRadians)/dt; //rad/s
     
     //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.motor3.counts = countsMotor3[1];
         
         ::motorReturn.motor1.angle = angleMotor1;
         ::motorReturn.motor2.angle = angleMotor2;
-        ::motorReturn.motor3.angle = angleMotor3;
+        //::motorReturn.motor3.angle = angleMotor3;
         
         ::motorReturn.motor1.velocity = velocityMotor1;
         ::motorReturn.motor2.velocity = velocityMotor2;
-        ::motorReturn.motor3.velocity = velocityMotor3;
+        //::motorReturn.motor3.velocity = velocityMotor3;
     
 }