-
Notifications
You must be signed in to change notification settings - Fork 0
/
22_example_1.ino
279 lines (199 loc) · 7.81 KB
/
22_example_1.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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
#include <Servo.h> //[3104] Servo header File include
/////////////////////////////
// Configurable parameters //
/////////////////////////////
// Arduino pin assignment
#define PIN_LED 9 // [3110] 9번핀 LED 연결
#define PIN_SERVO 10 // [3110] 10번핀 서보 연결
#define PIN_IR A0 //[3104] 적외선 거리센서 PIN - Analog0 정의
// Framework setting
#define _DIST_TARGET 255 //[3104] 탁구공을 위치 시킬 목표
#define _DIST_MIN 100 //[3117] 거리 최소값
#define _DIST_MAX 410 //[3117] 거리 최대값
// Distance sensor
#define _DIST_ALPHA 0.9 //[3099] EMA 필터링을 위한 alpha 값
// Servo range
#define _DUTY_MIN 1890 //[3100] 최저 서보 위치
#define _DUTY_NEU 1590 //[3100] 중립 서보 위치
#define _DUTY_MAX 1350 //[3100] 최대 서보 위치
// Servo speed control
#define _SERVO_ANGLE 30
#define _SERVO_SPEED 30
// Event periods
#define _INTERVAL_DIST 20 //[3099] 각 event 사이에 지정한 시간 간격
#define _INTERVAL_SERVO 20
#define _INTERVAL_SERIAL 100
// PID parameters
#define _KP 0.04
// dist 100, 400mm 일때 값, 각자 a,b로 수정
// define으로 해야 할지, int로 해야할지 고민되네요.. // 큰 상관은 없겠지만, 값이 런타임에서 변화하지 않으니 define쪽에 맞다고 생각합니다 // 넵!
#define a 105
#define b 430
// value 보정값을 위한 setting value 개수
#define SEQ_SIZE 10
//////////////////////
// global variables //
//////////////////////
// Servo instance
Servo myservo;
// Distance sensor
float dist_target; // location to send the ball
float dist_raw, dist_ema, dist_prev; //[1928] 측정된 값과 ema 필터를 적용한 값
// Event periods
unsigned long last_sampling_time_dist, last_sampling_time_servo, last_sampling_time_serial;
//[3104] 각 event의 진행 시간 저장 변수
bool event_dist, event_servo, event_serial;
//[3104] 각 event의 시간체크를 위한 변수 (ex_20초 주기 >> 0초(True,시작), 10초(False), 20초(True))
// Servo speed control
int duty_chg_per_interval; // [3116] 주기 당 서보 duty값 변화량
int duty_target, duty_curr; //[1928] 목표 위치와 현재 위치
// PID variables
float error_curr, error_prev, control, pterm, dterm, iterm;
//error_curr: 현재 측정값과 목표값의 차이
//error_prev: 직전에 구한 차이로, P제어에서는 사용하지 않을 것임
//control: PID제어의 결과로 얻은 제어값
//pterm: Proportional term, 현재 상태의 error값으로부터 얻은 Proportional gain을 저장하는 변수
// [3099]
// value 보정값을 위한 setting value
// sensor values
float x[SEQ_SIZE] = {70.53, 87.3, 120.5, 157.0, 174.4,
211.0, 260.6, 366.0, 446.6, 464.7};
// real values
float y[SEQ_SIZE] = {100.0, 125.0, 150.0, 200.0, 250.0,
275.0, 300.0, 350.0, 400.0, 440.0};
void setup() {
// initialize GPIO pins for LED and attach servo
myservo.attach(PIN_SERVO); // attach servo
pinMode(PIN_LED,OUTPUT); // initialize GPIO pins
// initialize global variables
event_dist = event_servo = event_serial = true;
// move servo to neutral position
myservo.writeMicroseconds(_DUTY_NEU);
duty_curr = _DUTY_NEU;
// initialize serial port
Serial.begin(57600);
// convert angle speed into duty change per interval.
duty_chg_per_interval = (_DUTY_MIN - _DUTY_MAX) * (_SERVO_SPEED / _SERVO_ANGLE) * 1.75 * (_INTERVAL_SERVO / 1000.0);
}
void loop() {
/////////////////////
// Event generator //
/////////////////////
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;
}
////////////////////
// Event handlers //
////////////////////
if(event_dist) {
event_dist = false;
// get a distance reading from the distance sensor
dist_ema = ir_distance_filtered(); // [3099] dist_ema?
// PID control logic
error_curr = dist_ema - _DIST_TARGET;
pterm = error_curr;// * (pow(error_curr,3)/pow(20.5, 3)); //;
// [3099]
iterm = 0;
dterm = 0;
control = _KP * pterm + iterm + dterm;
// duty_target = f(duty_neutral, control)
//duty_target = control;
// target보다 멀리 있음
if (control>0){
duty_target = _DUTY_NEU + (_DUTY_NEU - _DUTY_MAX) * _SERVO_ANGLE / 180.0 * control;
}
// target보다 가까이 있음
else {
duty_target = _DUTY_NEU + (_DUTY_MIN - _DUTY_NEU) * _SERVO_ANGLE / 180.0 * control;
}
//duty_target = ((control>0)?(_DUTY_NEU - _DUTY_MAX)*_SERVO_ANGLE / 180.0:(_DUTY_MIN - _DUTY_NEU) * _SERVO_ANGLE / 180.0) * control;
// [3099] 확실하지 않음, 비례이득의 비대칭 해결가능
// duty_neutral?
// keep duty_target value within the range of [_DUTY_MIN, _DUTY_MAX]
if(duty_target < _DUTY_MAX){
duty_target = _DUTY_MAX;
}
else if(duty_target > _DUTY_MIN){
duty_target = _DUTY_MIN;
}
}
if(event_servo) {
event_servo=false;
// adjust duty_curr toward duty_target by duty_chg_per_interval
if(duty_target>duty_curr) {
duty_curr += duty_chg_per_interval;
if(duty_target < duty_curr){
duty_curr = duty_target;
}
}
else {
duty_curr -= duty_chg_per_interval;
if(duty_target > duty_curr){
duty_curr = duty_target;
}
}
// update servo position
myservo.writeMicroseconds(duty_curr);
}
if(event_serial) {
event_serial = false; //[3117] // 이거 맞나요? // 저도 이렇게 했어요
// 아래 출력문은 수정없이 모두 그대로 사용하기 바랍니다.
Serial.print("dist_ir:");
Serial.print(dist_raw);
//Serial.print(",pterm:");
//Serial.print(map(pterm,-1000,1000,510,610));
Serial.print(",duty_target:");
//Serial.print(duty_target);
Serial.print(map(duty_target,1000,2000,410,510));
Serial.print(",duty_curr:");
//Serial.print(duty_curr);
Serial.print(map(duty_curr,1000,2000,410,510));
Serial.println(",Min:100,Low:200,dist_target:255,High:310,Max:410");
}
}
float ir_distance(void){ // return value unit: mm
float value;
float volt = float(analogRead(PIN_IR));
value = ((6762.0/(volt-9.0))-4.0) * 10.0;
return value;
}
//[3099]
float ir_distance_sequence(void){
float real_value;
int s = 0, e = SEQ_SIZE - 1, m;
float value = ir_distance();
// binary search, value가 어떤 거리 값인지 확인
while(s <= e){
m = (s + e) / 2;
if(value < x[m]){ e = m - 1; }
else if(value > x[m+1]){ s = m + 1; }
else{ break; }
}
// out of sequence range: set value to neutral position
// 센서 전원 Off 시, 오차값 발생으로 구간에서 벗어났을 시 등
if(s > e){
if(value > 2000.0 || value < -2000.0) real_value = _DIST_TARGET;
else if(s == 0) real_value = _DIST_MIN;
else real_value = _DIST_MAX;
}
// calculate real values
real_value = (y[m+1]-y[m])/(x[m+1]-x[m]) * (value-x[m]) + y[m];
return real_value;
}
float ir_distance_filtered(void){ // return value unit: mm
dist_raw = ir_distance_sequence();
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 _DIST_ALPHA * dist_raw + (1 - _DIST_ALPHA) * dist_ema;
}