Hello,
I started to build bot with Arduino Uno R3, that uses 2 DC motors to drive around (with L293D IC), uses SHARP IR distance sensor (atached to micro servo) for finding obstacles and micro servo for pointing IR sensor to different directions for finding best way to continue driving.
Here is the code:
#include <DistanceGP2Y0A21YK.h> //distance library
#include <Servo.h> //servo library
const int motor11Pin = 3; // H-bridge pin 1A
const int motor12Pin = 5; // H-bridge pin 2A
const int motor21Pin = 9; // H-bridge pin 3A
const int motor22Pin = 10; // H-bridge pin 4A
const int enablePin1 = 8; // H-bridge enable pin motor1
const int enablePin2 = 12; // H-bridge enable pin motor2
DistanceGP2Y0A21YK Dist; //object Dist for measuring distance
int distance; //distance ahead
int distanceL; //distance left
int distanceD; //distance right
Servo mojservo; //servo object
void setup()
{
Serial.begin(9600); //opens communiaction
pinMode(motor11Pin, OUTPUT); //output for motor1
pinMode(motor12Pin, OUTPUT); //output for motor1
pinMode(motor21Pin, OUTPUT); //output for motor2
pinMode(motor22Pin, OUTPUT); //output for motor2
pinMode(enablePin1, OUTPUT); //output for enable pin1
pinMode(enablePin2, OUTPUT); //output for enable pin2
digitalWrite(enablePin1, HIGH); //activate motor1
digitalWrite(enablePin2, HIGH); //activate motor2
Dist.begin(A0); //begin measuring distance
mojservo.attach(6); //attaches servo on pin6 on Arduino
mojservo.write(90); //turn servo to middle
}
void stop() //stop driving
{
digitalWrite(motor11Pin, LOW);
digitalWrite(motor12Pin, LOW);
digitalWrite(motor21Pin, LOW);
digitalWrite(motor22Pin, LOW);
}
void naprej() //drive forward
{
digitalWrite(motor11Pin, HIGH);
digitalWrite(motor12Pin, LOW);
digitalWrite(motor21Pin, HIGH);
digitalWrite(motor22Pin, LOW);
}
void nazaj() //drive backward
{
digitalWrite(motor11Pin, LOW);
digitalWrite(motor12Pin, HIGH);
digitalWrite(motor21Pin, LOW);
digitalWrite(motor22Pin, HIGH);
}
void desno() //turn right
{
digitalWrite(motor11Pin, HIGH);
digitalWrite(motor12Pin, LOW);
digitalWrite(motor21Pin, LOW);
digitalWrite(motor22Pin, HIGH);
delay(200);
}
void levo() //turn left
{
digitalWrite(motor11Pin, LOW);
digitalWrite(motor12Pin, HIGH);
digitalWrite(motor21Pin, HIGH);
digitalWrite(motor22Pin, LOW);
delay(200);
}
int razdaljaL() //measure left distance
{
mojservo.write(180);
delay(300);
Dist.begin(A0);
distanceL = Dist.getDistanceCentimeter();
delay(300);
return distanceL;
}
int razdaljaD() //measure right distance
{
mojservo.write(0);
delay(300);
Dist.begin(A0);
distanceD = Dist.getDistanceCentimeter();
delay(300);
return distanceD;
}
void loop()
{
mojservo.write(90);
delay(2000);
distance = Dist.getDistanceCentimeter();
if (distance<14) {
stop();
nazaj();
stop();
if (razdaljaL() > razdaljaD()) levo(); else desno();
}
else {
naprej();
}
}
Picture of wiring is in the attachements!
And now my problems...
- it seems that IR sensor isn't reading accurate distance even though is using library,
- bot acts funky in a way that changeing code for PWM signals doesn't effect it's time of spinning wheels nor speed,
- bot stops working when ever he wants to
, or even worse, doesn't start at all when I plug in both batteries, nor via USB and battery.
It's really confusing for me and I'm wondering if I did something wrong with wirring or the code? Help would be appriciated!
THANKS!
