Diff: main.cpp

Revision:

10:a60b369c1711

Parent:

9:c4fa72ffa1c2

Child:

11:a3fd9d5144bb

--- a/main.cpp	Tue Oct 22 22:37:32 2019 +0000
+++ b/main.cpp	Tue Oct 29 13:36:53 2019 +0000
@@ -42,8 +42,7 @@
 double f=1/100;                     // Frequency
 const double Ts = f/10;            // Sampletime
 HIDScope scope(2);                  // Amount of HIDScope servers
-double positie_verschil_motor1;
-double positie_prev_motor1;
+
 
 // 3.3 BiQuad Filters **********************************************************
 static BiQuad notchfilter(9.97761e-01, -1.97095e+00, 9.97761e-01, -1.97095e+00, 9.95522e-01);
@@ -71,8 +70,10 @@
     FastPWM motor1(D6);                  // motor 1 velocity control (between 0-1)
     FastPWM motor2(D5);                  // motor 2 velocity control (between 0-1)
     DigitalOut motor2DirectionPin(D4);   // motor 2 direction control (1=cw, 0=ccw)
+    bool motor1_calibration_status=true;
+    bool motor2_calibration_status=true;
  
-// 3.5 PID variabelen ***************************************************************   
+// 3.5 Motor 1 variables ***************************************************************   
     //3.5a PID-controller motor 1
     double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
     static double error_integral_motor1 = 0;
@@ -88,6 +89,10 @@
     double error1_derivative_filtered_motor1;
     double P_motor1;
     
+    double positie_verschil_motor1;
+    double positie_prev_motor1;
+
+// 3.5 Motor 2 variables ***************************************************************
     //3.5b PID-controller motor 2
     double counts_per_rad_motor2 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
     static double error_integral_motor2 = 0;
@@ -102,6 +107,9 @@
     double error1_derivative_motor2;
     double error1_derivative_filtered_motor2;
     double P_motor2;
+    
+    double positie_verschil_motor2;
+    double positie_prev_motor2;    
 
 //******************************************************************************
 // 4. Functions ****************************************************************
@@ -116,7 +124,7 @@
 
         scope.send();   
     }
-    
+    // 4.x Encoder motor1 ****************************************************************
         double fencoder_motor1()                                    // bepaalt de positie van de motor
         {
             positie_motor1 = encoder_motor1.getPulses();                // haalt encoder waardes op
@@ -125,7 +133,50 @@
             
             return positie_motor1;                                 //geeft positie van motor
         }
-  
+    // 4.x Encoder motor2 ****************************************************************        
+        double fencoder_motor2()                                    // bepaalt de positie van de motor
+        {
+            positie_motor2 = encoder_motor2.getPulses();                // haalt encoder waardes op
+            positie_verschil_motor2 =  (positie_motor2-positie_prev_motor2)/Ts;
+            positie_prev_motor2 = positie_motor2;
+            
+            return positie_motor2;                                 //geeft positie van motor
+        }
+        
+    // 4.xa Calibration motor 1
+    void motor1_calibration()
+    {
+        motor1DirectionPin=0; //direction of the motor
+        motor1=1.0;
+        wait(0.05);
+        while (abs(positie_verschil_motor1)>5)
+        {
+            motor1=0.2  ; 
+            pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibration_status ? "true" : "false");
+        }
+        motor1=0.0;
+        motor1_calibration_status=false;  
+        pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibration_status ? "true" : "false");
+    
+    }
+    
+    // 4.xa Calibration motor 2
+    void motor2_calibration()
+    {
+        motor2DirectionPin=0; //direction of the motor
+        motor2=1.0;
+        wait(1);
+        while (abs(positie_verschil_motor2)>5)
+        {
+            motor2=0.2  ; 
+            pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibration_status ? "true" : "false");
+        }
+        motor2=0.0;
+        motor2_calibration_status=false;  
+        pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibration_status ? "true" : "false");
+    
+    }
+    
     // 4.2a PID-Controller motor 1**************************************************
     double PID_controller_motor1()
     {
@@ -173,21 +224,9 @@
 
         return P_motor2; 
     }
+    
     // 4.3 State-Machine *******************************************************
-        void motor1_calibration()
-        {
-            motor1DirectionPin=0; //direction of the motor
-            motor1=0.2;
-            wait(1);
-            while (abs(positie_verschil_motor1)>5)
-            {
-                motor1=0.2  ; 
-                pc.printf("\r\n Kalibratie werkt ");
-            }
-            motor1=0.0;  
-            pc.printf("\r\n Kalibratie werkt niet");
-        
-        }
+
     
 //******************************************************************************
 // 5. Main Loop ****************************************************************
@@ -196,6 +235,7 @@
 void main_loop() { //Beginning of main_loop()
 //    pc.printf("main_loop is running succesfully \r\n"); //confirmation that main_loop is running (als je dit erin zet krijg je elke duizendste dit bericht. Dit is niet gewenst)
 fencoder_motor1() ;
+fencoder_motor2() ;
 
 
 // 5.1 Measure Analog and Digital input signals ********************************
@@ -227,6 +267,9 @@
     ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
     ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     motor1_calibration();
+    motor2_calibration();
+    
+    
 // 6.2 While loop in main function**********************************************   
     while (true) { } //Is not used but has to remain in the code!!