Please help me! i dont know what im doing wrong!

Other Hardware Robotics
1

Hello everyone, im new on this forum.

I buy an arduino board and I'm trying to build a robot tank to my son
but I doing something wrong on program code.
When I press the button (switch_Pin),
the tank move forward and when something in front is near (less than 30cm)
stop, DET_LED light on, back a little and move IR sensor to right, read rdistance
next rotate left and read ldistance.
And the next step is not working, because I need to compare rdistance and ldistance
but it not working, tank always move the same direction.
The code dont compare rdistance>ldistance and go right or ldistance>rdistance go left.
Please I need some advice! Thanks

ps: sorry about my english! im from japan!

thats the code!

#define LEFT_M 12  //Direction Motor A
#define LEFT_S 3   //Speed Motor A

#define RIGHT_M 13 //Direction Motor B d
#define RIGHT_S 11 //Speed Motor B   

#define FORWARD HIGH 
#define REVERSE LOW

#define DET_LED 9 //Led de Alerta
#define LED_PIN 10 //Led de Operacao

#define FRONT1_LED 4 //Led de Alerta
#define FRONT2_LED 5 //Led de Operacao

#define switchPin 2 //Botao de Start

#include <Servo.h> // biblioteca do servomotor
Servo servoMain;

#define FRONT_IR 0

int brightness = 0;    // how bright the LED is
int fadeAmount = 5;    // how many points to fade the LED by
long randNumber;

void start_wait() //wait for start
{ 
  // set the brightness of pin 5:
  analogWrite(LED_PIN, brightness);    

  // change the brightness for next time through the loop:
  brightness = brightness + fadeAmount;

  // reverse the direction of the fading at the ends of the fade: 
  if (brightness == 0 || brightness == 255) {
    fadeAmount = -fadeAmount ; 
    delay(25);
  }
}  

void full_stop()  //procedure FULL STOP
{
  analogWrite(RIGHT_S,LOW);
  analogWrite(LEFT_S,LOW);
}

void forward(int mspeed, int mtime)  //procedure to RUN FORWARD
{
  digitalWrite(LEFT_M,FORWARD);
  analogWrite(LEFT_S,mspeed);
  digitalWrite(RIGHT_M,FORWARD);
  analogWrite(RIGHT_S,mspeed);
  if(mtime > 0)
  {
    delay(mtime);
    analogWrite(LEFT_S,0);
    analogWrite(RIGHT_S,0);
  }
}

void reverse(int mspeed, int mtime) // procedure to RUN REVERSE
{
  digitalWrite(LEFT_M,REVERSE);
  analogWrite(LEFT_S,mspeed);
  digitalWrite(RIGHT_M,REVERSE);
  analogWrite(RIGHT_S,mspeed);
  if(mtime > 0)
  {
    delay(mtime);
    analogWrite(LEFT_S,0);
    analogWrite(RIGHT_S,0);
  }
}

void left(int mspeed, int mtime)  //procedure to go left
{
  digitalWrite(RIGHT_M,FORWARD);
  analogWrite(RIGHT_S,mspeed);
  if(mtime > 0);
  {
    delay(mtime);
    analogWrite(LEFT_S,0);
    analogWrite(RIGHT_S,0);
  }
}

void right(int mspeed, int mtime)  //procedure to go right
{
  digitalWrite(LEFT_M,FORWARD);
  analogWrite(LEFT_S,mspeed);
  if(mtime > 0);
  {
    delay(mtime);
    analogWrite(LEFT_S,0);
    analogWrite(RIGHT_S,0);
  }
}

int front_sens()
{
  int tmp,distance_r,distance_l,distance;
  tmp = analogRead(FRONT_IR);
  if(tmp < 4) tmp = 4;
  distance = (6787 / (tmp / 3)) -4;
  return(distance);
}

int rfront_sens()
{
  int tmp,rdistance,ldistance,distance;
  tmp = analogRead(FRONT_IR);
  if(tmp < 4) tmp = 4;
  rdistance = (6787 / (tmp / 3)) -4;
  return(rdistance);
}

int lfront_sens()
{
  int tmp,rdistance,ldistance,distance;
  tmp = analogRead(FRONT_IR);
  if(tmp < 4) tmp = 4;
  ldistance = (6787 / (tmp / 3)) -4;
  return(ldistance);
}


int val;             //variavel do botao
int buttonState = 0; //estado do botao inicial

