Revision 11:4b0dbbdc56fb, committed 2018-10-22

Comitter:

Florianhs

Date:

Mon Oct 22 10:01:47 2018 +0000

Parent:

10:61a7cb3b3aa3

Commit message:

P(I) controller voor 2 motoren, aangestuurd met potmeters voor positite

Changed in this revision

--- a/main.cpp	Wed Oct 17 13:50:35 2018 +0000
+++ b/main.cpp	Mon Oct 22 10:01:47 2018 +0000
@@ -4,20 +4,35 @@
 #include "math.h"
 #define _USE_MATH_DEFINES
 # define M_PI           3.14159265358979323846  /* pi */
-
-double Kp = 1;
+//motor 1
+double Kp = 5;
 double Ki = 1;
 double u_k = 1;
 double u_i = 1;
 double Kswitch;
 double kp_scalar = 1;
 double ki_scalar = 1;
-double y_ref=0;
+double m1_scalar;
 double motor1_ang=0;
-double y_ref_scalar = 0;
+double m1=0;
 double input_motor;
+//motor 2
+double Kp2 = 10;
+double Ki2 = 1;
+double u_k2 = 1;
+double u_i2 = 1;
+double Kswitch2;
+double kp_scalar2 = 1;
+double ki_scalar2 = 1;
+double m2_scalar;
+double motor2_ang=0;
+double m2=0;
+double input_motor2;
+
+//rest
 bool input_switch = true;
 const double Ts = 0.01f;
+volatile char input;
 
 Ticker Control;
 
@@ -49,42 +64,83 @@
     input_switch=!input_switch;             //verander de potmeter input tussen kp en ki
 }
 
-double PID_controller(double error)         //bereken de input die naar de motor wordt gestuurd aan de hand van kp en ki en de error op elk moment
+double PID_controller1(double error)         //bereken de input die naar de motor wordt gestuurd aan de hand van kp en ki en de error op elk moment
             {
             //Proportional part
             u_k = Kp*error;
             
             //Integral part
-            static double error_integral = 0;
-            error_integral = error_integral + error * Ts;
-            u_i = Ki * error_integral;
-            return u_k + u_i;
+            //static double error_integral = 0;
+            //error_integral = error_integral + error * Ts;
+            //u_i = Ki * error_integral;
+            return u_k; //+ u_i;
             }
 
