Code Optimization

I've written some code for a class project I'm working on. It functions as expected although it runs much slower than I thought a 16 Mhz clock speed would run it (I'm using the Mega.) A basic explanation of the functions can be found below.

The basic structure can be found in the section titled loop. The purpose of that section is to handle a pin voltage that corresponds to a button that, when pressed, should disable our robot (until it is pressed again.) The goes up for a brief moment and then down, so some logic is needed so that it's not constantly disabling and enabling itself during that time spam. I believe I've handled it efficiently although please tear it apart if it can somehow be made better. The joystick function takes data that's sent from another Xbee (our controller) and translates it into motion. The autonomous1() function isn't included here because I haven't written it yet (it's supposed to guide our robot without any user input for a brief amount of time.) The servo functions operate servos.

Currently, when being operated by the remote, it responds reasonably fast. However, that only came after we gutted some code out to accomplish this. We didn't even remove that much code. For example, 4 simple if statements were previously being used to execute the servo lift/grab functions (instead of directly from the control function.) These 4 simple if statements executed each cycle (they were boolean checks that, if true, would execute the appropriate code.) Their presence introduced a delay of about. 0.25~.5 seconds to every cycle. This just seems outlandish to me given the complexity of the drivers being called in this sketch. They are extremely complex yet execute quicker than my simple series of 4 if/then statements.

Ultimately, I'd like to do a lot with the autonomous function, but at this point I don't even know how much I'll be able to do. The contents will need to be nested in a similar structure to our loop structure to ensure the handling of the disable button is still done in autonomous mode. I have sensors I would like to use and perhaps incorporate into PID functions and the like, but I'm just skeptical of this given how poorly our system responded to the addition of four simple if statements.

Either way, any help would be very well recieved.

/************************************************  INCLUDE LIBRARIES  ***************************************************************/
#include "Servo.h"
#include "DualVNH5019MotorShield.h"

#include "LiquidCrystal.h"
LiquidCrystal lcd(24,26,28,30,32,33);

/*************************************************  OBJECT INTIALIZATION  ***********************************************************/

Servo grabberServo;
Servo liftServo;

DualVNH5019MotorShield md;   //Setup the motor driver structure

/*********** CONSTANT VARIABLES ****************/
    // XBEE COMMUNICATION
    char delimiter              = 127;

    // INDICATORS
    const int red_LED           = 23; 
    const int green_LED         = 22; 

    // SERVOS
    const int grab_servo        = 29;    
    const int lift_servo        = 27;
    
    // SENSORS
    const int easy_button       = 31; 
    const int start_switch      = 25;
    
    const int photo_1           = 13;         // Set Photoresistor pins: ANALOG
    const int photo_2           = 11;
    const int photo_3           = 12;
    const int range_1           = 15;         // Set Rangefinder pins: ANALOG
    const int range_2           = 14;
    
/*************************************************  END OBJECT INTIALIZATION  **********************************************************/

/*************************************************  VARIABLE INTIALIZATION  **********************************************************/

    char tagged               = false;    // Start untagged
    char tag_first            = true; 
    
    char autonomous_mode      = 1;        // Determines which autonomous mode is entered

    boolean grabber_open      = true;     // Begin with the grabber open
    boolean arm_down          = true;     // Begin with the arm down
    
//********** SOFT CONTROL VARIABLES ********** 
    // XBEE READINGS
    char index                = 0; 
    char skip_case            = 0;
    char incoming_byte        = 0;
    int  joystick_x           = 0; 
    int  joystick_y           = 0; 
    char left_button          = 0; 
    char right_button         = 0;
    char top_button           = 0; 
    char bottom_button        = 0;
    char select_button        = 0;

    uint32_t r                = 0;
    float tan_theta           = 0;

    // SENSOR READINGS
    int photo_1_read          = 0;
    int photo_2_read          = 0;
    int photo_3_read          = 0;
    int range_1_read          = 0;
    int range_2_read          = 0;
    
    
//************* HARD CONTROL VARIABLES **************
    // REMOTE CONTROL
    uint8_t r_dead_zone                  = 3200;
    uint32_t servo_enter_function_time   = 0;
    const float tan_theta_turn_zone      = 0.1405;   // Corresponds to 8 degrees
    const float tan_theta_straight_zone  = 7.115;    // Corresponds to 82 degrees
    int turbo_mode                 = false;
    
    // SERVO POSITIONS
    const int grabber_open_angle    = 3; 
    const int grabber_close_angle   = 130; 
    const int arm_lift_angle        = 180; 
    const int arm_lower_angle       = 3;     

    // PID CONTROL
    const float k_p                 = 0;
    const float k_i                 = 0;
    const float k_d                 = 0;

