0

I'm trying to use ultrasonic sensor, which are located on the back and front side of my rover, in order to straighten the rover. The rover seems to be doing 360 degree turns instead of correcting its paths then stopping. Would you help me to fix my code?

lt() and rt() are the turning left and right functions respectively.


#include <NewPing.h>

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

int motorLa = 10;
int motorLb = 9;
int dirLa = 24;
int dirLb = 25;
int motorRa = 6;
int motorRb = 5;
int dirRa = 22;
int dirRb = 23;
int lSpeed = 150;
int rSpeed = 180;

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(37, 36, MAX_DISTANCE), //Left Front
  NewPing(33, 32, MAX_DISTANCE), //Left Rear
  NewPing(35, 34, MAX_DISTANCE), //Right Front
  NewPing(39, 38, MAX_DISTANCE), //Right Rear
  NewPing(31, 30, MAX_DISTANCE), //Back
};

void setup() {
  Serial.begin(9600);
  pinMode (motorLa, OUTPUT);
  pinMode (motorLb, OUTPUT);
  pinMode (dirLa, OUTPUT);
  pinMode (dirLb, OUTPUT);
  pinMode (motorRa, OUTPUT);
  pinMode (motorRb, OUTPUT);
  pinMode (dirRa, OUTPUT);
  pinMode (dirRb, OUTPUT);
  delay(1000);
  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() {
  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.
  if (abs(cm[0] - cm[1]) < 4)
 {
  fwd();
 }
  else if (cm[0] < cm[1]) 
  {
    
    lt();
     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).
    }
  }
  }
  else if (cm[0] > cm[1]) 
  {
    rt();
    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).
  }
}

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 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 ");
  }
  Serial.println();
}

void fwd()
{
    digitalWrite(dirLa, LOW); // Left a Forward 
    digitalWrite(dirLb, HIGH); // Left b Forward 
    digitalWrite(dirRa, LOW); //Right a Forward
    digitalWrite(dirRb, HIGH);
    analogWrite(motorLa, lSpeed); // Sets the motor speed at a value of 180
    analogWrite(motorLb, lSpeed);
    analogWrite(motorRa, rSpeed);
    analogWrite(motorRb, rSpeed);
}
//void rev()
//{
//    digitalWrite(dirLb, LOW); // Left a Forward 
//    digitalWrite(dirLa, HIGH); // Left b Forward 
//    digitalWrite(dirRb, LOW); //Right a Forward
//    digitalWrite(dirRa, HIGH);
//    analogWrite(motorLa, lSpeed); // Sets the motor speed at a value of 180
//    analogWrite(motorLb, lSpeed);
//    analogWrite(motorRa, rSpeed);
//    analogWrite(motorRb, rSpeed);
//}
void lt()
{
    analogWrite(motorLa, 0); // Sets the motor speed at a value of 180
    analogWrite(motorLb, 0);
    analogWrite(motorRa, 0);
    analogWrite(motorRb, 0);
    digitalWrite(dirLb, LOW); // Left a Forward 
    digitalWrite(dirLa, HIGH); // Left b Forward 
    digitalWrite(dirRb, HIGH); //Right a Forward
    digitalWrite(dirRa, LOW);
    analogWrite(motorLa, 50); // Sets the motor speed at a value of 180
    analogWrite(motorLb, 50);
    analogWrite(motorRa, 50);
    analogWrite(motorRb, 50);
}
void rt()
{
    analogWrite(motorLa, 0); // Sets the motor speed at a value of 180
    analogWrite(motorLb, 0);
    analogWrite(motorRa, 0);
    analogWrite(motorRb, 0);
    digitalWrite(dirRb, LOW); // Left a Forward 
    digitalWrite(dirRa, HIGH); // Left b Forward 
    digitalWrite(dirLb, HIGH); //Right a Forward
    digitalWrite(dirLa, LOW);
    analogWrite(motorLa, 50); // Sets the motor speed at a value of 180
    analogWrite(motorLb, 50);
    analogWrite(motorRa, 50);
    analogWrite(motorRb, 50);
}

closed as too broad by Chris Stratton, SDsolar, Michel Keijzers, Greenonline, gre_gor Jun 16 '17 at 12:45

Please edit the question to limit it to a specific problem with enough detail to identify an adequate answer. Avoid asking multiple distinct questions at once. See the How to Ask page for help clarifying this question. If this question can be reworded to fit the rules in the help center, please edit the question.

  • 2
    I would say the first thing you should do is write a sketch which dumps out the reading of the various sensors in column form on the serial output. Then, with the motors unpowered, manually position the robot in different positions and look at the values. Are they useful? UItrasonic may not work well at detecting walls which are too close, or at a substantial angle to the beam. If the numbers seem to be useful, start writing some "rules" in plain language for how to respond to various conditions reflected in the readings. Then translate those to code. – Chris Stratton Jun 11 '15 at 16:20
  • Your code has a LOT of HTML tags throughout it... which makes it nigh impossible to edit (should the need arise). Did you copy it directly from a web site, i.e. here, here, or here? If so, could you please include the link from whence it was sourced? – Greenonline Feb 21 '16 at 8:19
  • 1
    OK, so the code problem formatting is good here on Arduino. I agree with @Chris about using the serial output so you can see intermediate results as the program works. (I have a long USB-to-Nano cable for just such things) Do keep in mind that this site is not a great place to come with broad "what should I do" or "what I am doing wrong" questions. Once you have localized the problem then this is a great place to ask "How should I fix it?" – SDsolar Jun 16 '17 at 8:11
1

5 sensors, located at the front and back side - that really does not make sense. Take a pic, or draw it out, which sensor is which?

How are you powering them? Are they steppers or linear DC motors? Is there 2 or 4 (is your variable 'a' front or back)?

left

digitalWrite(dirLb, LOW); // Left a Forward 
digitalWrite(dirLa, HIGH); // Left b Forward 
digitalWrite(dirRb, HIGH); //Right a Forward
digitalWrite(dirRa, LOW);

right

digitalWrite(dirRb, LOW); // Left a Forward 
digitalWrite(dirRa, HIGH); // Left b Forward 
digitalWrite(dirLb, HIGH); //Right a Forward
digitalWrite(dirLa, LOW);

Do us a favor and clean up the incorrect comments and use better variable names. Properly indent functions with more than 1 space (4 is suggested).

In general you are assuming that you are getting accurate sensors reading every time. That will not be the case. You need to handle missed echos (they show up as 0), incorrect and correct MAX Distance (also shows up as 0), and in general smooth out the readings.

It is then customary to apply a PID controller for line or wall following. A PID controller 'steers' your robot in a intelligent way. There are hundreds of line following tutorials on the internet to help with that concept.

As is, your code if too difficult to follow. You copy pasted from a poor single sensor example and made it into multi sensor pile of spaghetti. You are only using sensors 0 & 1, drop all the rest out!

Just curious, why do you drive a & b separately? Why not just leftSide, rightSide? Hook the motors on each side up to the same speed controller channel, or do it in code, but do not just pile it all in every function, it is easy to make mistakes, and your comments are incorrect since you never bothered to change them. It appears a & b are reversed, the same is true on one of my bots. Wiring them reversed made the code a lot easier to follow.

Not the answer you're looking for? Browse other questions tagged or ask your own question.