Timing issues in my code

Using Arduino Programming Questions
1

I need a bit of help. I have this simple little autonomous robot that seems to be working pretty well except I am having some timing issues in the code. I am using the timedaction library to have an 'interrupt' for the ultrasonic sensor sweep with a servo motor that seems to not have enough time to function properly. The code is really short so I am just pasting it all on here. I am doing this because sometimes fresh eyes can look at something and pinpoint my error. Basically I have a servo that is sweeping from 45 to 90 to 135 degrees and every time it stops, I ping the ultrasonic sensor and store the value in an array. The motors move with respect to the longest distance available. If anyone sees anything that would cause this to have issues please let me know. Everything seems to be working, but the servo is not sweeping all the way to the right and the robot is slow to respond. It will sometimes work great but sometimes hit the wall before turning. I figure that it works sometimes because the signal is received faster allowing enough time within the interrupt.

Thanks for the help in advance.

#include <TimedAction.h>
#include <Servo.h>
 
 int enable = 53;
 int dir1 = 49;
 int pwm1 = 2;
 int dir2 = 51;
 int pwm2 = 3;
 int servo = 4;
 int i = 0;
 int z = 0;
 
 Servo myservo;
 
 const int pingPin = 5; 
 
 long Ranging[3];
 //int x = 0;
 
 TimedAction repeatScan = TimedAction(1000, Scan);
 
void setup()
{
 Serial.begin(9600); 
 pinMode(enable, OUTPUT); 
 pinMode(dir1, OUTPUT);
 pinMode(pwm1, OUTPUT); 
 pinMode(dir2, OUTPUT); 
 pinMode(pwm2, OUTPUT); 
 pinMode(servo, OUTPUT); 
 
 myservo.attach(servo);  // attaches the servo on pin 9 to the servo object 
}

void loop()
{
 repeatScan.check();
}

long duration;
long cm;

long ping(){ // ping ultrasonic sensor
  
  
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  //delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  //delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  delay(300);
 
  return cm;
  
  
  // r m l
   
}
void Scan(){ // scan function for servo motor
int pos;
    
  myservo.write(45);
  Ranging[0] = ping();
  
  
  myservo.write(90);
  //delay(10);
  Ranging[1] = ping(); 

  myservo.write(135);
  //delay(10);
  Ranging[2] = ping();
   
  if((Ranging[2] >= 70)){
    digitalWrite(enable, HIGH);
    digitalWrite(dir1, LOW);
    digitalWrite(dir2, LOW); 
    forward();
    delay(25);
  }
  else if (Ranging[0] >= 70) {
 digitalWrite(dir1, HIGH);
 digitalWrite(dir2, LOW); 
 forward();
 delay(25);
  }
  else if (Ranging[1] >= 70) {
 digitalWrite(dir1, LOW);
 digitalWrite(dir2, HIGH); 
 forward();
 delay(25);
  }
  else{
    mstop();
  }
  
  
  
  }

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}

void forward(){
 
 while(i <= 30){
 analogWrite(pwm1, i); // PWM at 20% is 51
 analogWrite(pwm2, i); // PWM at 20% is 51
 i ++;
 delay(25);
}
 if(i == 30){
   
 analogWrite(pwm1, 51); // PWM at 20% is 51
 analogWrite(pwm2, 51); // PWM at 20% is 51
   
 }
 

}
void mstop(){
 i=51;
 bool stop = false;
 if(stop == false){
 while(i >= 51){
 analogWrite(pwm1, i); // PWM at 20% is 51
 analogWrite(pwm2, i); // PWM at 20% is 51
 i = i - 1;
 delay(10);
 digitalWrite(enable, LOW);
 stop = true;
 }
}
if(stop == true){
i = 0;
}
}
2 
void forward(){
 
 while(i <= 30){

Initial value of i?

3

i is 0 initially. That section of code was actually a pain for me to come up with because I needed to slowly increment the motors instead of just starting them at the duty cycle immediately. The motor functions seem to work perfectly but they could also cause the timing issue.

4

Not answering your question but some of this, and other similar constructs look very strange

void mstop()
{
  i=51;
  bool stop = false;
  if(stop == false)
  {
    while(i >= 51)
    {
      analogWrite(pwm1, i); // PWM at 20% is 51
      analogWrite(pwm2, i); // PWM at 20% is 51
      i = i - 1;
      delay(10);
      digitalWrite(enable, LOW);
      stop = true;
    }
  }
  if(stop == true)
  {
    i = 0;
  }
}

You set stop = false, then test whether it is false. Why ?
Later you set stop = true then test whether it is true. Why ?
The while loop may just as well be a for loop because you know the upper and lower bounds of i
Do you need to set the enable pin LOW on each pass through the while loop ?
Equally, do you need to set stop = true on each pass as well ?
Does that while loop actually do anything ?
i starts at 51 and is decremented. The while test condition is >= 51
How long will i remain >= 51 ?

Whatever is causing your problem the code would benefit from tidying up to remove unnecessary tests and setting of variables and to use a more natural way of looping through a range of values of a variable. Once it is shorter and more logical the actual cause of the problem may become apparent.

5

I guess the forward() function is designed to accelerate from a standstill up to full speed, but it also looks to me as if Scan() will be called every second and will call forward() repeatedly - not just when moving away from stationary. Is that what you intended? I would guess this causes the 'bot to hesitate once per second while it is moving.

You're writing new positions to the servo and then executing the ping immediately. There's no way the servo will be in position ready for the ping. The fact the servo will be pointing in a different direction to the one you intended for the ping might explain why it seems to be missing obstacles.

I think you would have more success if you managed the timing between the servo movements and the pings explicitly. Judging by forward() I guess you intend to include some sort of acceleration/deceleration logic, and if that's the case I suggest you do this timing asynchronously rather than using delays. For example, you could address the main problem by some relatively simple changes to scan. Arrange for it to be called (say) three times per second. Each time it is called, do a ping() in the direction the servo is currently pointing and command the servo to move to the position for the next ping. Remember the most recent result along each direction and re-evaluate your distance comparisons each time.

I also suggest you decouple the motor controls from this steering logic if you want to support accel/decel. Have the ping() and related logic produce a demanded speed and direction, and then have a little state machine to progressively slow down/speed up each motor as necessary to achieve that.