Diff: main.cpp

Revision:

5:600c5c9cbb19

Parent:

4:abf0070897ff

Child:

6:a44d6f7626f2

diff -r abf0070897ff -r 600c5c9cbb19 main.cpp
--- a/main.cpp	Sun Feb 24 12:00:49 2019 +0000
+++ b/main.cpp	Sun Feb 24 12:29:11 2019 +0000
@@ -36,10 +36,11 @@
 
 //motor associations within arrays
 //Right = 0; Left = 1
-int m_count [2] = {0,0}; //setup array for 4 encoder counts
-int m_speed_ref [2] = {0,0}; //setup array for 4 motor speeds
+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
 float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
-int m_distance_ref [2] = {0,0}; //setup array for 4 motor distances
+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
 
@@ -54,12 +55,19 @@
 void mrfEncoderIsr2();
 void mlfEncoderIsr1();
 void mlfEncoderIsr2();
+void controlTick();
 
 
 int main()
 {
-    float right_speed = 0;
-    float left_speed = 1;
+    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;
+    
+    
     //setup interrupts for encoders
     MRF_ENC1.fall(&mrfEncoderIsr1);
     MRF_ENC2.fall(&mrfEncoderIsr2);
@@ -72,36 +80,51 @@
     // Set PWM period in us
     setPeriod(100);
  //   pc.printf("Starting up");
+    //setup control loop
+    control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms
+    
+
     
     while (1) {
  //       pc.printf("Motor count %i\r\n", m_count[0]);
-        //write motor speed
-        
-        right_speed = float(m_speed_ref[0])/100.0;
-        if(right_speed < 0){
-            setRMotorDir(0); //set the motors backwards
-            right_speed = -1*right_speed; //make the negative value into positive
-        } else {
-            setRMotorDir(1); //set the motors forwards
+
+        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(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
+                m_duty[0] = -1*m_duty[0]; //make the negative value into positive
+            } else {
+                setRMotorDir(1); //set the motors forwards
+            }
+            if(m_duty[0] > 100){ 
+                m_duty[0] = 100; //make sure the speed isn't greater than 100%
+            }
+            
+            if(m_duty[1] < 0){
+                setLMotorDir(0); //set the motors backwards
+                m_duty[1] = -1*m_duty[1]; //make the negative value into positive
+            } else {
+                setLMotorDir(1); //set the motors forwards
+            }
+            if(m_duty[1] > 100){ 
+                m_duty[1] = 100; //make sure the speed isn't greater than 100%
+            }                   
+            setDuty(); //set all the duty cycles to the motor drivers
+            control_loop_flag = false;
         }
-        if(right_speed > 100){ 
-            right_speed = 100; //make sure the speed isn't greater than 100%
-        }
-        m_duty[0] = right_speed;
         
-        left_speed = float(m_speed_ref[1])/100.0;
-        if(left_speed < 0){
-            setLMotorDir(0); //set the motors backwards
-            left_speed = -1*left_speed; //make the negative value into positive
-        } else {
-            setLMotorDir(1); //set the motors forwards
-        }
-        if(left_speed > 100){ 
-            left_speed = 100; //make sure the speed isn't greater than 100%
-        }
-        m_duty[1] = left_speed;
-               
-        setDuty(); //set all the duty cycles to the motor drivers
         readADC(); //read all the ADC values when not doing something else
         spiComms(); //do SPI communication stuff
     }
@@ -235,3 +258,8 @@
         m_count[1] --;
     }
 }
+
+void controlTick()
+{
+    control_loop_flag = true;
+}