-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathusound_navigational_support__tanh_loop.ino
132 lines (106 loc) · 3.31 KB
/
usound_navigational_support__tanh_loop.ino
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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#include <math.h>
unsigned int flag_FAR = 0;
unsigned int flag_NEAR = 0;
// two motors
const int echo = 6;
const int trig = 5;
// single motor
//const int echo = 9;
//const int trig = 6;
const int motor_L = 11;
const int motor_R = 3;
long duration, distance;
int dutyCycle = 0;
float alpha = 0.5; // exponential smoothing constant
float average_cm = 0; // initialization @ generic distance
// Anything over 400 cm (23200 us pulse) is "out of range"
const unsigned int MAX_DIST = 23200;
unsigned long previousMillis = 0;
const long interval = 100;
unsigned int counts = 0;
signed int beat_out = 250;
void setup() {
//TCCR2B=(TCCR2B&0xF8) | 2;
pinMode(motor_L, OUTPUT);
digitalWrite(motor_L, LOW);
pinMode(motor_R, OUTPUT);
digitalWrite(motor_R, LOW);
pinMode(echo,INPUT);
pinMode(trig,OUTPUT);
digitalWrite(trig, LOW);
Serial.begin(9600);
}
void loop() {
unsigned long currentMillis = millis();
unsigned long t1;
unsigned long t2;
unsigned long pulse_width;
float cm;
if (currentMillis-previousMillis >= interval) {
counts += 1;
// Hold the trigger pin high for at least 10 us
digitalWrite(trig,HIGH);
delayMicroseconds(10);
digitalWrite(trig,LOW);
// Wait for pulse on echo pin
while (digitalRead(echo) == 0);
// Measure how long the echo pin was held high (pulse width)
// Note: the micros() counter will overflow after ~70 min
t1 = micros();
while (digitalRead(echo) == 1);
t2 = micros();
pulse_width = t2-t1;
if ( pulse_width > MAX_DIST ) {
cm = 400;
} else {
cm = pulse_width/58.0;
}
average_cm = alpha*cm+(1-alpha)*average_cm; //exponential smoothing
if(average_cm <= 200 && average_cm > 35){
//dutyCycle=round(255-1.7*average_cm);
dutyCycle=round(127+127*((exp(-(average_cm-70)/35)-exp((average_cm-70)/35))/(exp(-(average_cm-70)/35)+exp((average_cm-70)/35))));
//dutyCycle=round(296-1.48*average_cm);
dutyCycle = constrain(dutyCycle,0,255);
analogWrite(motor_L,dutyCycle);
analogWrite(motor_R,dutyCycle);
} else if (average_cm <= 35 && (counts % 2) == 0) {
beat_out = beat_out*(-1);
analogWrite(motor_L,constrain(beat_out,0,250));
analogWrite(motor_R,constrain(beat_out,0,250));
} else if (average_cm > 150 && (counts % 5) == 0) {
beat_out = beat_out*(-1);
analogWrite(motor_L,constrain(beat_out,0,250));
analogWrite(motor_R,constrain(beat_out,0,250));
} else if (average_cm > 250 && (counts % 9) == 0) {
//beat_out = beat_out*(-1);
analogWrite(motor_L,250);
analogWrite(motor_R,250);
delay(20);
analogWrite(motor_L,0);
analogWrite(motor_R,0);
delay(20);
analogWrite(motor_L,250);
analogWrite(motor_R,250);
delay(20);
analogWrite(motor_L,0);
analogWrite(motor_R,0);
delay(20);
analogWrite(motor_L,250);
analogWrite(motor_R,250);
delay(20);
analogWrite(motor_L,0);
analogWrite(motor_R,0);
} else {
dutyCycle = 0;
analogWrite(motor_L,dutyCycle);
analogWrite(motor_R,dutyCycle);
}
Serial.print(cm);
Serial.print(" cm \t");
Serial.print(average_cm);
Serial.print(" avg \t");
Serial.print(dutyCycle);
Serial.println(" dCycle");
previousMillis = currentMillis;
}
}