Diff: main.cpp

Revision:

9:0d274fc2e6df

Parent:

8:826ec74d53c9

Child:

10:63381713610c

--- a/main.cpp	Thu Feb 28 21:45:11 2019 +0000
+++ b/main.cpp	Sun Mar 03 11:16:16 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,9 +31,9 @@
 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
@@ -44,70 +44,70 @@
 int m_distance_ref [2] = {0,0}; //setup array for 2 motor distances
 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 
+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};
- 
+//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) ;
- 
- 
+float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed) ;
+
+
 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;
-    
-    float distance_error = 0;
-    float Kp_d = 0.01;
-    float max_duty = 0;
-    
-    
+
+//    float distance_error = 0;
+//    float Kp_d = 0.01;
+//    float max_duty = 0;
+
+
     //setup interrupts for encoders
     MRF_ENC1.fall(&mrfEncoderIsr1);
     MRF_ENC2.fall(&mrfEncoderIsr2);
     MLF_ENC1.fall(&mlfEncoderIsr1);
     MLF_ENC2.fall(&mlfEncoderIsr2);
-    
+
     //setup driver pins
     setLMotorDir(1);
     setRMotorDir(1);
     // Set PWM period in us
     setPeriod(100);
- //   pc.printf("Starting up");
+//   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]);
- 
+//       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
+                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
+                    speed_error = pos_change - float(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
                 }
@@ -115,72 +115,49 @@
                     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
