racer_sourcecode/ultrasonic_navigation1.ino at master · BarnabasRobotics/racer_sourcecode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include <Servo.h>

int trig = 3;
int echo = 4;
int led = 7;

int button = 2;
int mA = 10;
int mB = 11;

Servo motorA;
Servo motorB;

// float so that we can handle decimals
float speedOfSoundMetersPerSec = 343;
float duration_microSeconds;
float duration_seconds;
float distance_meters;
float distance_cm;

void setup() {
  // put your setup code here, to run once:

  pinMode(trig,OUTPUT);
  pinMode(echo,INPUT);
  pinMode(led,OUTPUT);

  pinMode(button,INPUT);

  motorA.attach(mA);
  motorB.attach(mB);

  stop();

  Serial.begin(9600);

}

/*************************/
/***** car movements *****/
/*************************/

void stop() {
  motorA.write(90);
  motorB.write(90);
}

void backward() {
  motorA.write(180);
  motorB.write(0);
}

void forward() {
  motorA.write(0);
  motorB.write(180);
}

void turnRight() {
  motorA.write(90);
  motorB.write(180);
  delay(500);
  stop();
}

void turnLeft() {
  motorA.write(0);
  motorB.write(90);
  delay(500);
  stop();
}


float ultrasonic() {
  // reset the ultrasonic sensor
 digitalWrite(trig,LOW);
 delayMicroseconds(5);

 // send a 10 microsecond pulse out through the trigger
 digitalWrite(trig, HIGH);
 delayMicroseconds(10);
 digitalWrite(trig, LOW);

 // wait for the response and store it in duration.  It will return in microseconds.
 duration_microSeconds = pulseIn(echo,HIGH);

 // convert duration to seconds
 duration_seconds = duration_microSeconds / 1000000;

 // get distance traveled in meters.  distance = (speed * time)/2

 distance_meters = (speedOfSoundMetersPerSec * duration_seconds)/2;

 // convert to cm

 distance_cm = distance_meters*100;
 //Serial.println(distance_cm);

 return distance_cm;

}

void loop() {

 Serial.println(ultrasonic());

 if (ultrasonic() < 5) {
  //do something
  digitalWrite(led,HIGH);
 }
 else {
  //do something else
  digitalWrite(led,LOW);
 }

}