Diff: main.cpp

Revision:

1:b66e14435f70

Parent:

0:85ca550760e9

Child:

2:02b385b96543

diff -r 85ca550760e9 -r b66e14435f70 main.cpp
--- a/main.cpp	Mon Oct 09 13:52:37 2017 +0000
+++ b/main.cpp	Wed Oct 11 09:11:20 2017 +0000
@@ -25,18 +25,30 @@
 Ticker controller;
 
 const double pi = 3.1415926535897;
-const float MOTOR1_KP = 5;
+const float M1_KP = 10, M1_KI = 0.0;
+const double M1_TS = 0.0001;
 const float RAD_PER_PULSE = (2*pi)/4200;
-const float CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ
+const float CONTROLLER_TS = 0.0001;     //TIME INTERVAL/ hZ
+double m1_err_int = 0;
 
-float P(double error, const float Kp){
-    return Kp * error;
-    }
+// the working P controller beneath here
+//float P(double error, const float Kp){
+//  return Kp * error;
+//    }
+
+// New PI controller
+
+double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){
+    e_int += Ts*e;
+    return Kp*e+Ki*e_int;
+}
+
+/* Working P controller part
 
 void motor1_Controller(){
     double reference = 10*potMeter1.read();
     double position = RAD_PER_PULSE*Encoder1.getPulses();
-    double motor1 = P(reference-position, MOTOR1_KP);
+    double motor1 = P(reference-position, M1_KP);
     motor1PWM = motor1;
     
     if(motor1 > 0.1){
@@ -64,15 +76,55 @@
     
     
 }
+*/
+
+// New PI part 
+
+void m1_Controller(){
+    double reference = 10*potMeter1.read();
+    double position = RAD_PER_PULSE*Encoder1.getPulses();
+    float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
+    
+    motor1PWM = motor1;
+    
+    if(motor1 > 0.5){
+        motor1DC = 1;
+        
+        ledr = 1;
+        ledg = 1;       //Blau
+        ledb = 0;
+    }
+    else if (motor1<-0.5) {
+        motor1DC = 0; 
+        
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;       //Groen
+        
+    }
+    else{ 
+    motor1PWM = 0;
+        
+        ledb = 1;       //Rood
+        ledr = 0;
+        ledg = 1;
+    }
+    
+    
+}
 
 int main(){
     pc.baud(115200);
-    controller.attach(&motor1_Controller, CONTROLLER_TS);      
+    //controller.attach(&m1_Controller, CONTROLLER_TS);            --> P one
+    
+    controller.attach(&m1_Controller, M1_TS);
     
     while(1){    
     double reference = 10*potMeter1.read();
     double position = RAD_PER_PULSE*Encoder1.getPulses();
-    double motor1 = P(reference-position, MOTOR1_KP);
+    // double motor1 = P(reference-position, M1_KP); --> old one
+    float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
+    
       pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);         
     }
 }