Diff: main.cpp

Revision:

14:236ae2d7ec41

Parent:

13:db1a8b51706b

Child:

15:849e0fc5d3a8

--- a/main.cpp	Tue Oct 29 16:13:50 2019 +0000
+++ b/main.cpp	Tue Oct 29 20:07:26 2019 +0000
@@ -39,8 +39,8 @@
 MODSERIAL pc(USBTX, USBRX); // Serial communication with the board
 QEI encoder_motor1(D12,D13,NC,64); // Defines encoder for motor 1
 QEI encoder_motor2(D10,D11,NC,64); // Defines encoder for motor 1
-double f=1/100;                     // Frequency
-const double Ts = f/10;            // Sampletime
+double f=1/100;                     // Frequency, currently unused
+const double Ts = 0.001;            // Sampletime
 HIDScope scope(2);                  // Amount of HIDScope servers
 
 
@@ -81,7 +81,10 @@
     double kp_motor1;
     double Ki_motor1;
     double Kd_motor1;
-
+    double Up_motor1;
+    double Ui_motor1;
+    double Ud_motor1;
+    
     double positie_motor1;                              //counts encoder
     double error1_motor1;
     double error1_prev_motor1;
@@ -119,8 +122,12 @@
     void HIDScope() //voor HIDscope
     {
         scope.set(0, positie_motor1);
-       scope.set(1, positie_prev_motor1); //nog te definieren wat we willen weergeven
-       scope.set(2, positie_verschil_motor1); //nog te definieren wat we willen weergeven
+       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(4, Ui_motor1);
+   //    scope.set(5, Uk_motor1);
+       
 
         scope.send();   
     }
@@ -154,11 +161,11 @@
         while (abs(positie_verschil_motor1)>5)
         {
             motor1=0.2  ; 
-            pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
+    //        pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
         }
         motor1=0.0;
         motor1_calibrated=true;  
-        pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
+     //   pc.printf("\r\n Motor1 kalibratie = %s", motor1_calibrated ? "true" : "false");
     
     
     
@@ -169,39 +176,41 @@
         while (abs(positie_verschil_motor2)>5)
         {
             motor2=0.2  ; 
-            pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
+  //          pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
             led2=1;
         }
         motor2=0.0;
         led2=0;
         motor2_calibrated=true;  
-        pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
+    //    pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
         
             
     }
     
     // 4.2a PID-Controller motor 1**************************************************
-    double PID_controller_motor1()
+    double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
     {
         //Proportional part
         kp_motor1 = 0.01 ;      // moet nog getweaked worden
-        double Up_motor1 = kp_motor1 * error1_motor1;
+        Up_motor1 = kp_motor1 * error1_motor1;
     
         //Integral part
         Ki_motor1 = 0.0001;     // moet nog getweaked worden
         error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
-        double Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
+        Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
     
         //Derivative part 
         Kd_motor1 = 0.01 ;// moet nog getweaked worden  
-        error1_derivative_motor1 = (error1_motor1  -  error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
+        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
-        double Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
+        Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
         error1_prev_motor1 = error1_motor1;
 
-        double P_motor1 = Up_motor1 + Ui_motor1 + Ud_motor1;                                           //sommatie van de u's
+        P_motor1 = Up_motor1 + Ui_motor1 + Ud_motor1;                                           //sommatie van de u's
+        
+        return P_motor1; 
+        
 
-        return P_motor1; 
     }
 
     // 4.2b PID-Controller motor 2**************************************************
@@ -228,6 +237,35 @@
         return P_motor2; 
     }
     
+    double motor1_pwm()
+    {
+        
+        if (P_motor1 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
+        {                
+            motor1DirectionPin=1;   // Clockwise rotation
+        } 
+        else 
+        {
+             motor1DirectionPin=0;   // Counterclockwise rotation
+        }
+            
+        if (fabs(P_motor1) > 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)
+        {         
+            motor1 = 0.99 ;
+        } 
+        else 
+        {
+            motor1 = fabs(P_motor1);
+        }
+            
+    }
+    
+    void motor1_controller(void)
+    {
+        error1_motor1 = (Yref_motor1 - positie_motor1);
+        motor1_pwm();
+    }
+
     
     
     // 4.3 State-Machine *******************************************************
@@ -243,14 +281,16 @@
             led_red.write(1);
             led_green.write(0); //Green Led on when in this state
             
-            if (motor1_calibrated==true&&motor2_calibrated==true) 
+            if (motor1_calibrated==true&&motor2_calibrated==true)
             {
                 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!");}
             
-            
             break; 
                 
         case StartWait:  
@@ -258,7 +298,7 @@
             led_blue.write(0);
             led_red.write(1);
             led_green.write(1);
-           
+            Yref_motor1=10000;
             if(button1==0) {State=EMGCalibration;}
             if(button2==0) {State=Demo;} 
             break;             
@@ -271,8 +311,11 @@
             State=Homing; 
             break; 
         case Homing: 
-  /*          pc.printf("\r\n State: Homing");
-            State=Operating; */
+       //     pc.printf("\r\n State: Homing");
+            
+            led2=1;
+            motor1_controller();
+            // State=Operating; 
             break; 
         case Demo:
            pc.printf("\r\n State: Demo");
@@ -327,6 +370,7 @@
 //    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() ;
+PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
 state_machine() ;
 
 // 5.1 Measure Analog and Digital input signals ********************************
@@ -353,11 +397,12 @@
                 "Mevlid Yildirim      - s2005735 \r\n");
     led_green.write(1);
     led_red.write(1);
-    led_blue.write(0);
-    State = MotorCalibration;
+    led_blue.write(1);
+    State = StartWait ; // veranderen inMotorCalibration;
+    ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     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
-    motor_calibration();
+    
+    //motor_calibration();
     
     
 // 6.2 While loop in main function**********************************************