Diff: main.cpp

Revision:

8:826ec74d53c9

Parent:

7:948651ca4c0e

Child:

9:0d274fc2e6df

diff -r 948651ca4c0e -r 826ec74d53c9 main.cpp
--- a/main.cpp	Sun Feb 24 14:25:23 2019 +0000
+++ b/main.cpp	Thu Feb 28 21:45:11 2019 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
-
+ 
 //Serial pc(SERIAL_TX, SERIAL_RX);
-
-
+ 
+ 
 //Analog inputs A0-A5 addressable as such. A6 to A9 do not work.
 AnalogIn ANALOG0(A0);
 AnalogIn ANALOG1(A1);
@@ -31,46 +31,48 @@
 InterruptIn MLF_ENC1(PC_11); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB
 InterruptIn MLF_ENC2(PC_10); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB
 Ticker      control_timer;
-
+ 
 bool        control_loop_flag = false;
-
+ 
 //motor associations within arrays
 //Right = 0; Left = 1
 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};
+int m_top_speed [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 max_accel = 5; //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
-
+ 
 float integral [2] = {0,0};
 bool control_mode = true; //control mode flag. True is speed control and False is distance control
-
+bool m_direction [2] = {true, true};
+ 
 void readADC(); //read 6 channels of ADC
 void setDuty(); //set the 4 motor PWM duty cycles
 void setPeriod(int period); //set PWM period for motors in microseconds
 void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
 void spiComms(); //do spi communications with raspberry pi
-
+ 
 void mrfEncoderIsr1();
 void mrfEncoderIsr2();
 void mlfEncoderIsr1();
 void mlfEncoderIsr2();
 void controlTick();
-
-
+float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) ;
+ 
+ 
 int main()
 {
     float pos_change = 0; //temp variable for speed control
     float speed_error = 0; //temp variable for speed control
-
+ 
     float Kp = 0.01; //proportional constant
     float Ki = 0.01; //integral constant
     int i = 0;
@@ -95,11 +97,11 @@
     //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]);
-
+ 
         if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
             if(control_mode == true) { //speed control
                 for(i=0;i<=1;i++){ //do this for right and left in turn
@@ -115,18 +117,31 @@
                     m_duty[1] = 0;
             }
             else if(control_mode == false) { //distance x and max speed y control
-                
+                int targetSpeed=0;
                 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; //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;
-                    }
-                } 
+                    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
+                    targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], m_direction[i]);//m_speed_ref is used as top speed
+                    speed_error = pos_change - targetSpeed; //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
+                
+                
+                
+                    //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; //speed proportional control
+                    //max_duty = float(m_top_speed[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;*/
             }
     
 
@@ -153,12 +168,17 @@
             setDuty(); //set all the duty cycles to the motor drivers
             control_loop_flag = false;
         }
-        
+        for(i=0;i<1;i++){
+            if(m_distance_ref[i]<0)
+                m_direction[i] = false;
+            else
+                m_direction[i] = true;
+        }
         readADC(); //read all the ADC values when not doing something else
         spiComms(); //do SPI communication stuff
     }
 }
-
+ 
 //function to read all 6 ADC channels
 void readADC(){
     adc_values[0] = int(ANALOG0.read()*255);
@@ -168,7 +188,7 @@
     adc_values[4] = int(ANALOG4.read()*255);
     adc_values[5] = int(ANALOG5.read()*255);
 }
-
+ 
 //function to set all 4 motor PWM duty cycles
 void setDuty(){
     MRB.write(m_duty[0]);
@@ -200,7 +220,7 @@
         RAIN2 = 0;
     }
 }
-
+ 
 //initialisation function to set motor PWM period and set to 0 duty
 void setPeriod(int period){
     MRB.period_us(period);
@@ -212,7 +232,7 @@
     MLF.period_us(period);
     MLF.write(0.0);
 }
-
+ 
 void spiComms(){
        //do SPI communication stuff
     int i = 0; //temp counter variable
@@ -241,14 +261,16 @@
                 m_distance_ref[i] = m_distance_ref[i] * distance_scale;
             }
             for (i=0;i<=1;i++){                   
-                m_max_duty[i] = rpi.read() - 128;
-                rpi.reply(m_max_duty[i]);
+                m_top_speed[i] = rpi.read() - 128;
+                rpi.reply(m_top_speed[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;
+            m_last_count[0] = 0;
+            m_last_count[1] = 0;
         }
         else if(v == char('A')){
             rpi.reply(0x03);                 
@@ -296,8 +318,61 @@
         m_count[1] --;
     }
 }
-
+ 
 void controlTick()
 {
     control_loop_flag = true;
 }
+
+        
+float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) {
+//targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], true);//m_speed_ref is used as top speed
+//note to self: check direction
+    
+ //   end = end/float(distance_scale);
+    float minSpeed=1;//deg per sec - to prevent motor speed=0 and therefore jam by delay=infinity.
+    float halfWay = 0.0;
+    float v = 0.0;
+    float midPointVsqrd = 0.0;
+    halfWay=start+(end-start)/2;
+      
+  //  if (direction == true) {
+        if (abs(currentPosition)>abs(halfWay)) {
+          //deaccelarate
+          midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-start);
+          v=sqrt(midPointVsqrd-2*accel*(currentPosition-halfWay));
+        } else {
+          //accelerate
+          //v^2=u^2+2as
+          v=sqrt(minSpeed*minSpeed+2*accel*(currentPosition-start));
+        }
+ /*   } else {
+        if (currentPosition<halfWay) {
+           //deaccelarate
+           midPointVsqrd=minSpeed*minSpeed+2*accel*(start-halfWay);
+           v=sqrt(midPointVsqrd-2*accel*(halfWay-currentPosition));
+        } else {
+          //accelerate
+          //calc velocity
+          //v^2=u^2+2as
+          v=sqrt(minSpeed*minSpeed+2*accel*(start-currentPosition));
+        } 
+    } */
+
+    if (v<minSpeed && v > 0) {
+       v=minSpeed;
+    }
+    else if (abs(v) < minSpeed && v < 0){
+        v = -1*minSpeed;
+    } 
+
+    if (v>topSpeed && v > 0) {
+        v=topSpeed;
+    }
+    else if (abs(v) > topSpeed && v < 0){
+        v = -1*topSpeed;
+    }
+
+    return v;
+    //return topSpeed;
+}