Need help with robot program

[mwebby93]

My school is having us do a robot project that basically goes around a track and picks up 12 golf balls and then deposits them in a box in the middle. I have posted a while ago with some questions and the overview of the project at this link:

http://arduino.cc/forum/index.php/topic,148424.msg1115163.html#msg1115163


My team and I have gotten the robot working for the most part but it seems like one day we get the coding working the same way everytime and the next day we get issues and it won't run like the night before. We are using a ping sensor, IR sensor, two servos, two DC drive motors, one lift motor, and a QRB IR sensor. We are using the arduino uno and arduino motor shield to control the two drive motors. The robot drives around the track, collects 4 golf balls on on straight side of the square track, gets just so far from the wall in front of the robot and turns left until it senses it has turned far enough and the repeats. There are four black lines that cross to make an x on the track (shown in the other post) that I use the QRB sensor to increase a counter and exit the while loop of picking up balls and turning. Once the counter is the right number, it collects the last two balls, backs up until it hits a black line, then turns and drives toward the center box. We haven't gotten it to raise the pan that has the golf balls and dump yet but I have tried coding it but it didn't work out. Hope this makes sense as it is a quick overview of what this program is supposed to do.

What I am hoping is to get some feedback on the coding we have done to see if maybe there are some things conflicting in the coding or maybe there is a much simpler way to write it. If there doesn't seem to be anything wrong with the coding really then I think it could have something to do with my circuitry because I am not anywhere near being that good at it. I just took some stuff I learned in my digital electronics class and attempted to put together something that would work.

Any help I can get would be greatly appreciated and if there are any questions to what something in the coding's purpose is I will answer the best I can.

[mwebby93]

Code: [Select]

#include <Servo.h>
#include <NewPing.h>

#define TRIGGER_PIN  A4
#define ECHO_PIN     A4
#define MAX_DISTANCE 400

#define TURNLEFT 1       //Defines variable used in void turning
#define FINALTURNLEFT 2

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

Servo rightservo;  // create servo object to control right servo arm
Servo leftservo;  //creates servo object to control left servo arm

int sidesensor;
int frontsensor;
int rsopen = 160;
int lsopen = 30;
int rsclose = 65;
int lsclose = 110;
int BASE = 200;
int RMOTOR;
int LMOTOR;
double KP = 5.5;
int SETPOINT = 215;
int fSETPOINT = 100;
int sensorPin = A3;
int linesensorPin = A2;
int linesensor;
int flag = 0;
int trueturn = 0;
int count;
int dump = 0;
int buttonpin = 7;

long interval = 500;
long pinginterval = 33;

void setup() {
 Serial.begin(115200);

 pinMode(12, OUTPUT);   //Initiates Motor Channel A pin - Right Motor
 pinMode(9, OUTPUT);    //Initiates Brake Channel A pin  - Right Motor
 pinMode(13, OUTPUT);   //Initiates Motor Channel B pin - Left Motor
 pinMode(8, OUTPUT);    //Initiates Brake Channel B pin  - Left Motor
 
 pinMode(4, OUTPUT);    //Initiates the lift motor
 pinMode(10, OUTPUT);   //Initiates the lift motor
 
 pinMode(buttonpin, INPUT);
 
 rightservo.attach(5);  // attaches the right arm servo on pin 5
 rightservo.write(160); // tells servo to go to position 160
 leftservo.attach(6);   // attaches the left arm servo on pin 6
 leftservo.write(30);   // tells the servo to go to position 30
 

 delay(500);                       // waits 500ms for the servo to reach the position

}