-
-void aansturing()
-    {       
-            y_ref_scalar = pot2.read();     //altijd de potmeters uitlezen, buiten de ticker
-            Kswitch = pot1.read();
+double PID_controller2(double error2)         //bereken de input die naar de motor wordt gestuurd aan de hand van kp en ki en de error op elk moment
+            {
+            //Proportional part
+            u_k2 = Kp2*error2;
+            
+            //Integral part
+            //static double error_integral2 = 0;
+            //error_integral2 = error_integral2 + error2 * Ts;
+            //u_i2 = Ki2 * error_integral2;
+            return u_k2; //+ u_i2;
+            }
             
-            //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
-            if(input_switch==true)
-            {kp_scalar = Kswitch;}
-            else
-            {ki_scalar = Kswitch;}
+void aansturing()
+    {       //posities voor de motoren bepalen   
+//            if(input=='a')
+//               {if(m1>0.1f)
+//                {m1 = m1-0.01f;}}
+//            else if(input=='s')
+//                {if(m1<2.1f)
+//                {m1 = m1+0.01f;}}
+//            else if(input=='q')
+//                {if(m2>0.01f)
+//                {m2 = m2-0.01f;}}
+//            else if(input=='w')
+//                {if(m2<2.1f)
+//                {m2 = m2+0.01f;}}
+            m1_scalar=pot1.read();
+            m2_scalar=pot2.read();
+            m1=m1_scalar*1.5f;
+            m2=m2_scalar*1.5f;
+            //Kswitch = pot1.read();      //kp en ki bepalen voor motor 1
+            //Kswitch2 = pot2.read();     //kp en ki bepalen voor motor 2
+            
+            //kp_scalar=Kswitch;
+            //kp_scalar2=Kswitch2;
+//            //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
+//            if(input_switch==true)
+//            {kp_scalar = Kswitch;}
+//            else
+//            {ki_scalar = Kswitch;}
+//            
+//            if(input_switch==true)
+//            {kp_scalar2 = Kswitch2;}
+//            else
+//            {ki_scalar2 = Kswitch2;}
+            
+            //waarde voor de inputcontrol value instellen aan de hand van potmeter
+           // Kp = kp_scalar*20.0f;
+           // Ki = ki_scalar*2.0f;
+           // Kp2 = kp_scalar2*20.0f;
+           // Ki2 = ki_scalar2*2.0f;
             
             
-            //waarde voor de inputcontrol value instellen aan de hand van potmeter
-            Kp = kp_scalar*20.0f;
-            Ki = ki_scalar*2.0f;
-            
-            y_ref = y_ref_scalar*2.0f*M_PI;                          // de postitie waar je naartoe wilt instellen tussen 0 en 2pi aan de hand van de potmeter waarde
+           
             motor1_ang = countstoangle(motor1_enc.getPulses());      // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
-            double error = motor1_ang -y_ref;
+            double error = motor1_ang -m1;
             
+            motor2_ang = countstoangle(motor2_enc.getPulses());      // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
+            double error2 = motor2_ang -m2;
             
-            double input_motor = PID_controller(error);
+            //input voor de motor met de PID controllers bepalen
+            double input_motor = PID_controller1(error);
+            double input_motor2 = PID_controller2(error2);
             
+            //signaal naar motor 1 sturen
             if(input_motor>1.0)                 //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
                 {input_motor = 1.0;}
             else if(input_motor<-1.0)           //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
@@ -96,6 +152,19 @@
             else                                
                 {motor1_dir = 1;}                      
             motor1_pwm.write(fabs(input_motor));    // de uiteindelijke input aan de motor geven 
+            
+            //signaal naar motor 12 sturen
+            if(input_motor2>1.0)                 //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
+                {input_motor2 = 1.0;}
+            else if(input_motor2<-1.0)           //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
+                {input_motor2 = -1.0;}
+            
+                    
+            if(input_motor2<0.0)                 //bepaal de richting aan de hand van het teken van het pwm signaal.
+                {motor2_dir = 0;}
+            else                                
+                {motor2_dir = 1;}                      
+            motor2_pwm.write(fabs(input_motor2));    // de uiteindelijke input aan de motor geven 
  //   pc.printf("ki: %f  kp: %f  yref: %f     error: %f   u_k: %f  angle: %f\r\n",Ki, Kp, y_ref,error,u_k,motor1_ang);
     }
 
@@ -115,13 +184,17 @@
     
     while (true)
     {       
+            //input = pc.getc();
             if(input_switch==true)
             {pc.printf("Kp ingeschakeld\r\n");}
             else
             {pc.printf("Ki ingeschakeld\r\n");}
-            double errorprint = motor1_ang-y_ref;
-                pc.printf("ki: %f   kp: %f  yref: %f    u_k: %f angle: %f   error: %f\r\n",Ki, Kp, y_ref,u_k,motor1_ang,errorprint);
-
+            
+            double errorprint = motor1_ang-m1;
+            double errorprint2 = motor2_ang-m2;
+            pc.printf("ki1: %f   kp1: %f  m1: %f   u_k1: %f  angle1: %f   error1: %f\r\n",Ki, Kp, m1,u_k,motor1_ang,errorprint);
+            pc.printf("ki2: %f   kp2: %f  m2: %f   u_k2: %f  angle2: %f   error2: %f\r\n",Ki2, Kp2, m2,u_k2,motor2_ang,errorprint2);
+            pc.printf("toets=%c\r\n",input);
             wait(0.5);  
     }
 }