servo_angle
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
main.cpp
[詳解]
1
#include <Arduino.h>
2
#include <Servo.h>
3
#include <math.h>
4
5
// --- 設定項目(定数) ---
6
const
int
SERVO_PIN
= 9;
// サーボ信号ピン
7
const
int
SERVO_UPDATE_INTERVAL_MS
= 20;
// サーボ目標角度を更新する周期 [ms] (50Hz)
8
const
int
STATE_PUBLISH_FREQ_HZ
= 10;
// LiDARとの角度データ同期周波数 [Hz]
9
const
int
STATE_PUBLISH_INTERVAL_MS
= 1000 /
STATE_PUBLISH_FREQ_HZ
;
// 100ms
10
11
// 動作設定
12
const
int
MIN_ANGLE_DEG
= 70;
// 最小指令角度 [度]。中央90度から-20度
13
const
int
MAX_ANGLE_DEG
= 110;
// 最大指令角度 [度]。中央90度から+20度
14
const
float
SWEEP_TIME_SEC
= 1.0f;
// 片道1秒、往復2秒
15
16
Servo
servo
;
17
21
struct
ServoState
22
{
23
float
angle_deg
;
24
float
velocity_deg_s
;
25
};
26
27
ServoState
calculate_servo_state
(
unsigned
long
timestamp_ms) {
28
// 1往復は2 * SWEEP_TIME_SEC。端点で速度が0になる正弦波にする。
29
const
float
period_sec =
SWEEP_TIME_SEC
* 2.0f;
30
const
float
t = timestamp_ms / 1000.0f;
31
const
float
phase = fmodf(t, period_sec) / period_sec;
32
const
float
center = (
MIN_ANGLE_DEG
+
MAX_ANGLE_DEG
) * 0.5f;
33
const
float
amplitude = (
MAX_ANGLE_DEG
-
MIN_ANGLE_DEG
) * 0.5f;
34
const
float
omega = 2.0f * PI / period_sec;
35
const
float
phase_rad = 2.0f * PI * phase;
36
return
{
37
center - amplitude * cosf(phase_rad),
38
amplitude * omega * sinf(phase_rad)
39
};
40
}
41
45
void
send_data_to_pc
(
unsigned
long
timestamp_ms,
float
angle_deg,
float
velocity_deg_s) {
46
// 例: "TIMESTAMP,ANGLE_DEG,ANGULAR_VELOCITY_DEG_S" 形式
47
Serial.print(timestamp_ms);
48
Serial.print(
","
);
49
Serial.print(angle_deg, 4);
50
Serial.print(
","
);
51
Serial.println(velocity_deg_s, 4);
52
}
53
54
void
setup
() {
55
Serial.begin(115200);
56
servo
.attach(
SERVO_PIN
, 500, 2400);
57
servo
.write(
static_cast<
int
>
(
calculate_servo_state
(millis()).angle_deg + 0.5f));
58
}
59
60
void
loop
() {
61
static
unsigned
long
last_servo_update = 0;
62
static
unsigned
long
last_state_publish = 0;
63
static
ServoState
current_servo_state{
MIN_ANGLE_DEG
, 0.0f};
64
unsigned
long
now = millis();
65
66
// サーボの軌道更新はシリアル通信と独立して50Hzで実行する。
67
if
(now - last_servo_update >=
SERVO_UPDATE_INTERVAL_MS
) {
68
last_servo_update = now;
69
current_servo_state =
calculate_servo_state
(now);
70
servo
.write(
static_cast<
int
>
(current_servo_state.
angle_deg
+ 0.5f));
71
}
72
73
// LiDARの公称10Hzに合わせ、角度サンプルだけを100msごとに送る。
74
if
(now - last_state_publish >=
STATE_PUBLISH_INTERVAL_MS
) {
75
last_state_publish = now;
76
current_servo_state =
calculate_servo_state
(now);
77
send_data_to_pc
(now, current_servo_state.
angle_deg
, current_servo_state.
velocity_deg_s
);
78
}
79
}
MAX_ANGLE_DEG
const int MAX_ANGLE_DEG
定義
main.cpp:13
SWEEP_TIME_SEC
const float SWEEP_TIME_SEC
定義
main.cpp:14
setup
void setup()
定義
main.cpp:54
send_data_to_pc
void send_data_to_pc(unsigned long timestamp_ms, float angle_deg, float velocity_deg_s)
タイムスタンプと角度をシリアル通信でPCへ送信
定義
main.cpp:45
STATE_PUBLISH_FREQ_HZ
const int STATE_PUBLISH_FREQ_HZ
定義
main.cpp:8
servo
Servo servo
定義
main.cpp:16
SERVO_PIN
const int SERVO_PIN
定義
main.cpp:6
STATE_PUBLISH_INTERVAL_MS
const int STATE_PUBLISH_INTERVAL_MS
定義
main.cpp:9
calculate_servo_state
ServoState calculate_servo_state(unsigned long timestamp_ms)
定義
main.cpp:27
SERVO_UPDATE_INTERVAL_MS
const int SERVO_UPDATE_INTERVAL_MS
定義
main.cpp:7
MIN_ANGLE_DEG
const int MIN_ANGLE_DEG
定義
main.cpp:12
loop
void loop()
定義
main.cpp:60
ServoState
指定時刻の正弦波サーボ角度と角速度を計算する
定義
main.cpp:22
ServoState::velocity_deg_s
float velocity_deg_s
定義
main.cpp:24
ServoState::angle_deg
float angle_deg
定義
main.cpp:23
src
main.cpp
構築:
1.18.0