Skip to content

Commit b229467

Browse files
author
Shota Aoki
authored
変数名、関数名のリファクタリング (#7)
* STEP3 関数名をlowerCamelCaseに変更 * STEP4 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP5 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP6 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP7 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP8 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP8 アンダースコアで始まる変数modeを改名 * STEP10 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP11 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP12 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP13 関数名をlowerCamelCaseに、変数名をsnake_caseに変更 * STEP13 アンダースコアで始まる変数modeを改名 * STEP8 アンダースコアで始まる変数modeを改名.変更漏れの修正
1 parent f4198a9 commit b229467

39 files changed

+846
-846
lines changed

uROS_STEP10_twistMsg/interrupt.ino

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
void control_interrupt(void)
15+
void controlInterrupt(void)
1616
{
1717
double speed_r, speed_l;
1818

@@ -36,7 +36,7 @@ void control_interrupt(void)
3636
} else if (speed_r < MIN_SPEED) {
3737
speed_r = MIN_SPEED;
3838
}
39-
RStepHz = (signed short)(speed_r / PULSE);
39+
r_step_hz = (signed short)(speed_r / PULSE);
4040

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

uROS_STEP10_twistMsg/uROS_STEP10_twistMsg.ino

Lines changed: 28 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -52,12 +52,12 @@ hw_timer_t * timer0 = NULL;
5252
hw_timer_t * timer2 = NULL;
5353
hw_timer_t * timer3 = NULL;
5454

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

57-
unsigned short RStepHz = MIN_HZ;
58-
unsigned short LStepHz = MIN_HZ;
57+
unsigned short r_step_hz = MIN_HZ;
58+
unsigned short l_step_hz = MIN_HZ;
5959

60-
volatile unsigned int StepR, StepL;
60+
volatile unsigned int step_r, step_l;
6161

6262
volatile double max_speed = MIN_SPEED;
6363
volatile double min_speed = MIN_SPEED;
@@ -75,18 +75,18 @@ volatile bool motor_move = 0;
7575
{ \
7676
rcl_ret_t temp_rc = fn; \
7777
if ((temp_rc != RCL_RET_OK)) { \
78-
error_loop(); \
78+
errorLoop(); \
7979
} \
8080
}
8181
#define RCSOFTCHECK(fn) \
8282
{ \
8383
rcl_ret_t temp_rc = fn; \
8484
if ((temp_rc != RCL_RET_OK)) { \
85-
error_loop(); \
85+
errorLoop(); \
8686
} \
8787
}
8888

89-
void error_loop()
89+
void errorLoop()
9090
{
9191
while (1) {
9292
digitalWrite(LED0, !digitalRead(LED0));
@@ -96,49 +96,49 @@ void error_loop()
9696

9797
//割り込み
9898
//目標値の更新周期1kHz
99-
void IRAM_ATTR OnTimer0(void)
99+
void IRAM_ATTR onTimer0(void)
100100
{
101-
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
102-
control_interrupt();
103-
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
101+
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
102+
controlInterrupt();
103+
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
104104
}
105105

106106
//Rモータの周期数割り込み
107-
void IRAM_ATTR IsrR(void)
107+
void IRAM_ATTR isrR(void)
108108
{
109-
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
109+
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
110110
if (motor_move) {
111-
if (RStepHz < 30) RStepHz = 30;
112-
timerAlarmWrite(timer2, 2000000 / RStepHz, true);
111+
if (r_step_hz < 30) r_step_hz = 30;
112+
timerAlarmWrite(timer2, 2000000 / r_step_hz, 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-
StepR++;
118+
step_r++;
119119
}
120-
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
120+
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
121121
}
122122

123123
//Lモータの周期数割り込み
124-
void IRAM_ATTR IsrL(void)
124+
void IRAM_ATTR isrL(void)
125125
{
126-
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
126+
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
127127
if (motor_move) {
128-
if (LStepHz < 30) LStepHz = 30;
129-
timerAlarmWrite(timer3, 2000000 / LStepHz, true);
128+
if (l_step_hz < 30) l_step_hz = 30;
129+
timerAlarmWrite(timer3, 2000000 / l_step_hz, 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-
StepL++;
135+
step_l++;
136136
}
137-
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
137+
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
138138
}
139139

140140
//twist message cb
141-
void subscription_callback(const void * msgin)
141+
void subscriptionCallback(const void * msgin)
142142
{
143143
const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
144144

@@ -175,17 +175,17 @@ void setup()
175175
delay(2000);
176176

177177
timer0 = timerBegin(0, 80, true); //1us
178-
timerAttachInterrupt(timer0, &OnTimer0, true);
178+
timerAttachInterrupt(timer0, &onTimer0, true);
179179
timerAlarmWrite(timer0, 1000, true); //1kHz
180180
timerAlarmEnable(timer0);
181181

182182
timer2 = timerBegin(2, 40, true); //0.5us
183-
timerAttachInterrupt(timer2, &IsrR, true);
183+
timerAttachInterrupt(timer2, &isrR, true);
184184
timerAlarmWrite(timer2, 13333, true); //150Hz
185185
timerAlarmEnable(timer2);
186186

187187
timer3 = timerBegin(3, 40, true); //0.5us
188-
timerAttachInterrupt(timer3, &IsrL, true);
188+
timerAttachInterrupt(timer3, &isrL, true);
189189
timerAlarmWrite(timer3, 13333, true); //150Hz
190190
timerAlarmEnable(timer3);
191191

@@ -204,7 +204,7 @@ void setup()
204204
// create executor
205205
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
206206
RCCHECK(rclc_executor_add_subscription(
207-
&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
207+
&executor, &subscriber, &msg, &subscriptionCallback, ON_NEW_DATA));
208208

209209
digitalWrite(MOTOR_EN, HIGH);
210210
}

uROS_STEP11_tfMsg/intrrupt.ino

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
void control_interrupt(void)
15+
void controlInterrupt(void)
1616
{
1717
double speed_r, speed_l;
1818

@@ -48,12 +48,12 @@ void control_interrupt(void)
4848
} else {
4949
digitalWrite(CW_R, HIGH);
5050
}
51-
R_STEP_HZ = abs((signed short)(speed_r / PULSE));
51+
r_step_hz = 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+
l_step_hz = abs((signed short)(speed_l / PULSE));
5959
}

uROS_STEP11_tfMsg/uROS_STEP11_tfMsg.ino

Lines changed: 25 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -60,10 +60,10 @@ hw_timer_t * timer0 = NULL;
6060
hw_timer_t * timer2 = NULL;
6161
hw_timer_t * timer3 = NULL;
6262

63-
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
63+
portMUX_TYPE timer_mux = portMUX_INITIALIZER_UNLOCKED;
6464

65-
unsigned short R_STEP_HZ = MIN_HZ;
66-
unsigned short L_STEP_HZ = MIN_HZ;
65+
unsigned short r_step_hz = MIN_HZ;
66+
unsigned short l_step_hz = MIN_HZ;
6767

6868
volatile unsigned int step_r, step_l;
6969

@@ -86,26 +86,26 @@ volatile bool motor_move = 0;
8686
{ \
8787
rcl_ret_t temp_rc = fn; \
8888
if ((temp_rc != RCL_RET_OK)) { \
89-
error_loop(); \
89+
errorLoop(); \
9090
} \
9191
}
9292
#define RCSOFTCHECK(fn) \
9393
{ \
9494
rcl_ret_t temp_rc = fn; \
9595
if ((temp_rc != RCL_RET_OK)) { \
96-
error_loop(); \
96+
errorLoop(); \
9797
} \
9898
}
9999

100-
void error_loop()
100+
void errorLoop()
101101
{
102102
while (1) {
103103
digitalWrite(LED0, !digitalRead(LED0));
104104
delay(100);
105105
}
106106
}
107107

108-
const void euler_to_quat(float x, float y, float z, double * q)
108+
const void eulerToQuat(float x, float y, float z, double * q)
109109
{
110110
float c1 = cos(y / 2);
111111
float c2 = cos(z / 2);
@@ -125,47 +125,47 @@ const void euler_to_quat(float x, float y, float z, double * q)
125125
//目標値の更新周期1kHz
126126
void IRAM_ATTR onTimer0(void)
127127
{
128-
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
129-
control_interrupt();
130-
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
128+
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
129+
controlInterrupt();
130+
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
131131
}
132132

133133
//Rモータの周期数割り込み
134-
void IRAM_ATTR isr_r(void)
134+
void IRAM_ATTR isrR(void)
135135
{
136-
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
136+
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
137137
if (motor_move) {
138-
if (R_STEP_HZ < 30) R_STEP_HZ = 30;
139-
timerAlarmWrite(timer2, 2000000 / R_STEP_HZ, true);
138+
if (r_step_hz < 30) r_step_hz = 30;
139+
timerAlarmWrite(timer2, 2000000 / r_step_hz, true);
140140
digitalWrite(PWM_R, HIGH);
141141
for (int i = 0; i < 100; i++) {
142142
asm("nop \n");
143143
}
144144
digitalWrite(PWM_R, LOW);
145145
step_r++;
146146
}
147-
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
147+
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
148148
}
149149

150150
//Lモータの周期数割り込み
151-
void IRAM_ATTR isr_l(void)
151+
void IRAM_ATTR isrL(void)
152152
{
153-
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
153+
portENTER_CRITICAL_ISR(&timer_mux); //割り込み禁止
154154
if (motor_move) {
155-
if (L_STEP_HZ < 30) L_STEP_HZ = 30;
156-
timerAlarmWrite(timer3, 2000000 / L_STEP_HZ, true);
155+
if (l_step_hz < 30) l_step_hz = 30;
156+
timerAlarmWrite(timer3, 2000000 / l_step_hz, true);
157157
digitalWrite(PWM_L, HIGH);
158158
for (int i = 0; i < 100; i++) {
159159
asm("nop \n");
160160
};
161161
digitalWrite(PWM_L, LOW);
162162
step_l++;
163163
}
164-
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
164+
portEXIT_CRITICAL_ISR(&timer_mux); //割り込み許可
165165
}
166166

167167
//twist message cb
168-
void subscription_callback(const void * msgin)
168+
void subscriptionCallback(const void * msgin)
169169
{
170170
const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
171171

@@ -207,12 +207,12 @@ void setup()
207207
timerAlarmEnable(timer0);
208208

209209
timer2 = timerBegin(2, 40, true); //0.5us
210-
timerAttachInterrupt(timer2, &isr_r, true);
210+
timerAttachInterrupt(timer2, &isrR, true);
211211
timerAlarmWrite(timer2, 13333, true); //150Hz
212212
timerAlarmEnable(timer2);
213213

214214
timer3 = timerBegin(3, 40, true); //0.5us
215-
timerAttachInterrupt(timer3, &isr_l, true);
215+
timerAttachInterrupt(timer3, &isrL, true);
216216
timerAlarmWrite(timer3, 13333, true); //150Hz
217217
timerAlarmEnable(timer3);
218218

@@ -239,7 +239,7 @@ void setup()
239239
// create executor
240240
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
241241
RCCHECK(rclc_executor_add_subscription(
242-
&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
242+
&executor, &subscriber, &msg, &subscriptionCallback, ON_NEW_DATA));
243243

244244
tf_message = tf2_msgs__msg__TFMessage__create();
245245
geometry_msgs__msg__TransformStamped__Sequence__init(&tf_message->transforms, 1);
@@ -280,7 +280,7 @@ void loop()
280280
jstate.header.stamp.sec = current / 1000000;
281281
jstate.header.stamp.nanosec = current - jstate.header.stamp.sec * 1000000;
282282

283-
euler_to_quat(0, 0, odom_theta, q);
283+
eulerToQuat(0, 0, odom_theta, q);
284284
tf_message->transforms.data[0].transform.translation.x = odom_x;
285285
tf_message->transforms.data[0].transform.translation.y = odom_y;
286286
tf_message->transforms.data[0].transform.translation.z = 0.0;

uROS_STEP12_SensorMsg/uROS_STEP12_SensorMsg.ino

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ rcl_node_t node;
4949

5050
hw_timer_t * timer1 = NULL;
5151

52-
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
52+
portMUX_TYPE timer_mux = portMUX_INITIALIZER_UNLOCKED;
5353

5454
volatile short sensor_fr_value;
5555
volatile short sensor_fl_value;
@@ -61,7 +61,7 @@ volatile short battery_value;
6161
{ \
6262
rcl_ret_t temp_rc = fn; \
6363
if ((temp_rc != RCL_RET_OK)) { \
64-
error_loop(); \
64+
errorLoop(); \
6565
} \
6666
}
6767
#define RCSOFTCHECK(fn) \
@@ -71,7 +71,7 @@ volatile short battery_value;
7171
} \
7272
}
7373

74-
void error_loop()
74+
void errorLoop()
7575
{
7676
while (1) {
7777
digitalWrite(LED0, !digitalRead(LED0));
@@ -82,7 +82,7 @@ void error_loop()
8282
void IRAM_ATTR onTimer1(void)
8383
{
8484
static char cnt = 0;
85-
portENTER_CRITICAL_ISR(&timerMux);
85+
portENTER_CRITICAL_ISR(&timer_mux);
8686
switch (cnt) {
8787
case 0:
8888
digitalWrite(SLED_FR, HIGH); //LED点灯
@@ -120,10 +120,10 @@ void IRAM_ATTR onTimer1(void)
120120
}
121121
cnt++;
122122
if (cnt == 4) cnt = 0;
123-
portEXIT_CRITICAL_ISR(&timerMux);
123+
portEXIT_CRITICAL_ISR(&timer_mux);
124124
}
125125

126-
void publisher_task(void * pvParameters)
126+
void publisherTask(void * pvParameters)
127127
{
128128
while (1) {
129129
sensor_msg.forward_r = sensor_fr_value;
@@ -183,7 +183,7 @@ void setup()
183183

184184
xTaskCreateUniversal(
185185
// xTaskCreatePinnedToCore(
186-
publisher_task, "publisher_task", 4096, NULL, 1, NULL,
186+
publisherTask, "publisherTask", 4096, NULL, 1, NULL,
187187
// PRO_CPU_NUM
188188
// APP_CPU_NUM
189189
CONFIG_ARDUINO_RUNNING_CORE);

0 commit comments

Comments
 (0)