Lawn mower arduino mega

[neno1991]

Hi there, i am building an arduino lawn mower. I have 4 ultrasonic sensors hc-sr04, but only one is working. Can someone check what did i do wrong? my programming skills are not that great. thanks

#include <NewPing.h>
#include <Servo.h>
#include "U8glib.h"
#include <TimerOne.h>

Servo myservo;

int green;
#define s0 53
#define s1 52
#define s2 51
#define s3 50
#define sensorOut 49
#define TRIGGER_PIN 11
#define ECHO_PIN 10
#define MAX_DISTANCE 1000
#define TRIGGER_PIN 9
#define ECHO_PIN 8
#define MAX_DISTANCE 1000
#define TRIGGER_PIN 2
#define ECHO_PIN 3
#define MAX_DISTANCE 1000
#define TRIGGER_PIN 4
#define ECHO_PIN 7
#define MAX_DISTANCE 1000
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE); // VDD=5V GND=GND SCL=SCL SDA=SDA
NewPing sonar1(11, 10, 100);
NewPing sonar2(9, 8, 50);
NewPing sonar3(2, 3, 50);
NewPing sonar4(4, 7,50);

#define motor_aPin1 A0
#define motor_aPin2 A1
#define motor_bPin1 A2
#define motor_bPin2 A3
#define motor_aEnable 5
#define motor_bEnable 6
#define OB_range 30

int i=0, pos = 0,current_distance=0;
int range0=0, range30=0, range60=0, range85=0,range90=0,range95=0, range120=0, range150=0, range180=0 ;
unsigned long previous_millis = 0;
char serialData;

void setup()
{
Serial.begin(9600);
myservo.attach(12);
u8g.setRot180();
}

void draw(int distance) {
u8g.setFont(u8g_font_fur30);
u8g.setPrintPos(0, 63);
u8g.print(distance);
u8g.print("cm");

pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(sensorOut, INPUT);
digitalWrite(s1,LOW);
digitalWrite(s0,HIGH);
pinMode(motor_aPin1,OUTPUT);
pinMode(motor_aPin2,OUTPUT);
pinMode(motor_bPin1,OUTPUT);
pinMode(motor_bPin2,OUTPUT);
pinMode(motor_aEnable,OUTPUT);
pinMode(motor_bEnable,OUTPUT);
analogWrite(motor_aEnable,255);
analogWrite(motor_bEnable,255);

}

void brake()
{
digitalWrite(motor_aPin1,LOW);
digitalWrite(motor_aPin2,LOW);
digitalWrite(motor_bPin1,LOW);
digitalWrite(motor_bPin2,LOW);

}

void forward()
{
digitalWrite(motor_aPin1,LOW);
digitalWrite(motor_aPin2,HIGH);
digitalWrite(motor_bPin1,LOW);
digitalWrite(motor_bPin2,HIGH);

}

void forward_left()
{
digitalWrite(motor_aPin1,LOW);
digitalWrite(motor_aPin2,HIGH);
digitalWrite(motor_bPin1,LOW);
digitalWrite(motor_bPin2,LOW);

}

void forward_right()
{
digitalWrite(motor_aPin1,LOW);
digitalWrite(motor_aPin2,LOW);
digitalWrite(motor_bPin1,LOW);
digitalWrite(motor_bPin2,HIGH);

}

void backward()
{
digitalWrite(motor_aPin1,HIGH);
digitalWrite(motor_aPin2,LOW);
digitalWrite(motor_bPin1,HIGH);
digitalWrite(motor_bPin2,LOW);

}

void backward_right()
{
digitalWrite(motor_aPin1,LOW);
digitalWrite(motor_aPin2,LOW);
digitalWrite(motor_bPin1,HIGH);
digitalWrite(motor_bPin2,LOW);

}
void backward_left()
{
digitalWrite(motor_aPin1,HIGH);
digitalWrite(motor_aPin2,LOW);
digitalWrite(motor_bPin1,LOW);
digitalWrite(motor_bPin2,LOW);

}

void left()
{
digitalWrite(motor_aPin1,LOW);
digitalWrite(motor_aPin2,HIGH);
digitalWrite(motor_bPin1,HIGH);
digitalWrite(motor_bPin2,LOW);

}

void right()
{
digitalWrite(motor_aPin1,HIGH);
digitalWrite(motor_aPin2,LOW);
digitalWrite(motor_bPin1,LOW);
digitalWrite(motor_bPin2,HIGH);

}
int range(int pos)
{
myservo.write(pos);
delay(300);
current_distance = sonar1.ping_cm();
if(current_distance==0)
current_distance=100;
if(current_distance > 0 && current_distance < 15){
Serial.print("B");
if(pos==90){
backward();delay(500);}
if(pos < 90){
backward_right();delay(500);}
if(pos > 90){
backward_left();delay(500);}
return current_distance;

}}

