Diff: main.cpp

Revision:

15:849e0fc5d3a8

Parent:

14:236ae2d7ec41

Child:

16:1be144329f05

--- a/main.cpp	Tue Oct 29 20:07:26 2019 +0000
+++ b/main.cpp	Tue Oct 29 20:21:54 2019 +0000
@@ -103,6 +103,9 @@
     double kp_motor2;
     double Ki_motor2;
     double Kd_motor2;
+    double Up_motor2;
+    double Ui_motor2;
+    double Ud_motor2;
 
     double positie_motor2;                              //counts encoder
     double error1_motor2;
@@ -122,15 +125,15 @@
     void HIDScope() //voor HIDscope
     {
         scope.set(0, positie_motor1);
-       scope.set(1, error1_motor1); //nog te definieren wat we willen weergeven
-       scope.set(2, P_motor1); //nog te definieren wat we willen weergeven
-       scope.set(3, Up_motor1);
+       scope.set(1, positie_motor2); //nog te definieren wat we willen weergeven
+   //    scope.set(2, P_motor1); //nog te definieren wat we willen weergeven
+   //    scope.set(3, Up_motor1);
    //    scope.set(4, Ui_motor1);
    //    scope.set(5, Uk_motor1);
        
-
         scope.send();   
     }
+    
     // 4.x Encoder motor1 ****************************************************************
         double fencoder_motor1()                                    // bepaalt de positie van de motor
         {
@@ -195,12 +198,12 @@
         Up_motor1 = kp_motor1 * error1_motor1;
     
         //Integral part
-        Ki_motor1 = 0.0001;     // moet nog getweaked worden
+        Ki_motor1 = 0.001;     // moet nog getweaked worden
         error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
         Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
     
         //Derivative part 
-        Kd_motor1 = 0.01 ;// moet nog getweaked worden  
+        Kd_motor1 = 0.001 ;// moet nog getweaked worden  
         error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
         error1_derivative_filtered_motor1 = LowPassFilter.step(error1_derivative_motor1); //derivative wordt gefiltered
         Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
@@ -214,25 +217,25 @@
     }
 
     // 4.2b PID-Controller motor 2**************************************************
-    double PID_controller_motor2()
+    double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
     {
         //Proportional part
         kp_motor2 = 0.01 ;      // moet nog getweaked worden
-        double Up_motor2 = kp_motor2 * error1_motor2;
+        Up_motor2 = kp_motor2 * error1_motor2;
     
         //Integral part
-        Ki_motor2 = 0.0001;     // moet nog getweaked worden
+        Ki_motor2 = 0.001;     // moet nog getweaked worden
         error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2);       // integrale fout + (de sample tijd * fout)
-        double Ui_motor2 = Ki_motor2 * error_integral_motor2;                     //de fout keer de integrale fout
+        Ui_motor2 = Ki_motor2 * error_integral_motor2;                     //de fout keer de integrale fout
     
         //Derivative part 
-        Kd_motor2 = 0.01 ;// moet nog getweaked worden  
+        Kd_motor2 = 0.001 ;// moet nog getweaked worden  
         error1_derivative_motor2 = (error1_motor2  -  error1_prev_motor2)/Ts;
         error1_derivative_filtered_motor2 = LowPassFilter.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
-        double Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
+        Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
         error1_prev_motor2 = error1_motor2;
 
-        double P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2;                                           //sommatie van de u's
+        P_motor2 = Up_motor2 + Ui_motor2 + Ud_motor2;                                           //sommatie van de u's
 
         return P_motor2; 
     }
@@ -257,7 +260,27 @@
         {
             motor1 = fabs(P_motor1);
         }
+    }  
+   double motor2_pwm()
+    {
+        
+        if (P_motor2 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
+        {                
+            motor2DirectionPin=2;   // Clockwise rotation
+        } 
+        else 
+        {
+             motor2DirectionPin=0;   // Counterclockwise rotation
+        }
             
+        if (fabs(P_motor2) > 0.99 )  // als de absolute waarde van de motorsnelheid groter is als 1, terug schalen naar 1, anders de absolute waarde van de snelheid. (Bij een waarde lager als 0 draait de motor niet)
+        {         
+            motor2 = 0.99 ;
+        } 
+        else 
+        {
+            motor2 = fabs(P_motor2);
+        }            
     }
     
     void motor1_controller(void)
@@ -266,7 +289,11 @@
         motor1_pwm();
     }
 
-    
+    void motor2_controller(void)
+    {
+        error1_motor2 = (Yref_motor2 - positie_motor2);
+        motor2_pwm();
+    }    
     
     // 4.3 State-Machine *******************************************************
 
@@ -286,7 +313,8 @@
                 pc.printf("\r\n Motor Calibration is done!");
                 encoder_motor1.reset();
                 encoder_motor2.reset();
-                Yref_motor1=10000;
+
+                
                 State=StartWait; 
             }
             else {;} //pc.printf("\r\n Motor Calibration is not done!");}
@@ -298,7 +326,9 @@
             led_blue.write(0);
             led_red.write(1);
             led_green.write(1);
-            Yref_motor1=10000;
+            
+            Yref_motor1=5000;
+            Yref_motor2=2000;
             if(button1==0) {State=EMGCalibration;}
             if(button2==0) {State=Demo;} 
             break;             
@@ -315,6 +345,7 @@
             
             led2=1;
             motor1_controller();
+            motor2_controller();
             // State=Operating; 
             break; 
         case Demo:
@@ -371,6 +402,7 @@
 fencoder_motor1() ;
 fencoder_motor2() ;
 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
+PID_controller_motor2(error_integral_motor2, error1_prev_motor2);
 state_machine() ;
 
 // 5.1 Measure Analog and Digital input signals ********************************