//*********** DEBUGGING VARIABLES **********************
    boolean debugging                    = false;
    unsigned int debugging_time_step     = 500;
    unsigned int debugging_current_time  = 0; 

/*******************************************************  END VARIABLE INITIALIZATION  *************************************/


/*******************************************************  SETUP  ***********************************************************/

void setup()
{ 
  pinMode     (red_LED,   OUTPUT);      // Set red LED as output
  digitalWrite(red_LED,   LOW);         // Red LED   --> OFF
  pinMode     (green_LED, OUTPUT);      // Set green LED as output
  digitalWrite(green_LED, HIGH);        // Green LED --> ON
  
  pinMode     (easy_button, INPUT);     // Set the easy button to be an input
  
  lcd.begin(16, 2);                     // Specify the columns and rows of the lcd
  
  Serial.begin(9600);                   // Set the baud rate to 9600 bps for Serial
  Serial1.begin(9600);                  // Set the baud rate to 9600 bps for Serial1


  // *************** DEBUGGING *************************
  while(debugging == true)
  {
  }
  /***************  END DEBUGGING  ******************/

  //WAIT FOR AUTONOMOUS MODE BEGIN
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Waiting for"); 
      lcd.setCursor(0,1);
      lcd.print("Start Switch");   
      
      while(!Serial1.available())              // While nothing is being sent from the remote
      {
      }
      
  // AUTONOMOUS MODE BEGIN    
      autonomous_mode  = int(Serial1.read());  // Read the autonomous mode from the remote button push
      
      lcd.clear();
      lcd.home();
      lcd.print(int(autonomous_mode));
      
      Serial1.end();
      autonomous_mode_start_time = millis();   // Start the autonomous mode clock
      
      switch(autonomous_mode)
      {
        case 1:
        // 3 SECOND DELAY
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("3....");
        delay(1000);                          // Delay 1 second
    
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("2....");
        delay(1000);                          // Delay 1 second
    
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("1....");
        delay(1000);                          // Delay 1 second
        
        lcd.clear();
        lcd.setCursor(0,0);
        lcd.print("CHEESE IT");
        autonomous_mode_1();
        break;
      }  

  md.setM1Speed(0);
  md.setM2Speed(0);
  
      lcd.clear();
      lcd.home();
      lcd.print("Set motors to 0");
  
  Serial1.begin(9600);
  Serial1.flush();         // Clear the buffer from the incoming serial data 
}

/***********************************************  END SETUP  ****************************************************/
/***********************************************  LOOP  *********************************************************/

void loop()
{
  
  // *************** EASY BUTTON PRESSED **************
  if (digitalRead(easy_button) == HIGH)      // If the easy button is being pressed
  {
    if (tag_first == true)                   // If this is the first time we are going through this loop after the tag button has been pressed
    {
      if (tagged == false)                   // If we were previously untagged
      {
        //****** DO SOMETHING ONCE WHEN TAGGED
        digitalWrite(red_LED, HIGH);         // turn red LED   --> OFF
        digitalWrite(green_LED, LOW);        // turn green LED --> ON
        
        tagged = true;                       // We are now tagged        
        Serial1.print(char(tagged));         // Send to remote: true is Tagged, false is Untagged
       
        md.setM1Speed(0);
        md.setM2Speed(0); 
      }
      
      else  //(tagged == true)               If we were previously tagged
      { 
        //******* DO SOMETHING ONCE WHEN UNTAGGED
        digitalWrite(red_LED, LOW);          // turn red LED   --> ON
        digitalWrite(green_LED, HIGH);       // turn green LED --> OFF
        
        tagged = false;                      // We are now untagged
        Serial1.print(char(tagged));        // Send to remote: true is Tagged, false is Untagged
      }
      
      tag_first = false;
    }
    
    if (tag_first == false)      
    {
      if (tagged == true)        // If we are tagged, do stuff constantly
      {
        //********** DO STUFF CONSTANTLY WHEN TAGGED
        Serial1.print(char(tagged));        // Send to remote: true is Tagged, false is Untagged  
      }
      
      else //(tagged == false)     If we are not tagged, do things constantly
      {
        //*********** DO STUFF CONSTANTLY WHEN NOT TAGGED
        
        read_joystick();
      }
    }
  }
  
  // *************** EASY BUTTON NOT PRESSED **************
  if (digitalRead(easy_button) == LOW)    // If the easy button is not being pressed
  {
    
    tag_first = true;
    
    if (tagged == true)          //If we are tagged, do things constantly
    {
      //DO STUFF CONSTANTLY WHEN TAGGED
    }
    
    else //(tagged == false)       If we are not tagged, do things constantly
    {
      lcd.clear();
      lcd.home();
      lcd.print(joystick_x);
      read_joystick();
    
    }    // end else (tagged = false)
  }      // end if (if easy_button == LOW)
}        // end void loop()
  
