-
Notifications
You must be signed in to change notification settings - Fork 0
/
21_bbc_20203108.ino
125 lines (96 loc) · 3.08 KB
/
21_bbc_20203108.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
#include <Servo.h>
// Arduino pin assignment
#define PIN_SERVO 10
#define PIN_IR A0
#define PIN_LED 9
#define duty 1550
#define duty_purse 200
#define _DUTY_MAX duty-duty_purse
#define _DUTY_MIN duty+duty_purse
#define _SERVO_SPEED 60
#define _INTERVAL_DIST 20
#define _INTERVAL_SERVO 20
#define _INTERVAL_SERIAL 100
#define _DIST_MIN 100
#define _DIST_MAX 400
#define _DIST_ALPHA 0.4
#define a 60 // when value 100
#define b 440 // when value 400
#define middle 216 // after handling when value 250
Servo myservo;
float dist_min, dist_max, dist_raw, dist_cali, dist_ema, dist_prev, alpha;
unsigned long last_sampling_time_dist, last_sampling_time_servo, last_sampling_time_serial;
bool event_dist, event_servo, event_serial;
int duty_chg_per_interval;
int toggle_interval, toggle_interval_cnt;
void setup() {
// initialize servo
myservo.attach(PIN_SERVO);
myservo.writeMicroseconds(duty);
// initialize GPIO pins
pinMode(PIN_LED,OUTPUT);
digitalWrite(PIN_LED, 1);
dist_min = _DIST_MIN;
dist_max = _DIST_MAX;
alpha = _DIST_ALPHA;
dist_raw = dist_prev = dist_cali = dist_ema = 0.0;
event_dist = event_servo = event_serial = true;
// initialize serial port
Serial.begin(57600);
// convert angle speed into duty change per interval.
duty_chg_per_interval = (float)(_DUTY_MIN-_DUTY_MAX)*(_SERVO_SPEED / 180)*(_INTERVAL_SERVO / 1000);
}
void loop() {
unsigned long time_curr = millis();
if(time_curr >= last_sampling_time_dist + _INTERVAL_DIST){
last_sampling_time_dist += _INTERVAL_DIST;
event_dist = true;
}
if(time_curr >= last_sampling_time_servo + _INTERVAL_SERVO){
last_sampling_time_servo += _INTERVAL_SERVO;
event_servo = true;
}
if(time_curr >= last_sampling_time_serial + _INTERVAL_SERIAL){
last_sampling_time_serial += _INTERVAL_SERIAL;
event_serial = true;
}
if (event_dist){
event_dist = false;
dist_cali = ir_distance();
dist_ema = ir_distance_filtered();
}
// output the read value to th eserial port
if (event_serial){
event_serial = false;
Serial.print("min:0,max:500,dist_cali:");
Serial.print(dist_cali);
Serial.print(",dist_ema:");
Serial.println(dist_ema);
}
// Servo
if(event_servo){
event_servo = false;
if (dist_ema > middle){
myservo.writeMicroseconds(duty+duty_purse);
}
else{
myservo.writeMicroseconds(duty-duty_purse);
}
}
// LED Set : OFF
if( middle-30 < dist_ema && dist_ema < middle + 30) digitalWrite(PIN_LED, 0);
else digitalWrite(PIN_LED, 255);
}
float ir_distance(void){ // return value unit: mm
float val;
float volt = float(analogRead(PIN_IR));
val = ((6762.0/(volt-9.0))-4.0) * 10.0;
return 100 + 300.0 / (b-a) * (val-a);
}
float ir_distance_filtered(void){ // return value unit: mm
dist_raw = ir_distance();
if (dist_raw > dist_max) dist_raw = dist_max;
else if (dist_raw < dist_min) dist_raw = dist_prev;
else dist_prev = dist_raw;
return alpha*dist_raw + (1 - alpha)*dist_ema;
}