So the code is copied below but im unsure how to keep the sensor checking for objects once it has done one loop of the MoveRobot function. Once this function is complete, the serial monitor/sensor is locked to a distance of 24 and I have to re-upload the code for it to work again.
I have tried a while and a for loop to no avail, any help would be greatly appreciated
// defines pins numbers
const int trigPin = 9;
const int echoPin = 10;
const int buzzer = 11;
const int ledPin = 13;
// defines variables
long duration;
int distance;
int safetyDistance;
#include <BraccioRobot.h>
#include <Servo.h>
#include <Braccio.h>
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_rot;
Servo wrist_ver;
Servo gripper;
void setup() {
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
pinMode(buzzer, OUTPUT);
pinMode(ledPin, OUTPUT);
Serial.begin(9600); // Starts the serial communication
}
void loop() {
// Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance= duration*0.034/2;
safetyDistance = distance;
if (safetyDistance <= 5){
MoveRobot();
digitalWrite(buzzer, HIGH);
digitalWrite(ledPin, HIGH);
return(loop());
}
else{
digitalWrite(buzzer, LOW);
digitalWrite(ledPin, LOW);
}
// Prints the distance on the Serial Monitor
Serial.print("Distance: ");
Serial.println(distance);
}
Position pos, down, up, pos2;
void MoveRobot() {
// Calculates the distance based on the duration of the echo pulse
distance = duration * 0.034 / 2;
safetyDistance = distance;
if (safetyDistance <= 5) {
// (M1, M2, M3, M4, M5, M6)
BraccioRobot.init();
pos.set( 90, 90, 45, 45, 90, 20);
down.set( 90, 90, 22, 22, 90, 70);
up.set(90, 90, 90, 90, 90, 73);
pos2.set(90, 90, 10, 10, 90, 20);
// Move the robot to the position with a specified speed between 20-200 degrees per second
BraccioRobot.moveToPosition(pos, 25);
//Wait 5 seconds
delay(5000);
BraccioRobot.moveToPosition(down.set(90, 90, 10, 10, 90, 20), 50);
delay(5000);
BraccioRobot.moveToPosition(pos.set(90, 90, 10, 10, 90, 70), 50);
//Wait 2 seconds
delay(2000);
BraccioRobot.moveToPosition(up, 50);
delay(2000);
BraccioRobot.moveToPosition(pos.set(90, 90, 10, 10, 90, 70), 25);
BraccioRobot.moveToPosition(pos2, 50);
digitalWrite(ledPin, HIGH);
}
else {
BraccioRobot.moveToPosition(up, 25);
digitalWrite(ledPin, LOW);
}
}