Method calling problem

Hello,

Our intention is to make a kind of a grid/maze solver with pre programmed map and obstacles.
We could make it work with our first algorithm to calculate the coordinates where we need to pass.
Here is a video of how it works on a crosspoint. - YouTube
Butt we found a better algorithm (so that we can dodge unprogrammed obstacles with an ultrasonic sensor).
We implemented that but there are a few problems with turing left/right/around.
In the method floyd, we look what the currentCoordinate en currentDirection are, and what they should be to go to the next crosspoint. There whe call the method draaien(); it means turning (Duth) But when it shoud just turn over 90° or turn aroud, it turns but it never stops. So the robot turns and turns and …
In the old program (first algoritm) we did it exactly the same way, the only difference was to calculate the nextcoordinate. If i call the method draaien(); directly in the loop, it works perfect, but when i call it in the method floyd, it never ends turning.
What could be the problem or what’s a possible solution?

//Kjell Deboysere 09/10/2014
//Dieter Jonckheere 14/10/2014

#include <avr/pgmspace.h>


const byte arrayCoordinates[25][25] PROGMEM={
... to long to post here};



const byte directions[25][25] PROGMEM={
    ... to long to post here};

... declaring pins and variables...to long to post here

void setup(){
startCoordinate=currentCoordinate=1;
  startDirection=currentDirection=200;
  goToCoordinate=3;
  pinMode(linkermotorDirection,OUTPUT); 
  pinMode(rechtermotorDirection,OUTPUT); 
  pinMode(linkerIR,INPUT); 
  pinMode(rechterIR,INPUT); 
  pinMode(middenIR,INPUT);  
  pinMode(beeper,OUTPUT);

  digitalWrite(beeper, HIGH);
  delay(15);
  digitalWrite(beeper, LOW);
  delay(200);
  digitalWrite(beeper, HIGH);
  delay(15);
  digitalWrite(beeper, LOW);
  delay(200);
  digitalWrite(beeper, HIGH);
  delay(15);
  digitalWrite(beeper, LOW);
  delay(200);
  digitalWrite(beeper, HIGH);
  delay(15);
  digitalWrite(beeper, LOW);
  delay(200);
  Serial.begin(9600);
  
  Serial.println(digitalRead(rechterIR));
  delay(1000);
  Serial.println(digitalRead(linkerIR));
  delay(1000);
  Serial.println(digitalRead(middenIR));
  delay(3000);

}

void loop(){
  routeMaken();
  floyd();
}

void corrigeren(){  // correct direction (line following)
  while(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==LOW){
    digitalWrite(rechtermotorDirection,HIGH);
    digitalWrite(linkermotorDirection,LOW);                       
    analogWrite(linkermotorSpeed,150);
    analogWrite(rechtermotorSpeed,150);
  }
  while(digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==HIGH){ 
    digitalWrite(rechtermotorDirection,LOW);                         
    digitalWrite(linkermotorDirection,HIGH);
    analogWrite(linkermotorSpeed,150);
    analogWrite(rechtermotorSpeed,150);
  }
}
/////////
void vooruit(){  // go FORWARD to the next crosspoint

if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){
      analogWrite(rechtermotorSpeed,0);
      analogWrite(linkermotorSpeed,0); 
      while( digitalRead(middenIR)==HIGH ){
        digitalWrite(rechtermotorDirection,HIGH);
        digitalWrite(linkermotorDirection,HIGH);
        analogWrite(rechtermotorSpeed,200);
        analogWrite(linkermotorSpeed,200); 
        corrigeren();
      }}
    if(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==HIGH){
    while(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==HIGH ){
      digitalWrite(rechtermotorDirection,HIGH);
      digitalWrite(linkermotorDirection,HIGH);
      analogWrite(rechtermotorSpeed,200);
      analogWrite(linkermotorSpeed,200);
    }
  }
  corrigeren();
  if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){
    analogWrite(rechtermotorSpeed,0);
    analogWrite(linkermotorSpeed,0); 
currentCoordinate=nextCoordinate;
  }
}

