Programming a robot - just looking for advice/ideas

I have an RC car with an Arduino Mega. I have three ultra-sonic sensors connected. Everything is talking to each other.

Using motor and NewPing libraries

Im really just looking to see if there are more efficient ways to writing this sketch. It is a working progress, I tried to explain what all is going on.

Any advice an or ideas would be greatly appreciated.

This is the whole thing:

// ---------------------------------------------------------------------------
// This example code was used to successfully communicate with 15 ultrasonic sensors. You can adjust
// the number of sensors in your project by changing SONAR_NUM and the number of NewPing objects in the
// "sonar" array. You also need to change the pins for each sensor for the NewPing objects. Each sensor
// is pinged at 33ms intervals. So, one cycle of all sensors takes 495ms (33 * 15 = 495ms). The results
// are sent to the "oneSensorCycle" function which currently just displays the distance data. Your project
// would normally process the sensor results in this function (for example, decide if a robot needs to
// turn and call the turn function). Keep in mind this example is event-driven. Your complete sketch needs
// to be written so there's no "delay" commands and the loop() cycles at faster than a 33ms rate. If other
// processes take longer than 33ms, you'll need to increase PING_INTERVAL so it doesn't get behind.
// ---------------------------------------------------------------------------
#include <NewPing.h>
#include <AFMotor.h>

AF_DCMotor motor(2, MOTOR12_2KHZ);    // create motor #2, max 64KHz 
AF_DCMotor steering(1, MOTOR12_2KHZ); // create steering motor #1, max 64KHz

#define SONAR_NUM     3 // Number or sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 34 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).


byte botSpeed = 80;                 // Sets initial speed to 80
byte botDir = 150;                  // Sets speed for steering motor
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  // Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(53, 52, MAX_DISTANCE), // LEFT Sensor
  NewPing(51, 50, MAX_DISTANCE), // FRONT Sensor
  NewPing(49, 48, MAX_DISTANCE)  // RIGHT Sensor
};

void setup() {
  Serial.begin(115200);
  motor.setSpeed(botSpeed);   // set the speed to 80/255
  steering.setSpeed(botDir);  // set stering motor speed to 200
  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}

void loop() {
  
  motor.setSpeed(botSpeed);                 //Think this will make sure the speed is better controlled (just felt like it was needed)
  
  for (uint8_t i = 0; i < SONAR_NUM; i++)   // Loop through all the sensors.
  { 
    if (millis() >= pingTimer[i])           // Is it this sensor's time to ping?
    {         
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
      currentSensor = i;                          // Sensor being accessed.
      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
    }
  }
  // The rest of your code would go here.
}

void echoCheck() { // If ping received, set the sensor distance to array.
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}


void goForward()
{
  motor.run(BACKWARD);  
}

void goBackwards()
{
  motor.run(FORWARD);  
}

void Coast()
{
 motor.run(RELEASE); 
}

void goLeft()
{
  steering.run(BACKWARD);  
}

void goRight()
{
  steering.run(FORWARD);  
}

void speedUp()
{
  float Sinc = 1.1;
  
  botSpeed = min(botSpeed, 130); //sets maximum speed to 130
  botSpeed += Sinc;              //adds 1.1 to current speed
  motor.setSpeed(botSpeed);      //Sets new speed
 // Serial.println(botSpeed);      //used to check it is working properly(serves no other purpose) will comment out asap
}

void speedDown()
{  
  float Sdec = .96;
  
  botSpeed = max(botSpeed, 84);  //Sets minimum speed to 79
  botSpeed *= Sdec;              //Multiplies .95 by current speed
  motor.setSpeed(botSpeed);      //Sets new speed
  //Serial.println(botSpeed);      //used to check it is working properly(serves no other purpose) will comment out asap
}

void speedDown2()
{
  float Sdec2 = .90;
  
  botSpeed = max(botSpeed, 84);  //Sets minimum speed to 79
  botSpeed *= Sdec2;             //Multiplies .90 by current speed
  motor.setSpeed(botSpeed);      //Sets new speed
}

void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    Serial.print(i);
    Serial.print("=");
    Serial.print(cm[i]);
    Serial.print("cm ");
    
   
   
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
//////////  Is there a better way to write this?  |
 //                                               |
  //                                              V ?  


 
  if (cm[1] > 0)
{
      if(cm[1] < 50)        //If distance is between 1-49 go backwards
    {
       goBackwards();       //backwards function
    }
    else 
     { 
           if(cm[1] < 80)        //if distance is between 50-79 Slow down faster!!!
            {
              speedDown2();
              goForward();
            }    
    
        else    
        {
            if(cm[1] < 200)      //If distance is between 80-199 slow down
            {
               speedDown();      //Slow down function
               goForward();      //Forward function
            }
        }  
     }
}
else                             //If distance is 0, there is nothing in the way
{
  goForward();   
  speedUp();
}
  
  
   
   
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
// Havent really gotten to the turning part yet  ///////////

