Greetings, I have changed my code like below and my robot will stop when it see an obstacle , then my servo sweeps 90->150, 150->30, and 30->150 . There is a problem I have now, such as the IR sensor does not see anything when servo is after 130, sensor sees obstacle between 30-130, when there is an obstacle on 90 degree of servo servo starts to move ance again to 150 and 30 and 90. I want my loop to turn once and gives the information which degree has not an obstacle. I hope I was clear.
void loop()
{
unsigned int sensors[5];
int position = qtrrc.readLine(sensors);
int error = position - 2000;
myservo.attach(10);
int pos = 90;
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
if (sensorValue= analogRead(A5) < 500)
{
myservo.attach(10);
set_motors(0, 0);
int pos = 90 ;
for(pos = 90; pos < 150; pos += 1)
{
myservo.write(pos);
delay(15);
}
for(pos = 150; pos>=30; pos-=1)
{
myservo.write(pos);
delay(15);
}
for(pos = 30; pos<=89; pos+=1)
{
myservo.write(pos);
delay(15);
}
}
void set_motors(int motor1speed, int motor2speed)
{
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED; // limit top speed
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED; // limit top speed
if (motor1speed < 0) motor1speed = 0; // keep motor above 0
if (motor2speed < 0) motor2speed = 0; // keep motor speed above 0
motor1.setSpeed(motor1speed); // set motor speed
motor2.setSpeed(motor2speed); // set motor speed
motor1.run(FORWARD);
motor2.run(FORWARD);
}