void loop() {
int dist = sonar1.ping_cm();
u8g.firstPage();
do {
draw(dist);
}
while (u8g.nextPage());

delay(100);

Automatic: brake();
delay(300);

while(1){

if(Serial.available()>0)
serialData=Serial.read();
if(serialData=='M')
goto Manual;

above: range90=0;

range90=range(90);
delay(10);

while(range90 >= OB_range ){

if(millis()-previous_millis>2000){
previous_millis=millis();
range(180);
delay(50);
range(0);
delay(50);}

range90=range(90);
analogWrite(motor_aEnable,150);
analogWrite(motor_bEnable,150);

forward(); Serial.print("F");

if(Serial.available()>0)
serialData=Serial.read();
if(serialData=='M')
goto Manual;
}
brake();

if(range90 <OB_range)
{
analogWrite(motor_aEnable,100);
analogWrite(motor_bEnable,100);
brake();
range60=0;
range60=range(60);
delay(200);
range120=0;
range120=range(120);
delay(200);
if(Serial.available()>0)
serialData=Serial.read();
if(serialData=='M')
goto Manual;

if(range60 >OB_range || range120 >OB_range )
{

if(range60 >= range120){
forward_right();Serial.print("R");
delay(50);goto above;}

else if(range60 < range120){
forward_left();Serial.print("L");
delay(50);goto above;}
}

if(range60 <OB_range && range120 <OB_range)
{
above1: range30=0;
range30=range(30);
delay(200);
range150=0;
range150=range(150);
delay(200);
if(Serial.available()>0)
serialData=Serial.read();
if(serialData=='M')
goto Manual;

if(range30 >OB_range || range150 >OB_range )
{

if(range30 >= range150){
right();Serial.print("R");
delay(100);goto above;}

else if(range30 < range150){
left();Serial.print("L");
delay(100);goto above;}
}
if(range30 <OB_range && range150 <OB_range)
{
range0=0;
range0=range(0);
delay(200);
range180=0;
range180=range(180);
delay(200);
if(Serial.available()>0)
serialData=Serial.read();
if(serialData=='M')
goto Manual;

if(range0 >OB_range || range180 >OB_range )
{

if(range0 >= range180){
backward_right();Serial.print("R");
delay(200);goto above;}

else if(range0 < range180){
backward_left();Serial.print("L");
delay(200);goto above;}
}
if(range0 <OB_range && range180 <OB_range)
{
backward();Serial.print("B");
delay(200);
goto above1;
}
}
}
}
}

Manual: brake();
delay(300);

while(1){
if(Serial.available()>0)
serialData=Serial.read();
if(serialData=='A')
goto Automatic;

if(serialData=='F')
forward();
if(serialData=='B')
backward();
if(serialData=='S')
brake();
if(serialData=='L')
left();
if(serialData=='R')
right();
}
}

[anon73444976]

@neno1991

goto Manual;

Uh-oh

Please remember to use code tags when posting code

[anon73444976]

@neno1991

void setup()
{
Serial.begin(9600);
myservo.attach(12);
u8g.setRot180();
}

void draw(int distance) {
u8g.setFont(u8g_font_fur30);
u8g.setPrintPos(0, 63);
u8g.print(distance);
u8g.print("cm");

pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);

Did you miss something here?
Stuff that looks like it should be in setup, isn't

How did you get so much code without testing?

[srnet]

@neno1991

I have 4 ultrasonic sensors hc-sr04, but only one is working.

So when you swap a 'working' sensor and its wiring for a non-working one what happens ?

It may be possible that 3 of the sensors or their wiring are faulty.

[anon73444976]

@neno1991

#define TRIGGER_PIN 11
#define ECHO_PIN 10
...
#define TRIGGER_PIN 9
#define ECHO_PIN 8
...
#define TRIGGER_PIN 2
#define ECHO_PIN 3
...
#define TRIGGER_PIN 4
#define ECHO_PIN 7

Only one working, you say?
Hmmm.

[PerryBebbington]

Your post was MOVED to its current location as it is more suitable.

Could you also take a few moments to Learn How To Use The Forum.

Other general help and troubleshooting advice can be found here.
It will help you get the best out of the forum in the future.

[er_name_not_found]

@neno1991

goto

Goto get a book on structured programming. Anything written since the mid 1960s should do.

No, but in all seriousness did you learn to program in Basic or on a TI calculator?

Every program you will ever need to write can be written using:

• Selective logic - like if and else
• Iterative logic - like for, while, recursive loops, with or without blocks
• Functional logic - like creating functions or object methods

The use of goto was all but forbidden in civilized programming many years ago.

I suggest this be addressed before trying to fix anything else.

[mikedb]

Mplab X ide 5.25

Installed in 2019 still uses GOTO and even still works on my very old code written in Assembler for PIC Micro controllers.

[anon73444976]

@mikedb

even still works on my very old code written in Assembler for PIC Micro controllers.

Yeah, assemblers never really caught up with structured programming.
That's one of the reasons we don't use assembler much these days.

[er_name_not_found]

It's there because it's part of the standard.
It's part of the standard because it's a holdover from B and was used by a lot of PDP-11 programmers.

C++ is vastly better at writing assembly than we are, any of us. Simple fact.

Sure we can tweak and bit twiddle to achieve different tiny aspects, but as for programming at large, fighting the compiler or bringing ASM style to structured programming is foolish at best.

Now that's not a criticism of OP, whom we can help.
But anyone who knows ASM and C++ out to know better and this practice is to be discouraged and coached away, just like the use of delay and other blocking code.

Every single time I've coached someone away from goto their code has become beautifully simplistic and lightening fast in comparison.

To disagree is fine, but prove why one should use it if you're going to promote bad practice.

[anon73444976]

@neno1991

int range(int pos)
{
myservo.write(pos);
delay(300);
current_distance = sonar1.ping_cm();
if(current_distance==0)
current_distance=100;
if(current_distance > 0 && current_distance < 15){
Serial.print("B");
if(pos==90){
backward();delay(500);}
if(pos < 90){
backward_right();delay(500);}
if(pos > 90){
backward_left();delay(500);}
return current_distance;

}}

I can see a path here that doesn't return a value.

IMO, a function called "range" should do return a range value; nothing more, nothing less.

I'd chuck the lot in the bin, and start again.