This is the code for a basic obstacle avoiding insect bot. I borrowed some code from a book and the part about the newping library, I did it myself. I want it to go back and turn left/right and move forward on detecting an obstacle. Will the following code do that? I edited the code a bit . Can anyone tell me if it’s better now ?
#include <Servo.h>
#include <NewPing.h>
#define MAX_DIST 200
#define AVOID 20
#define ECHO 11
#define TRIG 12
NewPing sonar(TRIG, ECHO, MAX_DIST);
Servo frontServo;
Servo rearServo;
voidsetup() {
// initialize serial communication:Serial.begin(9600);
frontServo.attach(2);
rearServo.attach(3);
}
int centerPos = 90;
int frontRightUp = 72;
int frontLeftUp = 108;
int backRightForward = 75;
int backLeftForward = 105;
int walkSpeed = 150; // How long to wait between steps in millisecondsint centerTurnPos = 81;
int frontTurnRightUp = 63;
int frontTurnLeftUp = 117;
int backTurnRightForward = 66;
int backTurnLeftForward = 96;
void moveForward()
{
frontServo.write(frontRightUp);
rearServo.write(backLeftForward);
delay(125);
frontServo.write(centerPos);
rearServo.write(centerPos);
delay(65);
frontServo.write(frontLeftUp);
rearServo.write(backRightForward);
delay(125);
frontServo.write(centerPos);
rearServo.write(centerPos);
delay(65);
}
void moveBackRight()
{
frontServo.write(frontRightUp);
rearServo.write(backRightForward-6);
delay(125);
frontServo.write(centerPos);
rearServo.write(centerPos-6);
delay(65);
frontServo.write(frontLeftUp+9);
rearServo.write(backLeftForward-6);
delay(125);
frontServo.write(centerPos);
rearServo.write(centerPos);
delay(65);
}
void moveTurnLeft()
{
frontServo.write(frontTurnRightUp);
rearServo.write(backTurnLeftForward);
delay(125);
frontServo.write(centerTurnPos);
rearServo.write(centerTurnPos);
delay(65);
frontServo.write(frontTurnLeftUp);
rearServo.write(backTurnRightForward);
delay(125);
frontServo.write(centerTurnPos);
rearServo.write(centerTurnPos);
delay(65);
}
voidloop()
{
delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.unsignedint uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).unsignedint cm = (uS / US_ROUNDTRIP_CM);
if(cm < AVOID)
{
for(int i=0; i<=8; i++){
moveBackRight();
delay(150);}
for(int i=0; i<=10; i++)
{
moveTurnLeft();
delay(150);
}
}
else
{
moveForward();
delay(150);
}
}
code review The general structure of the code looks okay-ish. It will measure distance, if something is too nearby, back off and turn, if the way is free, move forward a bit.
I think it needs some fixes, in particular the “cm” variable is defined twice and therefore the cm in loop1 is not the same as the cm in loop (while it should be for your robot to work). Clean up some variables you dont use to make things more clear. Maybe something else, but there’s not that much that can go wrong…
Clearly it is impossible to tell for us whether all the servo constants are ok without knowing what the robot looks like or what servo’s you use (and even if i did know i could probably still not tell ).
Things that you could improve at a later point in time: measure all the time, and not just after every whole turn of the wheels (robot will be stuck in “moveforward” function most of the time and not measure anything meanwhile), average over multiple measurements to deal with noise, …
Before asking questions Before asking questions about the code, it is often better to wait until you really have questions.
From your initial post, I don’t think you’ve tried to compile this and run it on your robot. If I’m wrong, please let me know.
Compiling the code may have caught a few mistakes, and once those were fixed (perhaps with help here) running it on your robot would have given some further questions.
Just asking if the code will work without even trying to compile it seems incomplete somehow.
Magic Numbers I notice that in MoveBackRight() that you have Magic Constants. That is, numbers that just used (-6 and +9) without explanation or better yet turning them into real named constants. They also don’t even have comments to explain them.
We don’t even know what the robot looks like, so guess at some things is difficult. My guess is that this is a small legged robot but I’m not at all sure.
Perhaps you could update your initial post to include a picture of your robot.
One problem of the Arduino’s language is the delay() function, in that it stops the entire processing until the delay is over. This makes it convenient for many things, but since it really means you can only do one thing at a time, you can’t read sensors while you’re in the middle of a delay() and so forth. For example, during the function moveForward() you delay for over 1/4 of a second without checking sensors.
).
?