Obstacle avoiding Robot Platform
This is a code for Obstacle Avoiding Robot Platform
#include <AFMotor.h>
#define trigPin 3
#define echoPin 2
AF_DCMotor motor1(1, MOTOR12_64KHZ);
AF_DCMotor motor2(4, MOTOR12_64KHZ);
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
motor1.setSpeed(120);
motor2.setSpeed(130);
}
int CheckDistance()
{
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(2);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
return distance;
}
void MotorForward(int delaytime)
{
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(delaytime);
}
void MotorBackward(int delaytime)
{
motor1.run(BACKWARD);
motor2.run(BACKWARD);
delay(delaytime);
}
void MotorRelease()
{
motor1.run(RELEASE);
motor2.run(RELEASE);
delay(100);
}
void MotorLeft()
{
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(400);
}
void MotorRight()
{
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(400);
}
void loop()
{
int testDistance = CheckDistance();
Serial.print(testDistance);
Serial.println(" test");
if (testDistance >= 30 || testDistance <= 0){
Serial.println("Out of range");
MotorForward(300);
MotorRelease();
}
else
{
Serial.print(testDistance);
Serial.println(" cm");
MotorBackward(100);
MotorRelease();
MotorRight();
MotorRelease();
}
}
No comments:
Post a Comment