Revision 66:fa7171cf3f67, committed 2019-10-16

Comitter:

JornD

Date:

Wed Oct 16 13:39:19 2019 +0000

Branch:

Branch2

Parent:

65:6252198c3b67

Child:

67:5ebb08e337ae

Commit message:

WORKING - Implemented Jordan's motor control

Changed in this revision

--- a/functions.h	Wed Oct 16 12:37:02 2019 +0000
+++ b/functions.h	Wed Oct 16 13:39:19 2019 +0000
@@ -10,5 +10,7 @@
     
     //--EMG Processing; from "emgprocessing.cpp";
     float ProcessEMG(float X);
+    
+    void motorAndEncoder(float PWM1, float PWM2, float PWM3, float dt);
 
 #endif
\ No newline at end of file

--- a/global.cpp	Wed Oct 16 12:37:02 2019 +0000
+++ b/global.cpp	Wed Oct 16 13:39:19 2019 +0000
@@ -5,8 +5,11 @@
     const float Ts = 0.002;
     const float PI = 3.14159265359;
 
+//Define Motor Data structure
+    motorStruc motorData;
+
 //Define Controller structures, shorthand: Set_
-        //For the EMG filters
+    //For the EMG filters
     ControllerSettings Set_LPFEMG;
     ControllerSettings Set_NOTEMG;
     //For the different controllers

--- a/global.h	Wed Oct 16 12:37:02 2019 +0000
+++ b/global.h	Wed Oct 16 13:39:19 2019 +0000
@@ -4,6 +4,8 @@
 
     extern const float PI;
     extern const float Ts;
+    //Motor data
+    extern struct motorStruc motorData;
     
     //Define Controller structures, shorthand: Set_
     extern struct ControllerSettings Set_LPFEMG;

--- a/main.cpp	Wed Oct 16 12:37:02 2019 +0000
+++ b/main.cpp	Wed Oct 16 13:39:19 2019 +0000
@@ -3,6 +3,7 @@
     #include "FastPWM.h"
     #include "MODSERIAL.h"
     #include "QEI.h"
+    #include "stdio.h"
     
 //Homebrew libraries
 
@@ -38,6 +39,11 @@
     Ticker Main_Ticker;
     Ticker Tick_Blinky;//used for the blinking of the leds
     
+    
+    
+        //motorStruc motorData;
+    
+    
 //Changing LED colour depending on current state
 void FlipLED(void)
 {

--- 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
+

--- a/structures.h	Wed Oct 16 12:37:02 2019 +0000
+++ b/structures.h	Wed Oct 16 13:39:19 2019 +0000
@@ -3,21 +3,37 @@
 
 //Structures
 //--Motor data
-    struct motorReturnSub
+
+    struct motorDataOutputSub
     {
-        int zerocounts;
         int counts;
         float angle;
         float velocity;
     };
     
-    struct motorData
+    struct motorDataInputSub
+    {
+        float PWM;
+        int calibrationCounts;
+    };
+    
+    struct motorDataCombined
     {
-        motorReturnSub motor1;
-        motorReturnSub motor2;
-        motorReturnSub motor3;
+        motorDataOutputSub output;
+        motorDataInputSub input;
+    
     };
     
+    struct motorStruc
+    {
+        motorDataCombined motor1;
+        motorDataCombined motor2;
+        motorDataCombined motor3;
+        float dt;
+        bool debug;
+    };
+        
+    
 //--PID controller settings
     struct ControllerSettings //Controller settings of the discrete TF
     {