///////
void draaien(){  // TURNING
  if (draaizin=="R"){   //RIGHT
   if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){  
    while( digitalRead(middenIR)==HIGH ){
        digitalWrite(rechtermotorDirection,HIGH);
        digitalWrite(linkermotorDirection,HIGH);
        analogWrite(rechtermotorSpeed,150);
        analogWrite(linkermotorSpeed,150); 
        corrigeren();
      }}
      while(digitalRead(rechterIR)==HIGH ){  
        digitalWrite(6,HIGH);
        analogWrite(rechtermotorSpeed,150);
        digitalWrite(8,LOW);
        analogWrite(linkermotorSpeed,150);
      }
        analogWrite(rechtermotorSpeed,0);
        analogWrite(linkermotorSpeed,0); 
      currentDirection-=50;
      if(currentDirection==50){
      currentDirection=250;
      }
      draaizin="";
      }
 //--------------
  if (draaizin=="L"){   // LEFT
    if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){
      while( digitalRead(middenIR)==HIGH ){
        digitalWrite(rechtermotorDirection,HIGH);
        digitalWrite(linkermotorDirection,HIGH);
        analogWrite(rechtermotorSpeed,150);
        analogWrite(linkermotorSpeed,150); 
        corrigeren();
      }}
      while(digitalRead(linkerIR)==HIGH ){  
        digitalWrite(6,LOW);
        analogWrite(rechtermotorSpeed,150);
        digitalWrite(8,HIGH);
        analogWrite(linkermotorSpeed,150);
      }
        analogWrite(rechtermotorSpeed,0);
        analogWrite(linkermotorSpeed,0); 

      
      currentDirection+=50;
      if(currentDirection==50){
      currentDirection=250;
      }
      
      if(currentDirection==300){
      currentDirection=100;
      }
      draaizin="";
    }
    
  //--------------------
 if (draaizin=="OM"){   // TURN AROUND
   if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){  
    while( digitalRead(middenIR)==HIGH ){
        digitalWrite(rechtermotorDirection,HIGH);
        digitalWrite(linkermotorDirection,HIGH);
        analogWrite(rechtermotorSpeed,150);
        analogWrite(linkermotorSpeed,150); 
        corrigeren();
      }}
      while(omdraaiIndexer!=1){  
        while(digitalRead(rechterIR)==HIGH ){  
        digitalWrite(6,HIGH);
        analogWrite(rechtermotorSpeed,150);
        digitalWrite(8,LOW);
        analogWrite(linkermotorSpeed,150);
      }

        while(digitalRead(linkerIR)==HIGH ){  
        digitalWrite(6,HIGH);
        analogWrite(rechtermotorSpeed,150);
        digitalWrite(8,LOW);
        analogWrite(linkermotorSpeed,150);
        }
        while(digitalRead(rechterIR)==HIGH ){  
        digitalWrite(6,HIGH);
        analogWrite(rechtermotorSpeed,150);
        digitalWrite(8,LOW);
        analogWrite(linkermotorSpeed,150);
      } 
        analogWrite(rechtermotorSpeed,0);
        analogWrite(linkermotorSpeed,0); 
        omdraaiIndexer=1;
             }
      currentDirection-=100;
      if(currentDirection==0){
      currentDirection=200;
      }
     
      if(currentDirection==50){
      currentDirection=250;
      }
      draaizin="";
      omdraaiIndexer=0;
         }
       }
///////////
void floyd(){ // says to the robot what he needs to do for example; turn left, go forward, turn right, go forward
  Serial.println(F("My final destination:"));
  Serial.println(goToCoordinate);
   
  while(currentCoordinate!= goToCoordinate){
    Serial.println(F("current"));
    Serial.println(currentCoordinate);
     nextCoordinate = route[n];
    n++;
    Serial.println(F("next"));
    Serial.println(nextCoordinate);
    nextDirection = pgm_read_byte(&directions[currentCoordinate-1][nextCoordinate-1]);  

    while(currentDirection!= nextDirection){
      if(((currentDirection - nextDirection) ==100) || ((currentDirection - nextDirection)==-100 )){
        draaizin="OM";
         Serial.println(F("TURN AROUND")); 
        draaien(); 
           }      
      else if(((currentDirection - nextDirection) >0) || ((currentDirection - nextDirection)==-150 )){
        draaizin="R"; 
           Serial.println(F("TURN RIGHT"));
           draaien();
           }
      else if(((currentDirection - nextDirection) <0) || ((currentDirection - nextDirection)==150 )){
        draaizin="L"; 
Serial.println(F("TURN LEFT")); 
          draaien();
           }
              
    }
    vooruit();
    Serial.println("GO FORWARD");

  }
  while(1){}  //when the robot arrived at his final destination, it stops
}



void routeMaken(){ // making route 
... to long to post here
}

void arrayOmkeren(){    //changes the place of the numbers in the array
 ... to long to post here
}

void aanpassen(){   // making route if detected an unprogrammed obstacle (not needed yet, optional)
... to long to post here
}

