Guys Im trying to create an obstacle avoiding robot which uses 2 servos and a Panasonic PNA4602M IR Detector. Right now in my code I have a function which returns a 0 if it detects an object and a 1 if it does not detect anything. Based on those results I am telling the servos to go straight or make a turn. But I am not able to get the robot to make a turn. It just keeps going straight even with an obstacle in the way. Here is my code:
int servoPin6 = 6; // Control pin for servo motor on Digital Pin 6
int servoPin7 = 7; // Control pin for servo motor on Digital Pin 7
int pulse6 = 1019; // Amount to pulse the servo on Pin 6
int pulse7 = 1607; // Amount to pulse the servo on Pin 7
int irLedLeftPin = 10; // IR Left Led on this pin
int irLedRightPin = 9; // IR Right Led on this pin
int irSensorPin = 8; // IR sensor on this pin
int func_return;
int irRead(int readPin, int triggerPin); //function prototype
void setup()
{
pinMode(servoPin6, OUTPUT); // Set servo pin as an output pin
pinMode(servoPin7, OUTPUT); // Set servo pin as an output pin
pinMode(irSensorPin, INPUT);
pinMode(irLedLeftPin, OUTPUT);
}
void loop()
{
func_return = irRead(irSensorPin, irLedLeftPin);
if (func_return = 1)
{
Pulse(servoPin6, pulse6); //Turn right wheel
Pulse(servoPin7, pulse7); //Turn left wheel
func_return = irRead(irSensorPin, irLedLeftPin);
}
else if (func_return = 0)
{
Pulse(servoPin6, pulse6); //Turn right wheel
func_return = irRead(irSensorPin, irLedLeftPin);
}
}
/******************************************************************************
* This function can be used with a panasonic pna4602m ir sensor
* it returns a zero if something is detected by the sensor, and a 1 otherwise
* The function bit bangs a 38.5khZ waveform to an IR led connected to the
* triggerPin for 1 millisecond, and then reads the IR sensor pin to see if
* the reflected IR has been detected
******************************************************************************/
int irRead(int readPin, int triggerPin)
{
int halfPeriod = 13; //one period at 38.5khZ is aproximately 26 microseconds
int cycles = 38; //26 microseconds * 38 is more or less 1 millisecond
int i;
for (i=0; i <=cycles; i++)
{
digitalWrite(triggerPin, HIGH);
delayMicroseconds(halfPeriod);
digitalWrite(triggerPin, LOW);
delayMicroseconds(halfPeriod - 1); // - 1 to make up for digitaWrite overhead
}
return digitalRead(readPin);
}
/******************************************************************************
* This function is used to pulse the two servos
******************************************************************************/
void Pulse(int servoPin, int servoPulse)
{
digitalWrite(servoPin, HIGH); // Turn the motor on
delayMicroseconds(servoPulse); // Length of the pulse sets the motor position
digitalWrite(servoPin, LOW); // Turn the motor off
delayMicroseconds(19000);
}
Any help is greatly appreciated.