void setup()
{
  // Motores
  pinMode (LEFT_M,OUTPUT);
  pinMode (LEFT_S,OUTPUT);
  pinMode (RIGHT_M,OUTPUT);
  pinMode (RIGHT_S,OUTPUT);
  //botao de start
  pinMode (switchPin,INPUT);
  //LEDs
  pinMode (LED_PIN,OUTPUT);
  pinMode (DET_LED,OUTPUT);
  //Servomotor
  servoMain.attach(8);
  Serial.begin(9600);
}

void loop()
{
  int tmp,rdistance,ldistance,distance;
  int left_val;
  int right_val;
  int i;

  val = digitalRead(switchPin);
  full_stop();
  digitalWrite(LED_PIN,HIGH);
  if(val != buttonState)
  {
    while (val == LOW)
    {
      digitalWrite(LED_PIN,HIGH);

      if(val == HIGH)
      {
        Serial.println("button is not pressed");
        break;  
      }
      else
      {
        digitalWrite(LED_PIN,HIGH);
        Serial.println("button is pressed");
        delay(1000);
        //forward(255,0);
        while(front_sens() > 30)
        {
          
          forward(200,0);
        }
        if(front_sens() < 30); //something near
        {
          digitalWrite(DET_LED,HIGH);
          Serial.println("Warning Warning");
          full_stop(); // parada total
          delay(1000);

          reverse(200,500);
          full_stop();
          delay(1000);

          left_val=0;
          right_val=0;

          servoMain.write(5);
          delay(1000);
          int tmp,rdistance,ldistance,distance;
          tmp = analogRead(FRONT_IR);
          if(tmp < 4) tmp = 4;
          rdistance = (6787 / (tmp / 3)) -4;
          right_val = rdistance;
          Serial.print(rdistance,DEC);
          Serial.print(" cm\n");
          delay(1000);

          servoMain.write(90);
          delay(1000);

          servoMain.write(175);
          delay(1000);
          tmp = analogRead(FRONT_IR);
          if(tmp < 4) tmp = 4;
          ldistance = (6787 / (tmp / 3)) -4;
          left_val = ldistance;
          Serial.print(ldistance,DEC);
          Serial.print(" cm\n");
          delay(1000);
          
          servoMain.write(90);
          delay(500);

          if(left_val=right_val)
          {
            while(1)
            {
              delay(1000);
              left(200,900);
              break;
            }
          }
          else 
            if(right_val>left_val)
          {
            while(1)
            {
              delay(1000);
              left(200,900);
              break;
            }
          }
          else
          {
            if(right_val<left_val)
            {
              while(1)
              {
                delay(1000);
                right(200,900);
                if(front_sens()>25)
                {
                  delay(500);
                  break;
                }
              }

            }

          }
          digitalWrite(DET_LED,LOW);
          delay(500);  
        }  
      }
    }
  }
  buttonState = val;
}
2 
if(mtime > 0);

Lose the semicolon (";")

int front_sens()
{
  int tmp,distance_r,distance_l,distance;
  tmp = analogRead(FRONT_IR);
  if(tmp < 4) tmp = 4;
  distance = (6787 / (tmp / 3)) -4;
  return(distance);
}

There's no need for "distance_r" or "distance_l"

int front_sens()
{
  int tmp = max (4, analogRead(FRONT_IR));
  return  (6787 / (tmp / 3)) -4;
}

Better still:

int range(byte rangePin)
{
  int tmp = max (4, analogRead(rangePin));
  return  (6787 / (tmp / 3)) -4;
}
3

Thank you for reply AWOL
I delete the ";", but I think my problem is inside the void loop()

because I create a rdistance and ldistance but the tank still doing the last "else"

right(200,900);

I read a lot of help about "if" "else if" and "else" but i cant make the tank to go left or right! :blush:

If someone know what im doing wrong or have a advice to use "if", plz tell me! thank

4

You have a few variables with the same name in the main loop, so what you think you're using may not be what the compiler is using - it looks like you've just cut-and-pasted code without regard for the excess variables.
I'd look to simplify, and take out the inlined ranging in "loop()" and use the function I showed instead.

5

another advice! up

6

Post your code, post your results/observations.

if(left_val=right_val)

That's not going to go well.

if(front_sens() < 30);

Nor that.

Also, this

while(1)
            {
              delay(1000);
              left(200,900);
              break;
            }

which is repeated with slight variations a few times, is the same as

delay(1000);
left(200,900);

, which is shorter and easier to read.