RoboProtect:
If i call the method draaien(); directly in the loop, it works perfect, but when i call it in the method floyd, it never ends turning.
What could be the problem or what's a possible solution?

Without studying your code carefully, that sounds very like some variable is set to some value in one case and not in the other.

You have an enormous amount of repetition in your code. For example

digitalWrite(rechtermotorDirection,HIGH);
        digitalWrite(linkermotorDirection,HIGH);
        analogWrite(rechtermotorSpeed,200);
        analogWrite(linkermotorSpeed,200); 
        corrigeren();

Putting that stuff in a function will shorten the main code so the logic is easier to see and you will only need to make changes in one place reducing the risk of typing errors.

...R

Hey,

First off all, thanks for the comment about duplicating a lot, so I made a few methods (functions).
After a few hours and a lot of printing on the serial, i found the problem.
When I used the function “vooruit”, it calls the function “corrigeren”, but it does it only once what is not correct. It’s verry strange because in the other program that was exactly the same, it worked perfect.
So I edited the function and then there were even more problems but i solved them all !,
The next step now is to mount the ultrasonic sensor and make the code for it, that we can avoid (dodge) unprogrammed abstacles. And if thats working, then it will be a huge step to let it all work over WiFi.

Thanks a lot!!

Here you can see a video of how it works. - YouTube
The obstacles are preprogrammed (but we can easily change the map/grid).

Here is the new code.

//Kjell Deboysere 09/10/2014
//Dieter Jonckheere 14/10/2014
#include <avr/pgmspace.h>

const byte arrayCoordinates[25][25] PROGMEM={
... to long to post here};

const byte directions[25][25] PROGMEM={
... to long to post here};

byte n=0;
byte volgendePositie;
byte omgekeerd[15];
byte route[15];
byte voorlopigKlein;
byte voorlopigGroot;
byte getal;
byte startCoordinate;
byte startDirection;
byte currentCoordinate;
byte currentDirection;
byte nextCoordinate;
byte nextDirection;
byte goToCoordinate;
String draaizin;
byte omdraaiIndexer;
byte linkermotorSpeed=9;
byte rechtermotorSpeed=5;
byte linkermotorDirection=6;
byte rechtermotorDirection=8;
byte linkerIR=3;
byte rechterIR=14;
byte middenIR=2;
byte beeper=16;
byte NUM=15;

////////////////////////////////////////
void setup(){
  startCoordinate=currentCoordinate=9;
  startDirection=currentDirection=250;
  goToCoordinate=10;
  pinMode(linkermotorDirection,OUTPUT); 
  pinMode(rechtermotorDirection,OUTPUT); 
  pinMode(linkerIR,INPUT); 
  pinMode(rechterIR,INPUT); 
  pinMode(middenIR,INPUT);  
  pinMode(beeper,OUTPUT);

  digitalWrite(beeper, HIGH);
  delay(15);
  digitalWrite(beeper, LOW);
  delay(200);
  digitalWrite(beeper, HIGH);
  delay(15);
  digitalWrite(beeper, LOW);
  delay(200);
  Serial.begin(9600);

  Serial.println(digitalRead(rechterIR));
  Serial.println(digitalRead(linkerIR));
  Serial.println(digitalRead(middenIR));
  delay(3000);
  routeMaken();
  for(int i=0; i<NUM;i++) {
        Serial.println(route[i]);
      }

}
////////////////////////////////////////
void loop(){

 floyd();
}
////////////////////////////////////////
void corrigeren(){  // correct direction (line following)

  while(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==LOW){
    setMotorsTurnLeft();

  }
  while(digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==HIGH){ 
    setMotorsTurnRight();

  }

}
//////////////////////////
void vooruit(){  // go FORWARD to the next crosspoint

    if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){  
       setMotorsStop();
    while( digitalRead(middenIR)==HIGH ){    
       setMotorsForward();
       corrigeren();
    }
  }
  if(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==HIGH){              

    while(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==HIGH ){

      setMotorsForward();
    }
  }
    while(!(digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW)){
  corrigeren();
   while(digitalRead(rechterIR)==HIGH && digitalRead(linkerIR)==HIGH){
  setMotorsForward();
   }
  }
   if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){
    setMotorsStop();
  }
 currentCoordinate=nextCoordinate;
}

