Commit 4ae3d4fb by satsob

1. ylesanne

parents
Showing with 132 additions and 0 deletions
#include <Servo.h>
// Defines
#define right_servo_pin 5 // Define Servo right pin
#define left_servo_pin 6 // Define Servo left pin
// Global variables
Servo g_left_wheel;
Servo g_right_wheel;
// defines ultrasonic sensor pins numbers
const int trigPin = 4; // D4 - Sonic trig
const int echoPin = 3; // D3 - Sonic echo
// defines variables for ultrasonic sensor
long duration;
int distance;
int getSonicSensor_Distance() { // Get the distance from the Ultrasonic Sensor
// Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance= duration*0.034/2;
return distance;
}
// Stop the robot
void setWheels_Stop() {
g_right_wheel.write(1500);
g_left_wheel.write(1500);
}
// Drive forward
void setWheels_Forward(){
g_right_wheel.write(1300);
g_left_wheel.write(1700);
}
// drive right
void setWheels_Right(){
g_right_wheel.write(1700);
g_left_wheel.write(1700);
}
// Drive left
void setWheels_Left(){
g_right_wheel.write(1300);
g_left_wheel.write(1300);
}
// Drive backward
/*void setWheels_Backward(){
g_right_wheel.write(1700);
g_left_wheel.write(1300);
}*/
void setup() {
/* Start serial monitor */
Serial.begin(9600);
/* Attach servos to digital pins defined earlier */
g_left_wheel.attach(left_servo_pin);
g_right_wheel.attach(right_servo_pin);
/* Put servos to standstill */
//setWheels();
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
}
void loop() {
// Gets and Prints the distance on the Serial Monitor
distance = getSonicSensor_Distance();
Serial.print("Distance: ");
Serial.println(distance);
if (distance < 30){
Serial.println("Stop");
setWheels_Stop();
delay(500);
/*for(i=0; i++; i<10){
setWheels_Right();
delay(100);
}
if(distance < 30){
for(i=0; i++; i<20){
setWheels_Left();
delay(100);
}
}*/
int i = 0;
while (distance < 30){
distance = getSonicSensor_Distance();
setWheels_Right();
delay(100);
i++;
if (i >= 10){
break;
}
}
while (distance < 30){
distance = getSonicSensor_Distance();
Serial.println(distance);
setWheels_Left();
delay(100);
}
}
else if (distance > 30){
Serial.println("Forward");
setWheels_Forward();
delay(100);
}
// Read and write to the console
/*Serial.println("Servo right wheel:");
g_right_wheel.write();
Serial.println("Servo left wheel:");
g_left_wheel.write();*/
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or sign in to comment