void loop() {

 while (flag == 0){

   unsigned long currentMillis = millis();
   int frontsensor = sonar.ping_cm();

   while(millis() - currentMillis < pinginterval) {
     int linesensor = analogRead(linesensorPin);

     drive();
     if (++count > 300 && linesensor > 700)   //Compares lastOpen to 500 and ballcounter > 0 if true itll execute code
     {
       trueturn = trueturn + 1;
       count = 0;
     }
     if (frontsensor >= 165 && frontsensor <= 175){ //Activates right servo arm is front sensor between 180 and 170cm or turnarm equals 1
       flag = 1;
     }

     if (frontsensor >= 100 && frontsensor <= 105){  //Increases counter variable if front sensor is between 105 and 95cm    
       flag = 2;
     }

     if (frontsensor <= 63 && frontsensor > 30){  //Makes left turn if front sensor less than 60cm and ballcounter is greater than 1

       flag = 4;

     }
   }


   while(flag==1){  

     unsigned long currentMillis = millis();

     while(millis() - currentMillis < interval) {

       drive();
       rightservo.write(rsclose);              // tells right servo to go to position 70
     }


     flag = 3;
   }



   while(flag==2){

     unsigned long currentMillis = millis();

     while(millis() - currentMillis < interval) {    

       drive();
       rightservo.write(rsclose);                      //Tells the right servo to go to position 70
       leftservo.write(lsclose);                      //Tells the left servo to go to position 130
     }

     flag = 3;

   }

   while(flag==3){

     unsigned long currentMillis = millis();

     while(millis() - currentMillis < interval) {

       drive();
       rightservo.write(rsopen);                      //Tells the right servo to go to position 70
       leftservo.write(lsopen);                      //Tells the left servo to go to position 130

     }

     flag = 0;

   }

   while(flag==4){

     unsigned long currentMillis = millis();

     int frontsensor = sonar.ping_cm();
     while(millis() - currentMillis < pinginterval) {    

       int sidesensor = analogRead(sensorPin);
       turning(TURNLEFT);

       if (sidesensor > 195 && sidesensor < 210 && frontsensor > 140){

         flag = 0;

       }
     }
   }

   while (trueturn == 4){
     int linesensor = analogRead(linesensorPin);
     int frontsensor = sonar.ping_cm();

     drive();

     if (frontsensor >= 95 && frontsensor <= 105){
       flag = 5;
     }

     while(flag == 5){

       stopdrive();

       unsigned long currentMillis = millis();
       while(millis() - currentMillis < interval) {    

         rightservo.write(rsclose);                      //Tells the right servo to go to position 70
         leftservo.write(lsclose);                      //Tells the left servo to go to position 130
       }

       currentMillis = millis();
       while(millis() - currentMillis < interval) {    

         rightservo.write(rsopen);                      //Tells the right servo to go to position 70
         leftservo.write(lsopen);                      //Tells the left servo to go to position 130
       }

       flag = 6;
     }
     while(flag == 6){
       int linesensor = analogRead(linesensorPin);
       reverse();

       if (linesensor >= 700){

         flag = 7;
       }
     }
     while(flag == 7 ){

       int frontsensor = sonar.ping_cm();        
       finalturning(FINALTURNLEFT);

       if (frontsensor >= 10 && frontsensor <= 50){

         flag = 8;

       }
     }


     while(flag == 8){
       unsigned long currentMillis = millis();
       int frontsensor = sonar.ping_cm();
       while(millis() - currentMillis < pinginterval) {

         finaldrive();

         if (frontsensor <= 10 && frontsensor >=1){
           flag = 9;
         }
       }
     }

     while(flag == 9){
       stopdrive();
       digitalWrite(4, HIGH);  //Initiates Motor Channel B pin - Left Motor
       digitalWrite(10, LOW);
       
      if(digitalRead(buttonpin) == HIGH){
        flag = 10;
      }
     }
     while(flag == 10){
       digitalWrite(4, HIGH);  //Initiates Motor Channel B pin - Left Motor
       digitalWrite(10, HIGH);
       stopdrive();
   }

 }
}
}

void drive() {

 int sidesensor = analogRead(sensorPin);  
 RMOTOR = BASE - KP * (SETPOINT - sidesensor);
 LMOTOR = BASE + KP * (SETPOINT - sidesensor);

 if (RMOTOR > 255){
   RMOTOR = 255;
 }
 if (LMOTOR > 255){
   LMOTOR = 255;
 }
 if (RMOTOR < 0){
   RMOTOR = 0;
 }
 if (LMOTOR < 0){
   LMOTOR = 0;
 }

 digitalWrite(12, LOW);  //Establishes forward direction of Channel A - Right Motor
 analogWrite(3, RMOTOR);    //Spins the motor on Channel A at full speed - Right Motor
 
 digitalWrite(13, HIGH);  //Initiates Motor Channel B pin - Left Motor
 analogWrite(11, LMOTOR);    //Spins the motor on Channel B at 100 speed - Left Motor
 
 digitalWrite(4, HIGH);  //Initiates Motor Channel B pin - Left Motor
 digitalWrite(10, HIGH);
 
}

void turning(int leftturn) {

 switch (leftturn) {

 case TURNLEFT:

   digitalWrite(12, LOW);  //Establishes forward direction of Channel A - Right Motor
   analogWrite(3, 200);    //Spins the motor on Channel A at full speed - Right Motor

   digitalWrite(13, LOW);  //Establishes reverse direction of Channel B - Left Motor
   analogWrite(11, 200);   //Spins the motor on Channel B at full speed - Left Motor

   break;
 }
}

void finalturning(int finalleftturn) {

 switch (finalleftturn) {

 case FINALTURNLEFT:

   digitalWrite(12, LOW);  //Establishes forward direction of Channel A - Right Motor
   analogWrite(3, 255);    //Spins the motor on Channel A at full speed - Right Motor

   digitalWrite(13, LOW);  //Establishes reverse direction of Channel B - Left Motor
   analogWrite(11, 100);   //Spins the motor on Channel B at full speed - Left Motor

   break;
 }
}

void stopdrive() {
 digitalWrite(12, LOW);  //Establishes forward direction of Channel A - Right Motor
 analogWrite(3, 0);    //Spins the motor on Channel A at full speed - Right Motor

 digitalWrite(13, HIGH);  //Initiates Motor Channel B pin - Left Motor
 analogWrite(11, 0);    //Spins the motor on Channel B at 100 speed - Left Motor

}