////////////////////////////////////////
void draaien(){  // TURNING
  if (draaizin=="R"){   // TURN RIGHT
    if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){  
      while( digitalRead(middenIR)==HIGH ){
        setMotorsForward();
        corrigeren();
      }
    }
    while(digitalRead(rechterIR)==HIGH ){  
      setMotorsTurnRight();
    }
    setMotorsStop();
    currentDirection-=50;
    if(currentDirection==50){
      currentDirection=250;
    }
    draaizin="";
  }
//-----------------------
  if (draaizin=="L"){   // TURN LEFT
    if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){
      while( digitalRead(middenIR)==HIGH ){
        setMotorsForward();
        corrigeren();
      }
    }
    while(digitalRead(linkerIR)==HIGH ){  
      setMotorsTurnLeft();
    }
    setMotorsStop();
    
    
 if(currentDirection!=250){
    currentDirection+=50;
    Serial.println("inIF1");
 }
 else{
    currentDirection=100;
    Serial.println("else");
 }
    
    Serial.println("cudi");
    Serial.println(currentDirection);
    if(currentDirection==50){
      currentDirection=250;
      Serial.println("SP2");
    }

    draaizin="";
    Serial.println("SP4");
  }

//--------------------------------------------------------------------------   
  if (draaizin=="OM"){   // TURN AROUND
    if ( digitalRead(rechterIR)==LOW && digitalRead(linkerIR)==LOW){  
      while( digitalRead(middenIR)==HIGH ){
        setMotorsForward();
        corrigeren();
      }
    }
    while(omdraaiIndexer!=1){  
      while(digitalRead(rechterIR)==HIGH ){  
        setMotorsTurnRight();
      }
      while(digitalRead(linkerIR)==HIGH ){  
        setMotorsTurnRight();
      }
      while(digitalRead(rechterIR)==HIGH ){  
        setMotorsTurnRight();
      } 
      setMotorsStop();
      omdraaiIndexer=1;
    }
    currentDirection-=100;
    if(currentDirection==0){
      currentDirection=200;
    }
    if(currentDirection==50){
      currentDirection=250;
    }
    draaizin="";
    omdraaiIndexer=0;
  }
}
////////////////////////////////////////
void floyd(){ // says to the robot what he needs to do for example; turn left, go forward, turn right, go forward
  Serial.println(F("My final destination:"));
  Serial.println(goToCoordinate);
  while(currentCoordinate!= goToCoordinate){
    Serial.println(F("currentcoordinate"));
    Serial.println(currentCoordinate);
        Serial.println(F("currentdirection"));
    Serial.println(currentDirection);
    nextCoordinate = route[n];
    n++;

    nextDirection = pgm_read_byte(&directions[currentCoordinate-1][nextCoordinate-1]); 
       Serial.println(F("nextCOORDINATE"));
    Serial.println(nextCoordinate);
    Serial.println(F("nextDIRECTION"));
    Serial.println(nextDirection); 
      while(currentDirection!= nextDirection){
          if(((currentDirection - nextDirection) ==100) || ((currentDirection - nextDirection)==-100 )){
        draaizin="OM";
        Serial.println(F("TURN AROUND")); 
        draaien(); 
      }      
      else if(((currentDirection - nextDirection) ==50) || ((currentDirection - nextDirection)==-150 )){
        draaizin="R"; 
        Serial.println(F("TURN RIGHT"));
        draaien();
      }
      else if(((currentDirection - nextDirection) ==-50) || ((currentDirection - nextDirection)==150 )){
        draaizin="L"; 
        Serial.println(F("TURN LEFT")); 
        draaien();
      }
    }
    Serial.println("GO FORWARD");
    vooruit();
  }
}
////////////////////////////
void routeMaken(){ // making route 

... to long to post here};

/////////////////////////////
void arrayOmkeren(){    //changes the place of the numbers in the array

... to long to post here};
//////////////////////////////

void setMotorsForward(){
  digitalWrite(rechtermotorDirection,HIGH);
  digitalWrite(linkermotorDirection,HIGH);
  analogWrite(rechtermotorSpeed,200);
  analogWrite(linkermotorSpeed,200);
}

void setMotorsTurnLeft(){
  digitalWrite(6,LOW);
  analogWrite(rechtermotorSpeed,150);
  digitalWrite(8,HIGH);
  analogWrite(linkermotorSpeed,150);
}

void setMotorsTurnRight(){
  digitalWrite(6,HIGH);
  analogWrite(rechtermotorSpeed,150);
  digitalWrite(8,LOW);
  analogWrite(linkermotorSpeed,150);
}

void setMotorsStop(){
  analogWrite(rechtermotorSpeed,0);
  analogWrite(linkermotorSpeed,0); 
}