Skip to content

Commit 89b286f

Browse files
author
Shota Aoki
authored
リファクタリング (#8)
* Update .gitignore for arduino-esp32 * arduino-esp32のデバッグファイルをSTEP8から削除 * while無限ループを中括弧continue表記に変更 * ヘッダーファイルにインクルードガードを追加 * STEP2 グローバル変数state_*をg_state_*に変更 * STEP13 インクルードガード修正 * STEP5 グローバル変数に接頭辞g_を追加 変数名の末尾にr, lを付ける * STEP4、STEP5グローバル変数に接頭辞g_を追加 変数名の末尾にr, lを付ける * STEP6 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * STEP7 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * STEP8 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * アンダースコアから始まる変数を修正 * STEP10 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * STEP11 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * STEP12 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * STEP13 グローバル変数に接頭辞g_を追加、変数末尾にr, lをつける * STEP13 アンダースコアから始まる変数を修正 * unused-variableをCIのエラー対象にする * 未使用の変数を削除 * STEP13 未使用の変数を削除。変更漏れ * CI return-typeをエラーに含む * STEP8 getNextDir関数のデフォルト返り値にfrontを設定する * STEP13 getNextDir関数のデフォルト返り値にfrontを設定する * STEP8 未使用の変数を削除。変更漏れの対応 * switch文にdefault caseを追加。 CIでswitchをエラーとする * 配列を[]でアクセスするときcharではなくunsigned char変数を使用する。 CIで-Wchar-subscriptをエラーとして扱う * eulerToQuatに不要なconstがついていたので削除。CIで-Wignore-qualiferをエラーとして扱う * CIから不要なエラー無視フラグを削除 * STEP3 switch文にdefaultを追加 * STEP4 switch文にdefaultを追加 * STEP7 switch文にdefaultを追加 * STEP12 switch文にdefaultを追加 * STEP8と13 switch文にdefaultを追加 * STEP8 switch文にdefaultを追加。変更漏れ対応
1 parent b229467 commit 89b286f

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

52 files changed

+1606
-69060
lines changed

.github/workflows/compile-sketches.yaml

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -33,15 +33,7 @@ jobs:
3333
cli-compile-flags: | # 警告がエラーとして扱われるので、対処しない警告はエラーから除外する
3434
- --build-property
3535
- "compiler.cpp.extra_flags= \
36-
-Wno-error=unused-but-set-parameter \
37-
-Wno-error=unused-but-set-variable \
38-
-Wno-error=unused-variable \
39-
-Wno-error=switch \
40-
-Wno-error=char-subscripts \
41-
-Wno-error=return-type \
42-
-Wno-error=type-limits \
43-
-Wno-error=ignored-qualifiers \
44-
-Wno-error=implicit-function-declaration"
36+
-Wno-error=type-limits" # micro_ros_arduino側のエラーを防ぐ
4537
sketch-paths: |
4638
- uROS_STEP1_LED
4739
- uROS_STEP2_SWITCH

.gitignore

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,3 +70,47 @@ avr-toolchain-*.zip
7070
manifest.mf
7171
nbbuild.xml
7272
nbproject
73+
74+
# Ref: https://github.com/espressif/arduino-esp32/blob/master/.gitignore
75+
tools/xtensa-esp32-elf
76+
tools/xtensa-esp32s2-elf
77+
tools/xtensa-esp32s3-elf
78+
tools/riscv32-esp-elf
79+
tools/dist
80+
tools/esptool
81+
tools/esptool.exe
82+
tools/mkspiffs
83+
tools/mklittlefs
84+
tools/mkfatfs.exe
85+
tools/openocd-esp32
86+
87+
# Ignore editor backup files and macOS system metadata
88+
.DS_Store
89+
.*.swp
90+
.*.swo
91+
*~
92+
93+
# Ignore build folder
94+
/build
95+
96+
# Ignore files built by Visual Studio/Visual Micro
97+
[Dd]ebug/
98+
[Rr]elease/
99+
.vs/
100+
__vm/
101+
*.vcxproj*
102+
.vscode/
103+
platform.sloeber.txt
104+
boards.sloeber.txt
105+
106+
# Ignore docs build (Sphinx)
107+
docs/build
108+
docs/source/_build
109+
__pycache__/
110+
_build/
111+
112+
# Test log files
113+
*.log
114+
debug.cfg
115+
debug.svd
116+
debug_custom.json

README.md

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,19 @@
33
[![Compile Sketches](https://github.com/rt-net/pico_micro_ros_arduino_examples/actions/workflows/compile-sketches.yaml/badge.svg)](https://github.com/rt-net/pico_micro_ros_arduino_examples/actions/workflows/compile-sketches.yaml)
44
[![Lint](https://github.com/rt-net/pico_micro_ros_arduino_examples/actions/workflows/lint.yaml/badge.svg)](https://github.com/rt-net/pico_micro_ros_arduino_examples/actions/workflows/lint.yaml)
55

6+
Pi:Co Classic 3用のmicro-ROS Arduino サンプル集です
7+
68
## スケッチファイルの自動整形について
79

810
ソースコードのレイアウトを整えるため、各スケッチファイルにはArduino IDEの自動整形を適用しています。
911
自動整形のルールは[.clang-format](.clang-format) ファイルを参照してください。
12+
13+
## License
14+
15+
(C) 2023 RT Corporation
16+
17+
各ファイルはライセンスがファイル中に明記されている場合、そのライセンスに従います。特に明記されていない場合は、Apache License, Version 2.0に基づき公開されています。
18+
ライセンスの全文は[LICENSE](./LICENSE)または[https://www.apache.org/licenses/LICENSE-2.0](https://www.apache.org/licenses/LICENSE-2.0)から確認できます。
19+
20+
※このソフトウェアは基本的にオープンソースソフトウェアとして「AS IS」(現状有姿のまま)で提供しています。本ソフトウェアに関する無償サポートはありません。
21+
バグの修正や誤字脱字の修正に関するリクエストは常に受け付けていますが、それ以外の機能追加等のリクエストについては社内のガイドラインを優先します。

uROS_STEP10_twistMsg/interrupt.ino

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,14 +16,14 @@ void controlInterrupt(void)
1616
{
1717
double speed_r, speed_l;
1818

19-
if ((speed < 0.001) && (speed > -0.001) && (omega < 0.001) && (omega > -0.001)) {
20-
motor_move = 0;
19+
if ((g_speed < 0.001) && (g_speed > -0.001) && (g_omega < 0.001) && (g_omega > -0.001)) {
20+
g_motor_move = 0;
2121
} else {
22-
motor_move = 1;
22+
g_motor_move = 1;
2323
}
2424

25-
speed_r = speed + omega * TREAD_WIDTH / 2.0;
26-
speed_l = speed - omega * TREAD_WIDTH / 2.0;
25+
speed_r = g_speed + g_omega * TREAD_WIDTH / 2.0;
26+
speed_l = g_speed - g_omega * TREAD_WIDTH / 2.0;
2727

2828
if (speed_r > 0) {
2929
digitalWrite(CW_R, LOW);
@@ -36,7 +36,7 @@ void controlInterrupt(void)
3636
} else if (speed_r < MIN_SPEED) {
3737
speed_r = MIN_SPEED;
3838
}
39-
r_step_hz = (signed short)(speed_r / PULSE);
39+
g_step_hz_r = (signed short)(speed_r / PULSE);
4040

4141
if (speed_l > 0) {
4242
digitalWrite(CW_L, LOW);
@@ -45,5 +45,5 @@ void controlInterrupt(void)
4545
speed_l *= -1.0;
4646
}
4747
if (speed_l < MIN_SPEED) speed_l = MIN_SPEED;
48-
l_step_hz = (signed short)(speed_l / PULSE);
48+
g_step_hz_l = (signed short)(speed_l / PULSE);
4949
}

uROS_STEP10_twistMsg/uROS_STEP10_twistMsg.ino

Lines changed: 61 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -23,13 +23,13 @@
2323
#include <stdio.h>
2424
// clang-format on
2525

26-
geometry_msgs__msg__Twist msg;
26+
geometry_msgs__msg__Twist g_msg;
2727

28-
rcl_subscription_t subscriber;
29-
rclc_executor_t executor;
30-
rcl_allocator_t allocator;
31-
rclc_support_t support;
32-
rcl_node_t node;
28+
rcl_subscription_t g_subscriber;
29+
rclc_executor_t g_executor;
30+
rcl_allocator_t g_allocator;
31+
rclc_support_t g_support;
32+
rcl_node_t g_node;
3333

3434
#define LED0 1
3535
#define LED1 2
@@ -48,28 +48,28 @@ rcl_node_t node;
4848
#define MIN_SPEED (MIN_HZ * PULSE)
4949
#define TREAD_WIDTH (65.0)
5050

51-
hw_timer_t * timer0 = NULL;
52-
hw_timer_t * timer2 = NULL;
53-
hw_timer_t * timer3 = NULL;
51+
hw_timer_t * g_timer0 = NULL;
52+
hw_timer_t * g_timer2 = NULL;
53+
hw_timer_t * g_timer3 = NULL;
5454

55-
portMUX_TYPE timer_mux = portMUX_INITIALIZER_UNLOCKED;
55+
portMUX_TYPE g_timer_mux = portMUX_INITIALIZER_UNLOCKED;
5656

57-
unsigned short r_step_hz = MIN_HZ;
58-
unsigned short l_step_hz = MIN_HZ;
57+
unsigned short g_step_hz_r = MIN_HZ;
58+
unsigned short g_step_hz_l = MIN_HZ;
5959

60-
volatile unsigned int step_r, step_l;
60+
volatile unsigned int g_step_r, g_step_l;
6161

62-
volatile double max_speed = MIN_SPEED;
63-
volatile double min_speed = MIN_SPEED;
64-
volatile double r_accel = 0.0;
65-
volatile double speed = 0.0;
62+
volatile double g_max_speed = MIN_SPEED;
63+
volatile double g_min_speed = MIN_SPEED;
64+
volatile double g_accel = 0.0;
65+
volatile double g_speed = 0.0;
6666

67-
double max_omega;
68-
double min_omega;
69-
double r_acc_omega = 0.0;
70-
double omega = 0;
67+
double g_max_omega;
68+
double g_min_omega;
69+
double g_acc_omega = 0.0;
70+
double g_omega = 0;
7171

72-
volatile bool motor_move = 0;
72+
volatile bool g_motor_move = 0;
7373

7474
#define RCCHECK(fn) \
7575
{ \
@@ -98,52 +98,52 @@ void errorLoop()
9898
//目標値の更新周期1kHz
9999
void IRAM_ATTR onTimer0(void)
100100
{
101-
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
101+
portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止
102102
controlInterrupt();
103-
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
103+
portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可
104104
}
105105

106106
//Rモータの周期数割り込み
107107
void IRAM_ATTR isrR(void)
108108
{
109-
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
110-
if (motor_move) {
111-
if (r_step_hz < 30) r_step_hz = 30;
112-
timerAlarmWrite(timer2, 2000000 / r_step_hz, true);
109+
portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止
110+
if (g_motor_move) {
111+
if (g_step_hz_r < 30) g_step_hz_r = 30;
112+
timerAlarmWrite(g_timer2, 2000000 / g_step_hz_r, true);
113113
digitalWrite(PWM_R, HIGH);
114114
for (int i = 0; i < 100; i++) {
115115
asm("nop \n");
116116
}
117117
digitalWrite(PWM_R, LOW);
118-
step_r++;
118+
g_step_r++;
119119
}
120-
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
120+
portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可
121121
}
122122

123123
//Lモータの周期数割り込み
124124
void IRAM_ATTR isrL(void)
125125
{
126-
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
127-
if (motor_move) {
128-
if (l_step_hz < 30) l_step_hz = 30;
129-
timerAlarmWrite(timer3, 2000000 / l_step_hz, true);
126+
portENTER_CRITICAL_ISR(&g_timer_mux); //割り込み禁止
127+
if (g_motor_move) {
128+
if (g_step_hz_l < 30) g_step_hz_l = 30;
129+
timerAlarmWrite(g_timer3, 2000000 / g_step_hz_l, true);
130130
digitalWrite(PWM_L, HIGH);
131131
for (int i = 0; i < 100; i++) {
132132
asm("nop \n");
133133
};
134134
digitalWrite(PWM_L, LOW);
135-
step_l++;
135+
g_step_l++;
136136
}
137-
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
137+
portEXIT_CRITICAL_ISR(&g_timer_mux); //割り込み許可
138138
}
139139

140140
//twist message cb
141141
void subscriptionCallback(const void * msgin)
142142
{
143-
const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
143+
const geometry_msgs__msg__Twist * g_msg = (const geometry_msgs__msg__Twist *)msgin;
144144

145-
speed = msg->linear.x * 1000.0;
146-
omega = msg->angular.z;
145+
g_speed = g_msg->linear.x * 1000.0;
146+
g_omega = g_msg->angular.z;
147147
}
148148

149149
void setup()
@@ -174,43 +174,43 @@ void setup()
174174

175175
delay(2000);
176176

177-
timer0 = timerBegin(0, 80, true); //1us
178-
timerAttachInterrupt(timer0, &onTimer0, true);
179-
timerAlarmWrite(timer0, 1000, true); //1kHz
180-
timerAlarmEnable(timer0);
177+
g_timer0 = timerBegin(0, 80, true); //1us
178+
timerAttachInterrupt(g_timer0, &onTimer0, true);
179+
timerAlarmWrite(g_timer0, 1000, true); //1kHz
180+
timerAlarmEnable(g_timer0);
181181

182-
timer2 = timerBegin(2, 40, true); //0.5us
183-
timerAttachInterrupt(timer2, &isrR, true);
184-
timerAlarmWrite(timer2, 13333, true); //150Hz
185-
timerAlarmEnable(timer2);
182+
g_timer2 = timerBegin(2, 40, true); //0.5us
183+
timerAttachInterrupt(g_timer2, &isrR, true);
184+
timerAlarmWrite(g_timer2, 13333, true); //150Hz
185+
timerAlarmEnable(g_timer2);
186186

187-
timer3 = timerBegin(3, 40, true); //0.5us
188-
timerAttachInterrupt(timer3, &isrL, true);
189-
timerAlarmWrite(timer3, 13333, true); //150Hz
190-
timerAlarmEnable(timer3);
187+
g_timer3 = timerBegin(3, 40, true); //0.5us
188+
timerAttachInterrupt(g_timer3, &isrL, true);
189+
timerAlarmWrite(g_timer3, 13333, true); //150Hz
190+
timerAlarmEnable(g_timer3);
191191

192-
allocator = rcl_get_default_allocator();
192+
g_allocator = rcl_get_default_allocator();
193193

194194
//create init_options
195-
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
195+
RCCHECK(rclc_support_init(&g_support, 0, NULL, &g_allocator));
196196

197-
// create node
198-
RCCHECK(rclc_node_init_default(&node, "micro_ros_pico_node", "", &support));
197+
// create g_node
198+
RCCHECK(rclc_node_init_default(&g_node, "micro_ros_pico_node", "", &g_support));
199199

200-
// create subscriber
200+
// create g_subscriber
201201
RCCHECK(rclc_subscription_init_default(
202-
&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"));
202+
&g_subscriber, &g_node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"));
203203

204-
// create executor
205-
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
204+
// create g_executor
205+
RCCHECK(rclc_executor_init(&g_executor, &g_support.context, 1, &g_allocator));
206206
RCCHECK(rclc_executor_add_subscription(
207-
&executor, &subscriber, &msg, &subscriptionCallback, ON_NEW_DATA));
207+
&g_executor, &g_subscriber, &g_msg, &subscriptionCallback, ON_NEW_DATA));
208208

209209
digitalWrite(MOTOR_EN, HIGH);
210210
}
211211

212212
void loop()
213213
{
214214
delay(10);
215-
RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
215+
RCCHECK(rclc_executor_spin_some(&g_executor, RCL_MS_TO_NS(100)));
216216
}

uROS_STEP11_tfMsg/intrrupt.ino

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -16,14 +16,14 @@ void controlInterrupt(void)
1616
{
1717
double speed_r, speed_l;
1818

19-
if ((speed < 0.001) && (speed > -0.001) && (omega < 0.001) && (omega > -0.001)) {
20-
motor_move = 0;
19+
if ((g_speed < 0.001) && (g_speed > -0.001) && (g_omega < 0.001) && (g_omega > -0.001)) {
20+
g_motor_move = 0;
2121
} else {
22-
motor_move = 1;
22+
g_motor_move = 1;
2323
}
2424

25-
speed_r = speed + omega * TREAD_WIDTH / 2.0;
26-
speed_l = speed - omega * TREAD_WIDTH / 2.0;
25+
speed_r = g_speed + g_omega * TREAD_WIDTH / 2.0;
26+
speed_l = g_speed - g_omega * TREAD_WIDTH / 2.0;
2727

2828
if ((speed_r > 0.001) && (speed_r < MIN_SPEED)) {
2929
speed_r = MIN_SPEED;
@@ -35,25 +35,25 @@ void controlInterrupt(void)
3535
} else if ((speed_l < -0.001) && (speed_l > (-1.0 * MIN_SPEED))) {
3636
speed_l = -1.0 * MIN_SPEED;
3737
}
38-
speed = (speed_r + speed_l) / 2.0;
38+
g_speed = (speed_r + speed_l) / 2.0;
3939

40-
odom_x += speed * 0.001 * cos(odom_theta) * 0.001;
41-
odom_y += speed * 0.001 * sin(odom_theta) * 0.001;
42-
odom_theta += omega * 0.001;
43-
position_r += speed_r * 0.001 / (48 * PI) * 2 * PI;
44-
position_l -= speed_l * 0.001 / (48 * PI) * 2 * PI;
40+
g_odom_x += g_speed * 0.001 * cos(g_odom_theta) * 0.001;
41+
g_odom_y += g_speed * 0.001 * sin(g_odom_theta) * 0.001;
42+
g_odom_theta += g_omega * 0.001;
43+
g_position_r += speed_r * 0.001 / (48 * PI) * 2 * PI;
44+
g_position_l -= speed_l * 0.001 / (48 * PI) * 2 * PI;
4545

4646
if (speed_r > 0) {
4747
digitalWrite(CW_R, LOW);
4848
} else {
4949
digitalWrite(CW_R, HIGH);
5050
}
51-
r_step_hz = abs((signed short)(speed_r / PULSE));
51+
g_step_hz_r = abs((signed short)(speed_r / PULSE));
5252

5353
if (speed_l > 0) {
5454
digitalWrite(CW_L, LOW);
5555
} else {
5656
digitalWrite(CW_L, HIGH);
5757
}
58-
l_step_hz = abs((signed short)(speed_l / PULSE));
58+
g_step_hz_l = abs((signed short)(speed_l / PULSE));
5959
}

0 commit comments

Comments
 (0)