servo_angle
読み取り中…
検索中…
一致する文字列を見つけられません
main.cpp
[詳解]
1#include <Arduino.h>
2#include <Servo.h>
3#include <math.h>
4
5// --- 設定項目(定数) ---
6const int SERVO_PIN = 9; // サーボ信号ピン
7const int SERVO_UPDATE_INTERVAL_MS = 20; // サーボ目標角度を更新する周期 [ms] (50Hz)
8const int STATE_PUBLISH_FREQ_HZ = 10; // LiDARとの角度データ同期周波数 [Hz]
10
11// 動作設定
12const int MIN_ANGLE_DEG = 70; // 最小指令角度 [度]。中央90度から-20度
13const int MAX_ANGLE_DEG = 110; // 最大指令角度 [度]。中央90度から+20度
14const float SWEEP_TIME_SEC = 1.0f; // 片道1秒、往復2秒
15
16Servo servo;
17
22{
23 float angle_deg;
25};
26
27ServoState 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
45void 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
54void 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
60void 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}
const int MAX_ANGLE_DEG
const float SWEEP_TIME_SEC
void setup()
void send_data_to_pc(unsigned long timestamp_ms, float angle_deg, float velocity_deg_s)
タイムスタンプと角度をシリアル通信でPCへ送信
const int STATE_PUBLISH_FREQ_HZ
定義 main.cpp:8
Servo servo
const int SERVO_PIN
定義 main.cpp:6
const int STATE_PUBLISH_INTERVAL_MS
定義 main.cpp:9
ServoState calculate_servo_state(unsigned long timestamp_ms)
const int SERVO_UPDATE_INTERVAL_MS
定義 main.cpp:7
const int MIN_ANGLE_DEG
void loop()
指定時刻の正弦波サーボ角度と角速度を計算する
float velocity_deg_s
float angle_deg