void reverse(){
 digitalWrite(12, HIGH);  //Establishes forward direction of Channel A - Right Motor
 analogWrite(3, 225);    //Spins the motor on Channel A at full speed - Right Motor
 
 digitalWrite(13, LOW);  //Initiates Motor Channel B pin - Left Motor
 analogWrite(11, 225);    //Spins the motor on Channel B at 100 speed - Left Motor
}

void finaldrive() {

 int sidesensor = analogRead(sensorPin);  
 RMOTOR = BASE - KP * (fSETPOINT - sidesensor);
 LMOTOR = BASE + KP * (fSETPOINT - sidesensor);

 if (RMOTOR > 255){
   RMOTOR = 255;
 }
 if (LMOTOR > 255){
   LMOTOR = 255;
 }
 if (RMOTOR < 0){
   RMOTOR = 0;
 }
 if (LMOTOR < 0){
   LMOTOR = 0;
 }

 digitalWrite(12, LOW);  //Establishes forward direction of Channel A - Right Motor
 analogWrite(3, RMOTOR);    //Spins the motor on Channel A at full speed - Right Motor
 
 digitalWrite(13, HIGH);  //Initiates Motor Channel B pin - Left Motor
 analogWrite(11, LMOTOR);    //Spins the motor on Channel B at 100 speed - Left Motor
}




[mwebby93]

Just looking to see if anyone can see anything interfering or a better way this could be coded such as a simpler way or more efficient process. Also im looking for a way to activate a motor when the front ping sensor is a certain distance away from the middle box and then shut off after a button gets pressed.  Is there an easy way to do this when using an h bridge chip?

[oric_dan]

It's very difficult to debug code when you don't have the working system in front of you,
and some coding formats are much more difficult to understand than others. So just some
general recommendations.

1. it's harder to understand code where [multi-step] operations are intertwined with the
   logical flow. To me, your loop() is too complicated, and I'd put an upper level [logical]
   switch() in there.

For something like a state-machine, I code the logical flow at one level and use subroutine
calls for the low-level operations. Now that functions are separated better, it's easier to
troubleshoot one or the other. Also, it's easier to modify each and keep them straight. Eg, you
can add dozens of steps any of the operational subroutine calls, and the upper level logical code
won't change. And it's easier to change the logical flow without all the low-level stuff getting
in the way.

Code: [Select]

void loop() {

 switch( flag ) {
   case 0: flag=state0();   // pertinent comment here, or change label
                            //   state0() to something more descriptive.
   break;
   case 1: flag=state1();
           break;
   ....
   ....
   default: flag=0;
 }
}

int state0()
{
   unsigned long currentMillis = millis();
   int frontsensor = sonar.ping_cm();

   while(millis() - currentMillis < pinginterval) {
     int linesensor = analogRead(linesensorPin);

     drive();
     if (++count > 300 && linesensor > 700)   //Compares lastOpen to 500 and ballcounter > 0 if true itll execute code
     {
       trueturn = trueturn + 1;
       count = 0;
     }
     if (frontsensor >= 165 && frontsensor <= 175){ //Activates right servo arm is front sensor between 180 and 170cm or turnarm equals 1
       return( 1 );   // flag = 1;
     }
     if (frontsensor >= 100 && frontsensor <= 105){  //Increases counter variable if front sensor is between 105 and 95cm    
       return( 2 );    // flag = 2;
     }
     if (frontsensor <= 63 && frontsensor > 30){  //Makes left turn if front sensor less than 60cm and ballcounter is greater than 1
       return( 4 );    // flag = 4;
     }
   }
}

int state1()
{
     unsigned long currentMillis = millis();

     while(millis() - currentMillis < interval) {
       drive();
       rightservo.write(rsclose);              // tells right servo to go to position 70
     }
     return( 3 );   // flag = 3;
   }
}

2. Also, when you do it this way, the first thing that comes out is that some of these state()
   functions don't have a proper return value for all situations, so now you have to add them
   into the code, and that may be a clue as to why some things don't work properly.

3. the other thing I would do is use functional names on the different states, so instead of
    saying case 4: and return(4); , it would say return(TURN_LEFT), etc, and then have a set
    of defines at the top.

Code: [Select]


    #define RIGHT_ARM   0
    #define TURN_LEFT   4
    ....
    case TURN_LEFT: flag=state4();  break;
    ....

4. you have some switch statements in your current subroutine calls with only 1 case. Obviously,
   the code has gone through multiple iterations, but people haven't taken the time to clean things
   up long the way. You have to clean up as you go along, or else everything ends up getting more
   and more jumbled, and buggy.
   
Other than that, the basic coding looks pretty good.

[mwebby93]

Thanks for the feedback. I was actually thinking about doing switch like you suggest but thought doing the while loops would be simple at the time just to get the coding moving along. My group actually had a qualifier for a state competition and came In first today so we now have a week to fine tune everything so I will definitely look into what you have suggested.

[oric_dan]

Congratulations and good luck. It definitely pays to keep the code cleaned up as you
go along, especially when there are multiple people working on the project. Keeping
the high-level logic separate from the low-level operations becomes more and more
important as the program gets larger. Using functional labels is really important, so
you don't end up having to continually cross-reference every little thing. Good start.