Diff: main.cpp

Revision:

7:948651ca4c0e

Parent:

6:a44d6f7626f2

Child:

8:826ec74d53c9

diff -r a44d6f7626f2 -r 948651ca4c0e main.cpp
--- a/main.cpp	Sun Feb 24 14:09:25 2019 +0000
+++ b/main.cpp	Sun Feb 24 14:25:23 2019 +0000
@@ -49,6 +49,7 @@
 //with 48mm diameter wheels and 210 counts per revolution. Circumference = pi*D = 0.1508m per rev.
 //1m/0.1508m = 6.63. 6.63*210 = 1392 counts. So scale to make 100 = 1400 counts
 
+float integral [2] = {0,0};
 bool control_mode = true; //control mode flag. True is speed control and False is distance control
 
 void readADC(); //read 6 channels of ADC
@@ -69,15 +70,13 @@
 {
     float pos_change = 0; //temp variable for speed control
     float speed_error = 0; //temp variable for speed control
-    float integral [2] = {0,0};
+
     float Kp = 0.01; //proportional constant
     float Ki = 0.01; //integral constant
     int i = 0;
     
     float distance_error = 0;
     float Kp_d = 0.01;
-    float Ki_d = 0.001;
-    float integral_d [2]  = {0,0};
     float max_duty = 0;
     
     
@@ -116,9 +115,10 @@
                     m_duty[1] = 0;
             }
             else if(control_mode == false) { //distance x and max speed y control
-                 for(i=0;i<=1;i++){ //do this for right and left in turn
+                
+                for(i=0;i<=1;i++){ //do this for right and left in turn
                     distance_error = float(m_count[i]-m_distance_ref[i]); //calculate difference between current speed and reference speed
-                    m_duty[i] = Kp_d*distance_error + Ki_d*integral_d[i]; //speed proportional control
+                    m_duty[i] = Kp_d*distance_error; //speed proportional control
                     max_duty = float(m_max_duty[i])/100;
                     if(m_duty[i] > max_duty){ //check the max duty hasn't been exceeded
                         m_duty[i] = max_duty;
@@ -126,7 +126,7 @@
                     else if(m_duty[i] < -1*max_duty){
                         m_duty[i] = -1*max_duty;
                     }
-                 } 
+                } 
             }
     
 
@@ -230,6 +230,8 @@
             control_mode = true; //set the controller go do distance control
             m_count[0] = 0; //reset encoder counts to avoid overflow
             m_count[1] = 0;
+            integral[0] = 0; //reset integrals to avoid odd behaviour
+            integral[1] = 0;
         }
         else if(v == char('D')){
             rpi.reply(0x02);