Diff: main.cpp

Revision:

1:b75c4b9f1c98

Parent:

0:dbe7715dcfe9

Child:

2:d0076e9d0a7f

--- a/main.cpp	Thu Sep 24 15:30:24 2015 +0000
+++ b/main.cpp	Thu Sep 24 15:58:55 2015 +0000
@@ -2,6 +2,7 @@
 #include "QEI.h"
 #include "HIDScope.h"
 
+//////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
 //info uit
 HIDScope scope(3);
 
@@ -22,20 +23,26 @@
 //frequencies
 //const float pwm_frequency=1000;
 const double hidscope_frequency=100;
-const double p_control_frequency=5;
+const double pi_control_frequency=5;
 
 //tickers
 Ticker hidscope_ticker;
-Ticker p_control_ticker;
+Ticker pi_control_ticker;
 
 //constants
 const int cpr=32*131;
 const float PI=3.1415;
 const float counttorad=((2*PI)/cpr);
-const float motor1_p_kp=0.5;
+
+///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////
 
+//DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
+const float motor1_pi_kp=0.5;
+const double motor1_pi_ki=0.01;
+double motor1_error_int=0;
+////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
 //go flags
-volatile bool scopedata_go=false, p_control_go=false;
+volatile bool scopedata_go=false, pi_control_go=false;
 
 //acvitator functions
 
@@ -43,9 +50,9 @@
 {
     scopedata_go=true;
 }
-void p_control_activate()
+void pi_control_activate()
 {
-    p_control_go=true;
+    pi_control_go=true;
 }
 ///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
 
@@ -57,13 +64,15 @@
     scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
     scope.send();
 }
-
-//re usable P controller
-double p_control(float error,float kp)
+//////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
+// Reusable PI controller
+double pi_control( double e, const double Kp, const double Ki, double Ts, double& motor1_error_int)
 {
-    return (error*kp);
+    motor1_error_int = motor1_error_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
+    return Kp*e+Ki*motor1_error_int;
 }
 
+//////////////////////////////////////////////////MAIN///////////////////////////////////
 int main()
 {
     //set initial shizzle
@@ -72,12 +81,13 @@
 
     //tickers
     hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
-    p_control_ticker.attach(&p_control_activate,1.0/p_control_frequency);
+    pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
 
     while(1) {
         //control motor 1 with a p controller
-        if (p_control_go==true) {
-            float signal_motor1=p_control(2*PI*pot1.read()-counttorad*encoder1.getPulses(),motor1_p_kp);//send error to p controller
+        if (pi_control_go==true) {
+            double error=2*PI*pot1.read()-counttorad*encoder1.getPulses();
+            float signal_motor1=pi_control(error,motor1_pi_kp,motor1_pi_ki,1/pi_control_frequency,motor1_error_int);//send error to p controller
             if (signal_motor1>=0) {//determine CW or CCW rotation
                 motor1_direction.write(CW);
             } else {
@@ -91,7 +101,7 @@
             }
 
             motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
-            p_control_go=false;
+            pi_control_go=false;
         }
         //call scopedata
         if (scopedata_go==true) {