/****************************************************  END LOOP  *******************************************************/
/*************** GRAB FUNCTION ********************/
void grab_function()           // FUNCTION to Open/Close Grabber Arm 
{
  Serial1.end();
  
  servo_enter_function_time       = millis();
  grabberServo.attach(grab_servo);
  
  if( grabber_open == true)
  {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Grabber Open");
    grabber_open = false;
    grabberServo.write(grabber_close_angle);
  }
 
  else //( grabber_open == false)
  {
    lcd.clear();
    lcd.setCursor(0,0);
    Serial.print("Grabber Closed");
    grabber_open = true;
    grabberServo.write(grabber_open_angle);
  }
  
  while(millis() < servo_enter_function_time + 1250)   // Wait for 1250 ms
  {
  }
  
  grabberServo.detach();
  Serial1.begin(9600);

/**************** END GRAB FUNCTION *****************/


/*************** ARM FUNCTION ********************/
void arm_function()            //Function that closes the grabber arm 
{
  Serial1.end();      // Stop communicating with the XBee
  
  servo_enter_function_time       = millis();
  liftServo.attach(lift_servo);
  
  if( arm_down == true)               // IF the arm is down
  {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Arm Down");
    arm_down   = false;              // Now the arm is up
    liftServo.write(arm_lift_angle);  // Raise the arm
  }
  
  else //(arm_down == false)           // IF the arm is not down
  {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Arm Up");
    arm_down   = true;                // Now the arm is down
    liftServo.write(arm_lower_angle);  // Lower the arm
  }
  
  while(millis() < servo_enter_function_time + 1250)    // Wait for 1250 ms
  {
  }
  
  liftServo.detach();
  Serial1.begin(9600);    // Restart communicating with the XBee
}
/**************** END ARM FUNCTION *****************/[code]

[/code]

/***************  Joystick Read Function **********/
void read_joystick()
{
  while(1)              // Stay reading the joystick until it has fully been read 1 time
  {
    incoming_byte = Serial1.read();      // Read the byte and set it equal to incoming_byte
    
    if (incoming_byte == delimiter)       // If incoming byte is the delimiter
    {
      if (index == 3)                     // If we have also not read junk code
      {
        md.setM1Speed((joystick_x + joystick_y)/(2 - turbo_mode));      // Linear algebra equation to drive motor
        md.setM2Speed((-joystick_x + joystick_y)/(2 - turbo_mode));
       
        Serial1.flush();                   // Clear the buffer from the incoming serial data
        index           =0;
        break; 
      }   //end if(index ==3)
        
      index           =0;
      skip_case       =1;         
      
    }     //end if(incoming_byte==delimiter)
    
    switch(skip_case)
    {
      case 0:
 
        switch(index)
        {
          case 0:              //If index == 0 
              joystick_x= (((int(incoming_byte)-63)*470)/75);    // mapped joystick x from -400 to 400 (for Motor Voltages)
              index++;
              break;
        
          case 1:              //If index == 1
              joystick_y = (((int(incoming_byte)-63)*470)/75);   // mapped joystick y from -400 to 400 (for Motor Voltages)
              index++;
              break;
        
          case 2:              //If index == 2
          
            switch(int(incoming_byte))
            {
              case 32:        //No buttons have been pressed
                turbo_mode = false;
                break;
  
              case 1:
                //Serial.println("Left Button Press");
                break;
  
              case 2:
                //Serial.println("Top Button Press");
                arm_function();
                break;
  
              case 3:
                //Serial.println("Right Button Press");
                grab_function();
                break;
  
              case 4:
                //Serial.println("Bottom Button Press");
                turbo_mode = true;
                break;
                
              case 5:
                //Serial.println("Select Button Press");              
                break;
            }
            index++;
            break;         
        }

     case 1:
     skip_case    = 0;
      }  //end skip_case
    }    //end while(1)   
}        //end joystick function

I just had a quick glance at your code. Floats are usually a reason for slowing down code a lot. I would suggest to transform your formulas to integer computations.

However your code also contains lots of LCD IO which is usually also very slow.

All of this may or may not contribute to your issue.

The first step to actual perfomance improvements is to measure why it is so slow in the first place. Read Section 1.1 and 1.2 of this General Strategies

Once you have read these two sections you might want to google for "performance optimization c" which brings lots of reasonable ideas, e.g. this http://www.eventhelix.com/RealtimeMantra/Basics/OptimizingCAndCPPCode.htm.

But first read the paragraphs on bad and good optimization strategies.

These 4 simple if statements executed each cycle (they were boolean checks that, if true, would execute the appropriate code.) Their presence introduced a delay of about. 0.25~.5 seconds to every cycle.

To save me guessing, can you say which 4 if statements?

This could waste a lot of time:

  while(1)              // Stay reading the joystick until it has fully been read 1 time
  {
    incoming_byte = Serial1.read();      // Read the byte and set it equal to incoming_byte
    
    if (incoming_byte == delimiter)       // If incoming byte is the delimiter

And this:

  while(millis() < servo_enter_function_time + 1250)   // Wait for 1250 ms
  {
  }

The loop section of the code was modified just after the read_joystick() function was called, so that it looked like this.

      read_joystick();
      
      if(left_button == 1)
      {
        left_button = 0;
        grabber_close_function();
      }
      
      if(top_button == 1)
      {
        top_button = 0;
        arm_lift_function();
      }
      
      if(right_button == 1)
      {
        right_button = 0;
        grabber_open_function();
      }
      
      if(bottom_button == 1)
      {
        bottom_button = 0;
        arm_lower_function();
      }

This required some changes to the case structure in the joystick read function.

            switch(int(incoming_byte))
            {
              case 32:
                left_button    = 0;
                top_button     = 0;
                right_button   = 0;
                bottom_button  = 0;
                select_button  = 0;
                break;
  
              case 1:
                left_button    = 1;
                top_button     = 0;
                right_button   = 0;
                bottom_button  = 0;
                select_button  = 0;
                break;
  
              case 2:
                left_button    = 0;
                top_button     = 1;
                right_button   = 0;
                bottom_button  = 0;
                select_button  = 0;
                break;
  
              case 3:
                left_button    = 0;
                top_button     = 0;
                right_button   = 1;
                bottom_button  = 0;
                select_button  = 0;
                break;
  
              case 4:
                left_button    = 0;
                top_button     = 0;
                right_button   = 0;
                bottom_button  = 1;
                select_button  = 0;
                break;
                
              case 5:
                left_button    = 0;
                top_button     = 0;
                right_button   = 0;
                bottom_button  = 0;
                select_button  = 1;                
                break;
            }

So calling functions have been replaced with variable value reassignment. Either way, these small changes produced a very discernable increase in cycle time. I'm baffled by it, figured a 16 Mhz clock would do these calculations before I could even ponder them.

In regards to the while(), that actually speeds it up. It keeps it stuck in the joystick function until a complete joystick reading has been performed. It requires around 4~7 iterations of that loop, depending on where it starts reading from in the data stream. Previously, it was escaping after every reading and checking our disable button logic. This resulted in a very large delay, so that's why we changed it.

The while(millis() < servo_enter_function_time + 1250) statement is deliberate. We have to do this because the Xbee fills the buffer up incredibly fast, which seems to interfere with the servo libraries. Thus we turn off the Xbee serial, engage our servo (which takes about 1250 ms to fully actuate), disengage it (as to not draw power), and turn the Xbee serial back on and then resume cycling through our loop. The delay it causes only occurs when call on this function, and the resulting delay is expected.

In regards to the floats, they are not actually used in our loop. We removed all code using them earlier for various reasons. We left the preallocation of those control variables in though, but because they are not involved in the looping section of the code or anything else, we assumed that a few preallocated variables wouldn't cause much harm.

Either way, I'll take a look through those resources you've provided. Thanks for the help thus far.

while(millis() < servo_enter_function_time + 1250)

should be

while(millis() -1250 < servo_enter_function_time)

because the latter is not affected by the overflow of millis() - after 49++ days .. :slight_smile:
See - Arduino Playground - TimingRollover -

robtillaart:
while(millis() < servo_enter_function_time + 1250)

should be

while(millis() -1250 < servo_enter_function_time)

because the latter is not affected by the overflow of millis() - after 49++ days .. :slight_smile:
See - Arduino Playground - TimingRollover -

That won't work either. You're just delaying the problem by 1250 milliseconds.

Well to save me patching together lots of code from various posts, how about simply attaching the current sketch to a forum message using the "attach file" option? Click "Additional Options" under the posting box.