Revision 10:61a7cb3b3aa3, committed 2018-10-17

Comitter:

Florianhs

Date:

Wed Oct 17 13:50:35 2018 +0000

Parent:

9:466dff9ae128

Commit message:

Werkend programma om de PI controller motor aan te sturen en met een switch te kiezen tussen Kp en Ki;

Changed in this revision

--- a/main.cpp	Fri Oct 05 12:51:06 2018 +0000
+++ b/main.cpp	Wed Oct 17 13:50:35 2018 +0000
@@ -5,9 +5,21 @@
 #define _USE_MATH_DEFINES
 # define M_PI           3.14159265358979323846  /* pi */
 
-DigitalOut ledr(LED_RED);
-DigitalOut ledg(LED_GREEN);
-DigitalOut ledb(LED_BLUE);
+double Kp = 1;
+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 motor1_ang=0;
+double y_ref_scalar = 0;
+double input_motor;
+bool input_switch = true;
+const double Ts = 0.01f;
+
+Ticker Control;
 
 PwmOut  motor1_pwm(D5);
 PwmOut  motor2_pwm(D6);
@@ -24,57 +36,93 @@
 
 MODSERIAL pc(USBTX, USBRX);
 
-Ticker      sample_timer;
-
-const float sample_time = 0.05; //s
-const float sample_frequency = 200; //hz
-
+//hoek uitrekenen van de motor
 double countstoangle(int counts){
     double angle;
-    int counts_abs = abs(counts);
-    if(counts_abs >= 8400){
-       int partial_counts = counts_abs % 8400;
-       angle = (2 * M_PI * partial_counts)/8400;
-       return angle;
-    }else{
-        angle = (2 * M_PI * counts_abs)/8400;
-        return angle;
+    int counts_abs =(counts);
+    angle = (2 * M_PI * counts_abs)/8400;
+    return angle;
+    
+}
+void buttonpress()
+{
+    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
+            {
+            //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;
+            }
+
+
+void aansturing()
+    {       
+            y_ref_scalar = pot2.read();     //altijd de potmeters uitlezen, buiten de ticker
+            Kswitch = pot1.read();
+            
+            //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;}
+            
+            
+            //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 input_motor = PID_controller(error);
+            
+            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
+                {input_motor = -1.0;}
+            
+                    
+            if(input_motor<0.0)                 //bepaal de richting aan de hand van het teken van het pwm signaal.
+                {motor1_dir = 0;}
+            else                                
+                {motor1_dir = 1;}                      
+            motor1_pwm.write(fabs(input_motor));    // 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);
     }
-}
 
 int main()
 {
-    ledr = 1;
-    ledg = 1;
-    ledb = 1;
     
+    sw2.fall(buttonpress);
+    pc.baud(115200);
     pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
     
     motor1_pwm.period_us(60);
     motor1_pwm = 0;  
     motor2_pwm.period_us(60);
     motor2_pwm = 0;  
-  
+    
+    Control.attach(aansturing, Ts);         //de aansturing regelen met een sampletime
+    
     while (true)
-    {
-            float pot1_float = pot1.read();
-            float pot1_motor = (pot1_float * 2.0f) - 1.0f;
-            int mot1_direction = pot1_motor >= 0;
-            motor1_pwm.write(fabs(pot1_motor));
-            motor1_dir = !mot1_direction;
-            
-            float pot2_float = pot2.read();
-            float pot2_motor = (pot2_float * 2.0f) - 1.0f;
-            int mot2_direction = pot2_motor >= 0;
-            motor2_pwm.write(fabs(pot2_motor));
-            motor2_dir = mot2_direction;
-            
-            double motor1_ang = countstoangle(motor1_enc.getPulses());
-            double motor2_ang = countstoangle(motor2_enc.getPulses());
-            
-            pc.printf("Angle motor 1: %f pi rad, angle motor 2: %f pi rad.\r\n", motor1_ang/(M_PI), motor2_ang/(M_PI));
-            
-            wait(0.2f);
+    {       
+            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);
+
+            wait(0.5);  
     }
 }