Diff: motorAndSensorControl.cpp

Branch:

Branch2

Revision:

66:fa7171cf3f67

Parent:

40:82addb417220

diff -r 6252198c3b67 -r fa7171cf3f67 motorAndSensorControl.cpp
--- a/motorAndSensorControl.cpp	Wed Oct 16 12:37:02 2019 +0000
+++ b/motorAndSensorControl.cpp	Wed Oct 16 13:39:19 2019 +0000
@@ -2,14 +2,19 @@
 //period in seconds
 //dt the time between measurements, i.o.w. Ticker timing 
 
-/*
+
 #include "mbed.h"
 #include "FastPWM.h"
 #include "QEI.h"
-#include "functions.h"
+#include "MODSERIAL.h"
+//#include "stdio.h"
+
+#include "global.h"
 #include "structures.h"
+//#include "functions.h"
 
-    extern motorData motorReturn; 
+//Serial pc(USBTX,USBRX);
+
 
 //Objects
     //Motors
@@ -17,46 +22,68 @@
         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;
+        //const double PI = 3.14159265358979;
         const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
         
-        
+        //motorData motorReturn;
 
-void motorAndEncoder(float PWM1, float PWM2, float dt)
+void motorAndEncoder()
 {
+    float PWM1 = motorData.motor1.input.PWM;
+    float PWM2 = motorData.motor2.input.PWM;
+    float PWM3 = motorData.motor3.input.PWM;
+    float dt = motorData.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);
@@ -64,28 +91,52 @@
         
             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
+            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
-    
-    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
+        ::motorData.motor1.output.counts = countsMotor1[1]; //:: To signal the global structure
+        ::motorData.motor2.output.counts = countsMotor2[1];
+        //::motorReturn.motor3.counts = countsMotor3[1];
+        
+        ::motorData.motor1.output.angle = angleMotor1;
+        ::motorData.motor2.output.angle = angleMotor2;
+        //::motorReturn.motor3.angle = angleMotor3;
+        
+        ::motorData.motor1.output.velocity = velocityMotor1;
+        ::motorData.motor2.output.velocity = velocityMotor2;
+        //::motorReturn.motor3.velocity = velocityMotor3;
+        
+     //debug Mode
+        /*if (motorData.debug == true)
+        {
+            pc.printf("Motor debug mode is on");
+            pc.printf("PWM motor 1 to 3: .2%f, .2%f, .2%f \r\n",motorData.motor1.input.PWM, motorData.motor2.input.PWM, motorData.motor3.input.PWM);
+            pc.printf("CalibrationCounts motor 1 to 3: %i, %i, %i \r\n",motorData.motor1.input.calibrationCounts, motorData.motor2.input.calibrationCounts, motorData.motor3.input.calibrationCounts);
+            pc.printf("Current counts motor 1 to 3: %i, %i, %i \r\n", motorData.motor1.output.counts, motorData.motor2.output.counts, motorData.motor3.output.counts);
+            //pc.printf("Current Velocity motor 1 to 3: .3%f, .3%f, .3%f \r\n",);
+        }   
+        */
 }
-*/
\ No newline at end of file
+