Line following robot package picker

[danielrab]

Hi everyone,

I wrote some code for a line following robot that is used to pick up a box using pincers. See photo attached. It is meant to do the following functions:

1. Follow a line by using your line sensors.
2. Detect a box placed along the line using your ultrasonic sensor.
3. Pick up the box using the forks on your rover.
4. Continue to follow the line until a wall/barrier is detected at 15 cm.
5. Lower the box onto the floor.
6. Move backwards until the box is at 15 cm.

My code seems to compile and run but when it gets to the second operation it automatically reverts back to 0 and I can't seem to understand why.
Any help would be appreciated.

#include <ArduinoMotorCarrier.h>
int operation = 0; // Initialize operation variable
int R_S = A7;
int L_S = A2;
int state1;
int state2;
const int trig = A6; // TRIG pin
const int echo = A3; // ECHO pin
const int led = 8; // LED Pins
const float c = 343; // Speed of sound [m/s]
float t; // Time Variable
float distance; // Distance Variable
void setup()
{
  // put your setup code here, to run once:
  Serial.begin(9600);
  controller.begin();
  pinMode(R_S, INPUT);
  pinMode(L_S, INPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(led, OUTPUT);
  servo3.setAngle(30);
  operation = 0;
}

void loop()
{
  // put your main code here, to run repeatedly:
  state1 = analogRead(R_S);
  state2 = analogRead(L_S);
  Serial.write("left ");
  Serial.println(state2);
  Serial.write("right ");
  Serial.println(state1);
  Serial.write("distance ");
  Serial.println(distance);
  Serial.println(operation);
  distancemeasure();
  //digitalWrite(trig, HIGH); // Start ultrasound
  ////delayMicroseconds(10); // 10 microsecond delay
  //digitalWrite(trig, LOW); // End ultrasound
  //t = pulseIn(echo, HIGH); // Measure time taken [microseconds]
  //t = t*(0.000001); // [microseconds] -> [seconds]
  //distance = (t/2)*c; // distance [meters]
  //distance = distance*100; // [meters] -> [centimeters]
  if (distance > 5) //block not seen
  {
    operation = 0;
  }
  else
  {
    operation = 1;
  }
  if (operation == 0)
  {
    servo3.setAngle(140);
    servo1.setAngle(40);
    linefollow();
  }
  if (operation == 1)
  {
    delay(100);
    pick();
    delay(3000);
    operation = operation + 1;
  }
  if (operation == 2)
  {
    distancemeasure();
    if (distance > 15)
    {
      linefollow2();
      //  delay(1000);
    }
    else if (distance < 15)
    {
      stoprover();
      delay(3000);
      operation = operation + 1;
      delay(3000);
    }
  }
  if (operation == 3)
  {
    servo1.setAngle(0);
    servo3.setAngle(30);
    reverse();
    delay(3000);
    operation = operation + 1;
  }
  if (operation == 4)
  {
    stoprover();
    delay(10000);
  }
}
//-----------------------------------
//FUNCTIONS
//-----------------------------------
void linefollow()
{
  if ((state1 < 70) && (state2 < 70)) //forward
  {
    M1.setDuty(18);
    M2.setDuty(18);
    Serial.println(19);
  }
  else if ((state1 > 70) && (analogRead(L_S) < 70)) //right high therefore right turn
  {
    //RightTurn(30,15);
    M1.setDuty(8);
    M2.setDuty(20);
  }
  else if ((analogRead(R_S) < 70) && (analogRead(L_S) > 70)) //left high therefore left turn
  {
    //LeftTurn(15,30);
    M1.setDuty(20);
    M2.setDuty(8);
  }
  else if ((analogRead(R_S) > 70) && (analogRead(L_S) > 70)) //stop
  {
    M1.setDuty(0);
    M2.setDuty(0);
  }
}


void go()
{
  M1.setDuty(25);
  M2.setDuty(25);
}
void reverse()
{
  M1.setDuty(-25);
  M2.setDuty(-25);
}

void pick()
{
  M1.setDuty(0);
  M2.setDuty(0);
  servo3.setAngle(0);
  delay(1000);
  servo1.setAngle(140);
  delay(1000);
  servo3.setAngle(140);
  delay(5000);
}
void stoprover()
{
  M1.setDuty(0);
  M2.setDuty(0);
}

void linefollow2()
{
  servo3.setAngle(140);
  servo1.setAngle(120);
  if ((state1 < 70) && (state2 < 70)) //forward
  {
    M1.setDuty(18);
    M2.setDuty(20);
  }
  else if ((state1 > 70) && (analogRead(L_S) < 70)) //right high therefore right turn
  {
    //RightTurn(30,15);
    M1.setDuty(8);
    M2.setDuty(22);
  }
  else if ((analogRead(R_S) < 70) && (analogRead(L_S) > 70)) //left high therefore left turn
  {
    //LeftTurn(15,30);
    M1.setDuty(20);
    M2.setDuty(8);
  }
  else if ((analogRead(R_S) > 70) && (analogRead(L_S) > 70)) //stop
  {
    M1.setDuty(0);
    M2.setDuty(0);
  }
}

void distancemeasure()
{
  //  // put your main code here, to run repeatedly:
  digitalWrite(trig, HIGH); // Start ultrasound
  //delayMicroseconds(10000); // 10 microsecond delay
  digitalWrite(trig, LOW); // End ultrasound
  t = pulseIn(echo, HIGH); // Measure time taken [microseconds]
  t = t * (0.000001); // [microseconds] -> [seconds]
  distance = (t / 2) * c; // distance [meters]
  distance = distance * 100; // [meters] -> [centimeters]
  Serial.print("distance: "); // Print "Distance"
  Serial.print(distance); // Print Measured Distance
  Serial.println(" cm"); // Print Distance Units
  delay(50);
}

[UKHeliBob]

Is this a follow-on question from LEDS with distance sensor and for loop

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the </> icon above the compose window) to make it easier to read and copy for examination

