Diff: main.cpp

Revision:

6:a44d6f7626f2

Parent:

5:600c5c9cbb19

Child:

7:948651ca4c0e

--- a/main.cpp	Sun Feb 24 12:29:11 2019 +0000
+++ b/main.cpp	Sun Feb 24 14:09:25 2019 +0000
@@ -39,10 +39,17 @@
 int m_count [2] = {0,0}; //setup array for 2 encoder counts
 int m_last_count [2] = {0,0}; //setup array for storing previous encoder counts
 int m_speed_ref [2] = {0,0}; //setup array for 2 motor speeds
+int m_max_duty [2] = {0,0};
 float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
 int m_distance_ref [2] = {0,0}; //setup array for 2 motor distances
 int max_accel = 0; //for storing the maximum acceleration
 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
+int distance_scale = 14; 
+//max number over SPI is 100. Scale to make this equivalent to 1m 
+//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
+
+bool control_mode = true; //control mode flag. True is speed control and False is distance control
 
 void readADC(); //read 6 channels of ADC
 void setDuty(); //set the 4 motor PWM duty cycles
@@ -67,6 +74,12 @@
     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;
+    
     
     //setup interrupts for encoders
     MRF_ENC1.fall(&mrfEncoderIsr1);
@@ -89,18 +102,34 @@
  //       pc.printf("Motor count %i\r\n", m_count[0]);
 
         if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
-            for(i=0;i<=1;i++){ //do this for right and left in turn
-                pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
-                m_last_count[i] = m_count[i]; //store count for next cycle
-                speed_error = pos_change - m_speed_ref[i]; //calculate different between current speed and reference speed
-                integral[i] = integral[i] + speed_error;
-                m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
+            if(control_mode == true) { //speed control
+                for(i=0;i<=1;i++){ //do this for right and left in turn
+                    pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
+                    m_last_count[i] = m_count[i]; //store count for next cycle
+                    speed_error = pos_change - m_speed_ref[i]; //calculate different between current speed and reference speed
+                    integral[i] = integral[i] + speed_error;
+                    m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
+                }
+                if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
+                    m_duty[0] = 0;
+                if(m_speed_ref[1] == 0)
+                    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
+                    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
+                    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;
+                    }
+                    else if(m_duty[i] < -1*max_duty){
+                        m_duty[i] = -1*max_duty;
+                    }
+                 } 
             }
     
-            if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
-                m_duty[0] = 0;
-            if(m_speed_ref[1] == 0)
-                m_duty[1] = 0;
+
                  
             if(m_duty[0] < 0){
                 setRMotorDir(0); //set the motors backwards
@@ -198,19 +227,26 @@
             }
             v = rpi.read(); //last bit setup a blank reply
             rpi.reply(0x00);
+            control_mode = true; //set the controller go do distance control
+            m_count[0] = 0; //reset encoder counts to avoid overflow
+            m_count[1] = 0;
         }
         else if(v == char('D')){
             rpi.reply(0x02);
             for (i=0;i<=1;i++){                   
-                m_speed_ref[i] = rpi.read() - 128;
-                rpi.reply(m_speed_ref[i]);
+                m_distance_ref[i] = rpi.read() - 128;
+                rpi.reply(m_distance_ref[i]);
+                m_distance_ref[i] = m_distance_ref[i] * distance_scale;
             }
             for (i=0;i<=1;i++){                   
-                m_distance_ref[i] = rpi.read() - 128;
-                rpi.reply(m_distance_ref[i]);
+                m_max_duty[i] = rpi.read() - 128;
+                rpi.reply(m_max_duty[i]);
             }
             v = rpi.read(); //last bit setup a blank reply
             rpi.reply(0x00);
+            control_mode = false; //set the controller go do distance control
+            m_count[0] = 0; //reset encoder counts to avoid overflow
+            m_count[1] = 0;
         }
         else if(v == char('A')){
             rpi.reply(0x03);