////////// [0] = left sensor
////////// [2] = right sensor

     
  if (cm[0] > cm[2] )
    {
      //Serial.print("go Left");
      goLeft();
    }  
    
  if (cm[2] > cm[0] )
    {
      //Serial.print("go Right");
      goRight();   
    }  
    
    
    
    
    
    
    
    
   }
  Serial.println();
}
  motor.setSpeed(botSpeed);                 //Think this will make sure the speed is better controlled (just felt like it was needed)

Well, it wasn't.

  botSpeed = max(botSpeed, 84);  //Sets minimum speed to 79

No, it doesn't.

void speedDown()
{  
  float Sdec = .96;
  
  botSpeed = max(botSpeed, 84);  //Sets minimum speed to 79
  botSpeed *= Sdec;              //Multiplies .95 by current speed
  motor.setSpeed(botSpeed);      //Sets new speed
  //Serial.println(botSpeed);      //used to check it is working properly(serves no other purpose) will comment out asap
}

void speedDown2()
{
  float Sdec2 = .90;
  
  botSpeed = max(botSpeed, 84);  //Sets minimum speed to 79
  botSpeed *= Sdec2;             //Multiplies .90 by current speed
  motor.setSpeed(botSpeed);      //Sets new speed
}

The amount by which to adjust the speed should be an input to the function, rather than writing two nearly identical functions with hardcoded values.

//////////  Is there a better way to write this?  |

Yes. It does not make sense to loop a number of times equal to the number of values in the array, and then reference only the middle value in the array.

Putting each { on a new line and using Tools + Auto Format would be a big help.

The code
jerks all over the
page and is
very hard to follow.
Is it even possible for an element in the
cm array to be less than
zero?

Thanks for the input, I got you on the previous statements you made. fixed them right away.

“The amount by which to adjust the speed should be an input to the function, rather than writing two nearly identical functions with hardcoded values.”

When you say this do you mean, write a function that controls the acceleration of slowing down based off its current speed? I thought about doing that, it would really make since to do it that way.
I dont know the best way to decrement the speed.

I want to apply the brakes(slow motor speed) like I would in a car. The faster you go the harder you apply the brakes, because I only have 200 cm (6 ft) to work with.

 if (cm[1] > 0)
{
  if(cm[1] < 50)        //If distance is between 1-49 go backwards
  {
    goBackwards();       //backwards function
  }
  else 
  { 
    if(cm[1] < 80)        //if distance is between 50-79 
    {
      speedDown2();
      goForward();
    }    

    else    
    {
      if(cm[1] < 200)      //If distance is between 80-199 slow down
      {
        speedDown();      //Slow down function
        goForward();      //Forward function
      }
    }  
  }
}
else                             //If distance is 0, there is nothing in the way
{
  goForward();   
  speedUp();
}

cm[1] is just the front sensor. Depending on the distance it reads it will only go forward, backwards, speedup, and/or slow down.

Didn’t get your less than zero thing. Though if the sensor reads 0 there is nothing in the way. It only checks from 1 - 200 cm. After 200 cm it doesn’t care and makes the distance 0

cm[0] & cm[2] is the left and right sensor. they are in charge of the turning. Haven’t really written anything for that part yet.

You could have a slowDown() function that takes an argument - either the speed delta or the percentage delta, instead of two functions that have hardcoded percentages.

void slowDown(float pct)
{
   // Scale current speed by pct
}

Then, replace speeddown(); with slowDown(0.96); and replace speeddown2(); with slowDown(0.90);.

Thanks PaulS, that worked quite well. I’m working on the turning part of the robot now.

cm[0] = left sensor
cm[2] = right sensor

0 is greater than 200 cm

This is what I have so far:

if (cm[0] == 0 && cm[2] == 0){goStraight();} //If both sensors detect nothing - go straight
if (cm[0] == 0 && cm[2] > 0){goLeft();}      //If right sensor detects something and left doesnt - go left 
if (cm[2] == 0 && cm[0] > 0){goRight();}     //If left sensor detects something and right doesnt - go right 



if(cm[0] > 0 && cm[2] > 0)                   //If left sensor and right sensor both detect something  
{
  if(cm[0] > 2*cm[2]){goLeft();}             //If lefts distance is greater than rights * 2 - go left  
    else goStraight();                          //Go straight if statement is false
  
  if(cm[2] > 2*cm[0]){goRight();}            //If rights distance is greater than lefts * 2 - go Right
    else goStraight();
  
}

This was something I came up with pretty quick, haven’t been able to test it yet. Im trying to get it to change direction only if it gets to close to the wall. Not sure if *2 is the best way of going about it, or if I even wrote that statement correctly. ( It will compile )