How to get the best out of this forum

See also FAQ - Arduino Forum for general rules on forum behaviour and etiquette. Hello, Welcome to the Arduino Forum. This guide explains how to get the best out of this forum. Please read and follow the instructions below. Being new here you might think this is having rules for the sake of rules, but that is not the case. If you don’t follow the guidelines all that happens is there is a long exchange of posts while we try to get you to tell us what we need in order to help you, which is fru…

[danielrab]

Hi, yes it is similar to the LED one but its not the exact same and that's why I kept it as a separate post. I have fixed the code though to be formatted properly

[Railroader]

Please use the autoformat function in the IDE to make real formatting.

[danielrab]

I'm sorry, I dont see how the formatting is supposed to be different from how it is now? If it's causing an issue I really do apologize but I was hoping to get some help regarding the code?

[UKHeliBob]

Auto Formatting the code makes the code blocks easier to see which can be helpful when multiple if/else is involved

Your code Auto Formatted

#include <ArduinoMotorCarrier.h>
int operation = 0; // Initialize operation variable
int R_S = A7;
int L_S = A2;
int state1;
int state2;
const int trig = A6; // TRIG pin
const int echo = A3; // ECHO pin
const int led = 8; // LED Pins
const float c = 343; // Speed of sound [m/s]
float t; // Time Variable
float distance; // Distance Variable
void setup()
{
  // put your setup code here, to run once:
  Serial.begin(9600);
  controller.begin();
  pinMode(R_S, INPUT);
  pinMode(L_S, INPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(led, OUTPUT);
  servo3.setAngle(30);
  operation = 0;
}

void loop()
{
  // put your main code here, to run repeatedly:
  state1 = analogRead(R_S);
  state2 = analogRead(L_S);
  Serial.write("left ");
  Serial.println(state2);
  Serial.write("right ");
  Serial.println(state1);
  Serial.write("distance ");
  Serial.println(distance);
  Serial.println(operation);
  distancemeasure();
  //digitalWrite(trig, HIGH); // Start ultrasound
  ////delayMicroseconds(10); // 10 microsecond delay
  //digitalWrite(trig, LOW); // End ultrasound
  //t = pulseIn(echo, HIGH); // Measure time taken [microseconds]
  //t = t*(0.000001); // [microseconds] -> [seconds]
  //distance = (t/2)*c; // distance [meters]
  //distance = distance*100; // [meters] -> [centimeters]
  if (distance > 5) //block not seen
  {
    operation = 0;
  }
  else
  {
    operation = 1;
  }
  if (operation == 0)
  {
    servo3.setAngle(140);
    servo1.setAngle(40);
    linefollow();
  }
  if (operation == 1)
  {
    delay(100);
    pick();
    delay(3000);
    operation = operation + 1;
  }
  if (operation == 2)
  {
    distancemeasure();
    if (distance > 15)
    {
      linefollow2();
      //  delay(1000);
    }
    else if (distance < 15)
    {
      stoprover();
      delay(3000);
      operation = operation + 1;
      delay(3000);
    }
  }
  if (operation == 3)
  {
    servo1.setAngle(0);
    servo3.setAngle(30);
    reverse();
    delay(3000);
    operation = operation + 1;
  }
  if (operation == 4)
  {
    stoprover();
    delay(10000);
  }
}
//-----------------------------------
//FUNCTIONS
//-----------------------------------
void linefollow()
{
  if ((state1 < 70) && (state2 < 70)) //forward
  {
    M1.setDuty(18);
    M2.setDuty(18);
    Serial.println(19);
  }
  else if ((state1 > 70) && (analogRead(L_S) < 70)) //right high therefore right turn
  {
    //RightTurn(30,15);
    M1.setDuty(8);
    M2.setDuty(20);
  }
  else if ((analogRead(R_S) < 70) && (analogRead(L_S) > 70)) //left high therefore left turn
  {
    //LeftTurn(15,30);
    M1.setDuty(20);
    M2.setDuty(8);
  }
  else if ((analogRead(R_S) > 70) && (analogRead(L_S) > 70)) //stop
  {
    M1.setDuty(0);
    M2.setDuty(0);
  }
}


void go()
{
  M1.setDuty(25);
  M2.setDuty(25);
}
void reverse()
{
  M1.setDuty(-25);
  M2.setDuty(-25);
}

void pick()
{
  M1.setDuty(0);
  M2.setDuty(0);
  servo3.setAngle(0);
  delay(1000);
  servo1.setAngle(140);
  delay(1000);
  servo3.setAngle(140);
  delay(5000);
}
void stoprover()
{
  M1.setDuty(0);
  M2.setDuty(0);
}

void linefollow2()
{
  servo3.setAngle(140);
  servo1.setAngle(120);
  if ((state1 < 70) && (state2 < 70)) //forward
  {
    M1.setDuty(18);
    M2.setDuty(20);
  }
  else if ((state1 > 70) && (analogRead(L_S) < 70)) //right high therefore right turn
  {
    //RightTurn(30,15);
    M1.setDuty(8);
    M2.setDuty(22);
  }
  else if ((analogRead(R_S) < 70) && (analogRead(L_S) > 70)) //left high therefore left turn
  {
    //LeftTurn(15,30);
    M1.setDuty(20);
    M2.setDuty(8);
  }
  else if ((analogRead(R_S) > 70) && (analogRead(L_S) > 70)) //stop
  {
    M1.setDuty(0);
    M2.setDuty(0);
  }
}

void distancemeasure()
{
  //  // put your main code here, to run repeatedly:
  digitalWrite(trig, HIGH); // Start ultrasound
  //delayMicroseconds(10000); // 10 microsecond delay
  digitalWrite(trig, LOW); // End ultrasound
  t = pulseIn(echo, HIGH); // Measure time taken [microseconds]
  t = t * (0.000001); // [microseconds] -> [seconds]
  distance = (t / 2) * c; // distance [meters]
  distance = distance * 100; // [meters] -> [centimeters]
  Serial.print("distance: "); // Print "Distance"
  Serial.print(distance); // Print Measured Distance
  Serial.println(" cm"); // Print Distance Units
  delay(50);
}

[danielrab]

I copied and pasted the formatted code above.

[ToddL1962]

There are only 2 ways in your code that operation can be set back to 0. First is in setup() so put a Serial.println() statement in setup after Serial.begin(). Second is in loop() when distance > 5. Put a Serial.println() in that conditional.

If it is a reset then it is usually a power issue. How are your servos powered? Do they have a separate source from the Arduino? They need to be powered separately or they cause a voltage drop which causes the Arduino to get reset.