You probably shouldn't be using a while loop as it will take a few seconds for position1 to roll over into the negative for the while condition to be false.
Just have it to where it checks the distance and if it's above 20cm, position1 increments and if not then don't increment it.
Also make it unsigned this way you don't need to deal with roll over and just use the modulos % to keep it between 0 and 360, instead of the IF statement that resets position back to zero.
remove the delay too. Or if you need a delay then do it with a millis timer. Look up the example sketch "Blink without delay" to see how to implement this.
Hi there, as someone that has tried this...it doesn't work. Continuous servos are annoying! You cannot control position, you are better off just using motors.
I'll link my code below:
//Includes libraries necessary for operation
#include <Servo.h> //add Servo Motor library
#include <Ultrasonic.h> //add Ultrasonic sensor library
//Defines Parameters
Ultrasonic ultrasonic(7);
#define MAX_DISTANCE 300 // sets maximum useable sensor measuring distance to 300cm
#define MAX_SPEED 180 // sets speed of DC traction motors to 150/250 or about 70% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 40 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 1// sets distance at which robot stops and reverses to 30cm
#define TURN_DIST COLL_DIST+20 // sets distance at which robot veers away from object
//Gives each servo names
Servo M1;
Servo M2;
Servo M3;
Servo M4;
Servo Sense;
//Defines movement
const int FORWARD = 180;
const int BACKWARD = 0;
const int RELEASE = 90;
//Defines distances for turning
int leftDistance, rightDistance; //distances on either side
int curDist = 0;
String motorSet = "";
int speedSet = 0;
//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
void setup() {
Sense.attach(11);
M1.attach(10); // attaches the servo on pin 10 (SERVO_1 on the Motor Drive Shield to the servo object
M2.attach(9);
M3.attach(6);
M4.attach(5);
ultrasonic = ultrasonic.read(CM);
}
//------------------------------------------------------------------------------------------------------------------------------------
//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
void loop() {
Sense.write(90); // move eyes forward
curDist = digitalRead(7); // read distance
if (curDist < COLL_DIST) {changePath();} // if forward is blocked change direction
moveForward(); // move forward
delay(500);
}
//-------------------------------------------------------------------------------------------------------------------------------------
// defines the movement of the wheels
void changePath() {
moveStop(); // stop forward movement
Sense.write(36); // check distance to the right
delay(500);
rightDistance = readPing(); //set right distance
delay(500);
Sense.write(144); // check distace to the left
delay(700);
leftDistance = readPing(); //set left distance
delay(500);
Sense.write(90); //return to center
delay(100);
compareDistance();
}
void compareDistance() // find the longest distance
{
if (leftDistance>rightDistance) //if left is less obstructed
{
turnLeft();
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
turnRight();
}
else //if they are equally obstructed
{
turnAround();
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
int readPing() { // read the ultrasonic sensor distance
delay(70);
unsigned int cm = ultrasonic.read();
return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {M1.write(RELEASE); M2.write(RELEASE); M3.write(RELEASE); M4.write(RELEASE);} // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
motorSet = "FORWARD";
M1.write(FORWARD); // turn it on going forward
M2.write(FORWARD); // turn it on going forward
M3.write(FORWARD); // turn it on going forward
M4.write(FORWARD); // turn it on going forward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
M1.write(speedSet);
M2.write(speedSet);
M3.write(speedSet);
M4.write(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
motorSet = "BACKWARD";
M1.write(BACKWARD); // turn it on going backward
M2.write(BACKWARD); // turn it on going backward
M3.write(BACKWARD); // turn it on going backward
M4.write(BACKWARD); // turn it on going backward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
M1.write(speedSet);
M2.write(speedSet);
M3.write(speedSet);
M4.write(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
motorSet = "RIGHT";
M1.write(FORWARD); // turn motor 1 forward
M2.write(FORWARD); // turn motor 2 forward
M3.write(BACKWARD); // turn motor 3 backward
M4.write(BACKWARD); // turn motor 4 backward
M3.write(speedSet+MAX_SPEED_OFFSET);
M4.write(speedSet+MAX_SPEED_OFFSET);
delay(200); // write motors this way for 1500
motorSet = "FORWARD";
M1.write(FORWARD); // set both motors back to forward
M2.write(FORWARD);
M3.write(FORWARD);
M4.write(FORWARD);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
motorSet = "LEFT";
M1.write(BACKWARD); // turn motor 1 backward
M2.write(BACKWARD); // turn motor 2 backward
M1.write(speedSet+MAX_SPEED_OFFSET);
M2.write(speedSet+MAX_SPEED_OFFSET);
M3.write(FORWARD); // turn motor 3 forward
M4.write(FORWARD); // turn motor 4 forward
delay(2000); // write motors this way for 1500
motorSet = "FORWARD";
M1.write(FORWARD); // turn it on going forward
M2.write(FORWARD); // turn it on going forward
M3.write(FORWARD); // turn it on going forward
M4.write(FORWARD); // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
motorSet = "RIGHT";
M1.write(FORWARD); // turn motor 1 forward
M2.write(FORWARD); // turn motor 2 forward
M3.write(BACKWARD); // turn motor 3 backward
M4.write(BACKWARD); // turn motor 4 backward
M3.write(speedSet+MAX_SPEED_OFFSET);
M4.write(speedSet+MAX_SPEED_OFFSET);
delay(1700); // write motors this way for 1700
motorSet = "FORWARD";
M1.write(FORWARD); // set both motors back to forward
M2.write(FORWARD);
M3.write(FORWARD);
M4.write(FORWARD);
}