+            } 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
+                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
-                    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
+                    targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[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;
-                    //}
+                    m_duty[i] = Kp*0.1*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){
+            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){ 
+            if(m_duty[0] > 100) {
                 m_duty[0] = 100; //make sure the speed isn't greater than 100%
             }
-            
-            if(m_duty[1] < 0){
+
+            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){ 
+            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;
         }
-        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(){
+void readADC()
+{
     adc_values[0] = int(ANALOG0.read()*255);
     adc_values[1] = int(ANALOG1.read()*255);
     adc_values[2] = int(ANALOG2.read()*255);
@@ -188,41 +165,43 @@
     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(){
+void setDuty()
+{
     MRB.write(m_duty[0]);
     MRF.write(m_duty[0]);
     MLB.write(m_duty[1]);
     MLF.write(m_duty[1]);
 }
 //set left motor direction. 1 is forward, 0 is backwards.
-void setLMotorDir(bool direction){
+void setLMotorDir(bool direction)
+{
     LSTANDBY = 1;
-    if(direction == true){
+    if(direction == true) {
         LAIN1 = 1;
         LAIN2 = 0;
-    }
-    else if(direction == false){
+    } else if(direction == false) {
         LAIN1 = 0;
         LAIN2 = 1;
     }
 }
 //set right motor direction. 1 is forward, 0 is backwards.
-void setRMotorDir(bool direction){
+void setRMotorDir(bool direction)
+{
     RSTANDBY = 1;
-    if(direction == true){
+    if(direction == true) {
         RAIN1 = 0;
         RAIN2 = 1;
-    }
-    else if(direction == false){
+    } else if(direction == false) {
         RAIN1 = 1;
         RAIN2 = 0;
     }
 }
- 
+
 //initialisation function to set motor PWM period and set to 0 duty
-void setPeriod(int period){
+void setPeriod(int period)
+{
     MRB.period_us(period);
     MRB.write(0.0);
     MRF.period_us(period);
@@ -232,16 +211,17 @@
     MLF.period_us(period);
     MLF.write(0.0);
 }
- 
-void spiComms(){
-       //do SPI communication stuff
+
+void spiComms()
+{
+    //do SPI communication stuff
     int i = 0; //temp counter variable
     int v = 0; //temp SPI variable
     if(rpi.receive()) {
         v = rpi.read();   // Read byte from master
-        if(v == char('S')){
+        if(v == char('S')) {
             rpi.reply(0x01);
-            for (i=0;i<=1;i++){                   
+            for (i=0; i<=1; i++) {
                 m_speed_ref[i] = rpi.read() - 128;
                 rpi.reply(m_speed_ref[i]);
             }
@@ -252,15 +232,14 @@
             m_count[1] = 0;
             integral[0] = 0; //reset integrals to avoid odd behaviour
             integral[1] = 0;
-        }
-        else if(v == char('D')){
+        } else if(v == char('D')) {
             rpi.reply(0x02);
-            for (i=0;i<=1;i++){                   
+            for (i=0; i<=1; 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++){                   
+            for (i=0; i<=1; i++) {
                 m_top_speed[i] = rpi.read() - 128;
                 rpi.reply(m_top_speed[i]);
             }
@@ -271,18 +250,16 @@
             m_count[1] = 0;
             m_last_count[0] = 0;
             m_last_count[1] = 0;
-        }
-        else if(v == char('A')){
-            rpi.reply(0x03);                 
+        } else if(v == char('A')) {
+            rpi.reply(0x03);
             max_accel = rpi.read();
             rpi.reply(max_accel);
             v = rpi.read(); //last bit setup a blank reply
             rpi.reply(0x00);
-        }
-        else if(v == char('V')){
-            rpi.reply(0x04);                 
+        } else if(v == char('V')) {
+            rpi.reply(0x04);
             v = rpi.read();
-            if(v <= 6){ //check the ADC to be addressed exists
+            if(v <= 6) { //check the ADC to be addressed exists
                 rpi.reply(adc_values[v]);
             }
             v = rpi.read(); //last bit setup a blank reply
@@ -290,89 +267,83 @@
         }
     }
 }
-void mrfEncoderIsr1(){
-    if(MRF_ENC2 == 0) { 
+void mrfEncoderIsr1()
+{
+    if(MRF_ENC2 == 0) {
         m_count[0] ++;
     } else {
         m_count[0] --;
     }
 }
-void mrfEncoderIsr2(){
+void mrfEncoderIsr2()
+{
     if(MRF_ENC1 == 1) {
         m_count[0] ++;
     } else {
         m_count[0] --;
     }
 }
-void mlfEncoderIsr1(){
+void mlfEncoderIsr1()
+{
     if(MLF_ENC2 == 0) {
         m_count[1] ++;
     } else {
         m_count[1] --;
     }
 }
-void mlfEncoderIsr2(){
+void mlfEncoderIsr2()
+{
     if(MLF_ENC1 == 1) {
         m_count[1] ++;
     } else {
         m_count[1] --;
     }
 }
- 
+
 void controlTick()
 {
     control_loop_flag = true;
 }
 
-        
-float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) {
+
+float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed)
+{
 //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);
+    bool direction;
+
+    if(end<0)
+        direction = false;
+    else
+        direction = true;
+
     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));
-        } 
-    } */
+    halfWay=abs(start)+(abs(end)-abs(start))/2;
+
 
-    if (v<minSpeed && v > 0) {
-       v=minSpeed;
+    if (abs(currentPosition)>halfWay) {
+        //deaccelarate
+        midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-abs(start));
+        v=sqrt(midPointVsqrd-2*accel*(abs(currentPosition)-halfWay));
+    } else {
+        //accelerate
+        //v^2=u^2+2as
+        v=sqrt(minSpeed*minSpeed+2*accel*(abs(currentPosition)-abs(start)));
     }
-    else if (abs(v) < minSpeed && v < 0){
-        v = -1*minSpeed;
-    } 
+
 
-    if (v>topSpeed && v > 0) {
-        v=topSpeed;
+    if (v<minSpeed) {
+        v=minSpeed;
+    } else if (v>topSpeed) {
+        v = topSpeed;
     }
-    else if (abs(v) > topSpeed && v < 0){
-        v = -1*topSpeed;
+
+    if (direction == false) {
+        v = -1*v;
     }
 
     return v;
-    //return topSpeed;
 }