Hello, I am new to coding on the Arduino.
My project is to have a type of autonomous miniature car, but I am having a problem with my code.
In the Scan() function an array is created called distances[]. The problem is that I want to append a variable called 'range' which is the output of the function find_range(), yet it seems to append mostly zeroes to the array when it should not be. Also, when I print 'length_distances' to the serial monitor it prints a zero. Any help or guidance as to what the problem is would be appreciated. Here is my code:
//Test with Scan
#include <AFMotor.h>
//setting pin numbers
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
const int trigpin_R = 53;
const int echopin_R = 51;
const int trigpin_L = 22;
const int echopin_L = 24;
//setting up other variables
int range;
long duration_R;
int distance_R;
long duration_L;
int distance_L;
int i;
int largest;
int index_largest;
int n;
//defining threshold distance in cm
const int t_d=20;
void setup() {
Serial.begin(9600); // set up Serial library at 9600 bps
//set up sensor input, output
pinMode(trigpin_R, OUTPUT);
pinMode(echopin_R, INPUT);
pinMode(trigpin_L, OUTPUT);
pinMode(echopin_L, INPUT);
Serial.println("Start");
// turn on motors and set the speed of motors
motor1.setSpeed(155);
motor2.setSpeed(155);
motor3.setSpeed(155);
motor4.setSpeed(155);
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
//defining the function for getting the range to nearest obstacle in cm
int get_range() {
digitalWrite(trigpin_R, LOW);
delayMicroseconds(2);
digitalWrite(trigpin_R, HIGH);
delayMicroseconds(5);
digitalWrite(trigpin_R, LOW);
duration_R = pulseIn(echopin_R, HIGH);
digitalWrite(trigpin_L, LOW);
delayMicroseconds(2);
digitalWrite(trigpin_L, HIGH);
delayMicroseconds(5);
digitalWrite(trigpin_L, LOW);
duration_L = pulseIn(echopin_L, HIGH);
distance_R = duration_R*0.034/2;
//Serial.println(distance_R);
//Serial.println("distance_R");
distance_L = duration_L*0.034/2;
//Serial.println(distance_L);
//Serial.println("distance_L");
int distance=distance_L;
if (distance_R<distance_L){
distance=distance_R;
}
return distance;
}
int scan() {
Serial.println("scan");
int time_turning=0;
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
delay(1500);
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
int start_time=millis();
int distances[]={};
n=0;
while (time_turning<3010){
range=get_range();
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
delayMicroseconds(9986);
time_turning=millis()-start_time;
distances[n]=range;
n++;
Serial.println(distances[n]);
}
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
largest=distances[0];
int length_distances = sizeof(distances)/sizeof(distances[0]);
int largest_interval=0;
int largest_interval_midpoint=0;
for (int index=0; index<length_distances; index++){
if (distances[index]>largest){
largest=distances[index];
index_largest=index;
for (int i=index_largest+1;i<length_distances; i++){
if ((distances[i]>=distances[i-1]) and (distances[i]>20)){
continue;
}
else{
largest_interval=i-index_largest-1;
largest_interval_midpoint=index_largest+(floor(largest_interval/2));
index+=largest_interval;
break;
}
}
}
}
for(int i = 250; i < length_distances; i++)
{
Serial.println(distances[i]);
}
Serial.println(length_distances);
Serial.println("index_midpoint");
Serial.println(largest_interval_midpoint);
//Serial.println(2000-index_largest*1);
return (3000-(largest_interval_midpoint*10));
}
void loop() {
range=get_range();
while ((range)>=(t_d)){
//Serial.print("forward");
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
range=get_range();
}
Serial.println(range);
if ((range)<(t_d)){
//Serial.println("Scan");
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
int time_to_turn=scan();
if (time_to_turn>0){
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
delay(time_to_turn);
}
range=get_range();
if (range<t_d);{
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
delay(1500);
range=get_range();
}
}
}