AruduinoでL6470_gohome()が作動しない。

実現したいこと

AruduinoでL6470_gohome()及びL6470_gohome2()が作動するようにしたいです。

発生している問題・分からないこと

北の国から電子工作（仮）様（http://spinelify.blog.fc2.com/blog-entry-81.html）
コードをもとに改良してみたのですが、parseCommand関数内のL6470_gohome()及びL6470_gohome2()が作動しません。シリアル通信して、コマンドを送信しています。同じコマンドの送信でもキーボード入力関数(mannualCommand)を除いたプログラムでは作動するのですが、キーボード入力関数を入れた途端に作動しなくなります。また、L6470_gohome()及びL6470_gohome2()をL6470_move()及びL6470_move2()にすると普通に動きます。初心的な質問で申し訳ございません。教えていただけると幸いです。よろしくお願いいたします。

該当のソースコード

Aruduino_該当の問題プログラム
1/* デイジーチェン接続
2Arduino11番ピン（MOSI）- 6番ピン(SDI) L6470(一台目)
33番ピン(SDO) - (SDI)L6470(二台目)
4(SDO) - 12番ピン（MISO)
5Arduino SCK 共通
6SS - CS共通
7*/
8
9#include <SPI.h>
10#include <MsTimer2.h>
11#include <stdio.h>
12
13// ピン定義
14#define PIN_SPI_MOSI 11
15#define PIN_SPI_MISO 12
16#define PIN_SPI_SCK 13
17#define PIN_SPI_SS 10
18#define PIN_BUSY 9
19#define PIN_BUSY2 8
20
21// キー操作用フラグ
22bool runM1Forward = false, runM1Backward = false;
23bool runM2Forward = false, runM2Backward = false;
24bool rapidrunM1Forward = false, rapidrunM1Backward = false;
25bool rapidrunM2Forward = false, rapidrunM2Backward = false;
26
27// シリアル文字列バッファ
28String inputString = "";
29
30// -------------------- X/Yコマンド処理 --------------------
31
32void parseCommand(String cmd) {
33  Serial.print("Received command: ");
34  Serial.println(cmd);
35
36  // 原点復帰コマンド
37  if (cmd.equalsIgnoreCase("go")) {
38    Serial.println("Going home (both motors to origin)");
39
40    // モータ1（Y軸方向）原点復帰
41    L6470_gohome();
42    // モータ2（X軸方向）原点復帰
43    L6470_gohome2();
44
45    // --- 完了待ち ---
46    while (digitalRead(PIN_BUSY) == LOW || digitalRead(PIN_BUSY2) == LOW) {
47      delay(10);
48    }
49
50    Serial.println("DONE");
51
52    return;  // ここで終了
53  }
54
55  float xVal = 0.0, yVal = 0.0;
56  int xIndex = cmd.indexOf('X');
57  int yIndex = cmd.indexOf('Y');
58
59  if (xIndex != -1 && yIndex != -1) {
60    xVal = cmd.substring(xIndex + 1, yIndex).toFloat();
61    yVal = cmd.substring(yIndex + 1).toFloat();
62
63    // モータ2（X軸方向）
64    if (abs(xVal) > 0.01) {
65      int directionX = (xVal > 0) ? 0 : 1;  // 逆転が0, 正転が1 → 今回は逆にするので反転
66      Serial.print("Moving motor 2, direction: ");
67      Serial.print(directionX);
68      Serial.print(", steps: ");
69      Serial.println(abs(xVal));
70      L6470_move2(directionX, abs(xVal));
71    } else {
72      Serial.println("Stopping motor 2");
73      L6470_softstop2();
74    }
75
76    // モータ1（Y軸方向）
77    if (abs(yVal) > 0.01) {
78      int directionY = (yVal > 0) ? 0 : 1;  // 逆転が0, 正転が1 → 今回は逆にするので反転
79      Serial.print("Moving motor 1, direction: ");
80      Serial.print(directionY);
81      Serial.print(", steps: ");
82      Serial.println(abs(yVal));
83      L6470_move(directionY, abs(yVal));
84    } else {
85      Serial.println("Stopping motor 1");
86      L6470_softstop();
87    }
88
89    // --- 両方のモーターが完了するまで待つ ---
90    while (digitalRead(PIN_BUSY) == LOW || digitalRead(PIN_BUSY2) == LOW) {
91      delay(10);
92    }
93
94    // 完了したら通知
95    Serial.println("DONE");
96
97  } else {
98    Serial.println("Invalid format: use X##.## Y##.##");
99  }
100}
101
102void mannualCommand(char cmd2) {
103  // モーター1
104  if (cmd2 == 'w') {
105    runM1Forward = true;
106    runM1Backward = false;
107    rapidrunM1Forward = false;
108    rapidrunM1Backward = false;
109  }
110
111  else if (cmd2 == 's') {
112    runM1Forward = false;
113    runM1Backward = true;
114    rapidrunM1Forward = false;
115    rapidrunM1Backward = false;
116  }
117
118  else if (cmd2 == 'e') {
119    runM1Forward = false;
120    runM1Backward = false;
121    rapidrunM1Forward = true;
122    rapidrunM1Backward = false;
123  }
124
125  else if (cmd2 == 'd') {
126    runM1Forward = false;
127    runM1Backward = false;
128    rapidrunM1Forward = false;
129    rapidrunM1Backward = true;
130  }
131
132  else if (cmd2 == 'r') {
133    L6470_move(0, 4);
134    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
135    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
136  }
137
138  else if (cmd2 == 'f') {
139    L6470_move(1, 4);
140    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
141    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
142  }
143
144  // モーター2
145  else if (cmd2 == 'y') {
146    runM2Forward = true;
147    runM2Backward = false;
148    rapidrunM2Forward = false;
149    rapidrunM2Backward = false;
150  }
151
152  else if (cmd2 == 'h') {
153    runM2Forward = false;
154    runM2Backward = true;
155    rapidrunM2Forward = false;
156    rapidrunM2Backward = false;
157  }
158
159  else if (cmd2 == 'u') {
160    runM2Forward = false;
161    runM2Backward = false;
162    rapidrunM2Forward = true;
163    rapidrunM2Backward = false;
164  }
165
166  else if (cmd2 == 'j') {
167    runM2Forward = false;
168    runM2Backward = false;
169    rapidrunM2Forward = false;
170    rapidrunM2Backward = true;
171  }
172
173  else if (cmd2 == 'i') {
174    L6470_move2(0, 3);
175    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
176    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
177  }
178
179  else if (cmd2 == 'k') {
180    L6470_move2(1, 3);
181    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
182    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
183  }
184
185  // 原点リセット
186  else if (cmd2 == ' ') {
187    L6470_resetpos();
188    L6470_resetpos2();
189    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
190    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
191  }
192
193  // 原点へ
194  else if (cmd2 == 'b') {
195    L6470_gohome();
196    L6470_gohome2();
197    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
198    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
199  }
200
201  // 停止（任意）
202  else if (cmd2 == 'x') {
203    L6470_softstop();
204    L6470_softstop2();
205    runM1Forward = runM1Backward = rapidrunM1Forward = rapidrunM1Backward = false;
206    runM2Forward = runM2Backward = rapidrunM2Forward = rapidrunM2Backward = false;
207  }
208
209//連続回転
210if (runM1Forward) L6470_run(0, 0x1E);
211else if (runM1Backward) L6470_run(1, 0x1E);
212
213if (runM2Forward) L6470_run2(0, 0x1E);
214else if (runM2Backward) L6470_run2(1, 0x1E);
215
216if (rapidrunM1Forward) L6470_run(0, 0x258);
217else if (rapidrunM1Backward) L6470_run(1, 0x258);
218
219if (rapidrunM2Forward) L6470_run2(0, 0x258);
220else if (rapidrunM2Backward) L6470_run2(1, 0x258);
221}
222
223// -------------------- setup --------------------
224void setup() {
225  Serial.begin(115200);
226  Serial.setTimeout(5);  // 念のため短めのタイムアウト設定
227  delay(1000);
228
229  pinMode(PIN_SPI_MOSI, OUTPUT);
230  pinMode(PIN_SPI_MISO, INPUT);
231  pinMode(PIN_SPI_SCK, OUTPUT);
232  pinMode(PIN_SPI_SS, OUTPUT);
233  pinMode(PIN_BUSY, INPUT_PULLUP);
234  pinMode(PIN_BUSY2, INPUT_PULLUP);
235
236  SPI.begin();
237  SPI.setDataMode(SPI_MODE3);
238  SPI.setBitOrder(MSBFIRST);
239  digitalWrite(PIN_SPI_SS, HIGH);
240
241  L6470_resetdevice();
242  L6470_resetdevice2();
243  L6470_setup();
244  L6470_setup2();
245  L6470_getstatus();
246  L6470_getstatus2();
247
248  Serial.println("Arduino ready");
249  delay(2000);
250}
251
252// -------------------- loop --------------------
253void loop() {
254  while (Serial.available()) {
255    char inChar = (char)Serial.read();
256    if (inChar == '\r') continue;
257    if (inChar == '\n') {
258      if (inputString.length() > 0) {
259        Serial.println("Parsing inputString: " + inputString);
260        parseCommand(inputString);
261        inputString = "";
262      }
263    } else {
264      inputString += inChar;
265    }
266    mannualCommand(inChar);
267  }
268}
269
270  // -------------------- L6470 setup関数群はそのまま --------------------
271  void L6470_setup() {
272    L6470_setparam_acc(0x30);         //[R, WS] 加速度default 0x08A (12bit) (14.55*val+14.55[step/s^2])
273    L6470_setparam_dec(0x30);         //[R, WS] 減速度default 0x08A (12bit) (14.55*val+14.55[step/s^2])
274    L6470_setparam_maxspeed(0x2a);    //[R, WR]最大速度default 0x041 (10bit) (15.25*val+15.25[step/s])
275    L6470_setparam_minspeed(0x1200);  //[R, WS]最小速度default 0x000 (1+12bit) (0.238*val[step/s])
276    L6470_setparam_fsspd(0x027);      //[R, WR]μステップからフルステップへの切替点速度default 0x027 (10bit) (15.25*val+7.63[step/s])
277    L6470_setparam_kvalhold(0x28);    //[R, WR]停止時励磁電圧default 0x29 (8bit) (Vs[V]*val/256)
278    L6470_setparam_kvalrun(0x28);     //[R, WR]定速回転時励磁電圧default 0x29 (8bit) (Vs[V]*val/256)
279    L6470_setparam_kvalacc(0x28);     //[R, WR]加速時励磁電圧default 0x29 (8bit) (Vs[V]*val/256)
280    L6470_setparam_kvaldec(0x28);     //[R, WR]減速時励磁電圧default 0x29 (8bit) (Vs[V]*val/256)
281    L6470_setparam_stepmood(0x07);    //ステップモードdefault 0x07 (1+3+1+3bit)
282  }
283
284  void L6470_setup2() {
285    L6470_setparam_acc2(0x30);        
286    L6470_setparam_dec2(0x30);         
287    L6470_setparam_maxspeed2(0x2a);    
288    L6470_setparam_minspeed2(0x1200);  
289    L6470_setparam_fsspd2(0x027);   
290    L6470_setparam_kvalhold2(0x28);  
291    L6470_setparam_kvalrun2(0x28);    
292    L6470_setparam_kvalacc2(0x28);   
293    L6470_setparam_kvaldec2(0x28);    
294    L6470_setparam_stepmood2(0x07);    
295  }

試したこと・調べたこと

teratailやGoogle等で検索した
ソースコードを自分なりに変更した
知人に聞いた
その他

上記の詳細・結果

同じく原点に移動するgoto()なども試しましたがなぜか原点にだけは戻りませんでした。

補足

文字数のため、載せられるところだけ載せました。分かりにくくて申し訳ございません。

hiroki-o

2025/08/19 14:44

動作しないときのmannualCommand関数の引数は特定できていますか？ 'b'のときのみですか？

todo293

2025/08/19 14:53

コメントありがとうございます。現在、Radsberry piでシリアル通信しており、キーボード入力のプログラムとARマーカー追従のプログラムを別々に実行しています。キーボード入力のプログラムでは"b"を押すと、正常にL6470_gohome()及びL6470_gohome2()が作動します。その後にARマーカー追従のプログラムでARマーカーが見つからないときに"go"を送信しています。その際にparseCommand関数内のL6470_gohome()及びL6470_gohome2()が作動していません。しかし、L6470_gohome()及びL6470_gohome2()ではなく、別のL6470_move()及びL6470_move2()にすると正常に動きます。私の理解力が乏しく、正確な答えが出来ず申し訳ございません。