Freeボタン（Set Free Moveを発行する）を追加

3110 · Jan 17, 2021 · f17d273 · f17d273
1 parent 961524c
commit f17d273
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 8 deletions.
diff --git a/README.md b/README.md
@@ -3,19 +3,23 @@
 M5Stack Basic のシリアルから ATOM へコマンドを送信し，処理結果を ATOM から受信する[オリジナルの Transponder](https://github.com/elephantrobotics/myCobot/tree/main/Arduino/MycobotBasic/examples/Transponder)に起動画面とデータ送受信の様子を表示する機能を付けました。
 
 <div align="center">
-    <a href="https://gyazo.com/b3b63dd836e511b7ff89201de3dd3141"><img src="https://i.gyazo.com/b3b63dd836e511b7ff89201de3dd3141.png" alt="AtOM" width="100"/></a><a href="https://gyazo.com/1efae34d2c922ad8e92b43453d971049"><img src="https://i.gyazo.com/1efae34d2c922ad8e92b43453d971049.png" alt="起動画面" width="300"/></a>
+    <a href="https://gyazo.com/b3b63dd836e511b7ff89201de3dd3141"><img src="https://i.gyazo.com/b3b63dd836e511b7ff89201de3dd3141.png" alt="AtOM" width="100"/></a><a href="https://gyazo.com/556352b86c8309f99836349b492a17f1"><img src="https://i.gyazo.com/556352b86c8309f99836349b492a17f1.png" alt="起動画面" width="300"/></a>
 </div>
 
 Transponder を起動すると上記の画面が表示されます。ATOM が白色に光ることで，Basic から ATOM にデータの送信ができていることがわかります。
 
 <div align="center">
-    <a href="https://gyazo.com/7058c5abed3b6e779e8e7ba966516c70"><img src="https://i.gyazo.com/7058c5abed3b6e779e8e7ba966516c70.png" alt="送受信の表示" width="300"/></a>
+    <a href="https://gyazo.com/6792aca0b658745d90c7b9adc90f2f82"><img src="https://i.gyazo.com/6792aca0b658745d90c7b9adc90f2f82.png" alt="送受信の表示" width="300"/></a>
 </div>
 
 シリアル経由で ATOM へデータを送信すると左の丸が緑に，ATOM からデータを受信すると右の丸が赤になります。
 
 <div align="center">
-    <a href="https://gyazo.com/e672b6b8a5f62405cbae1bb4953b830c"><img src="https://i.gyazo.com/e672b6b8a5f62405cbae1bb4953b830c.png" alt="コマンド表示" width="300"/></a>
+    <a href="https://gyazo.com/078ff1d56b985e046877babb9c9e659e"><img src="https://i.gyazo.com/078ff1d56b985e046877babb9c9e659e.png" alt="コマンド表示" width="300"/></a>
 </div>
 
 A ボタン（一番左のボタン）を押すと，画面に送信コマンドを表示するかを選択できます。ON にすると，送信しているコマンドと今までに送ったコマンド数を画面に表示します。表示するかの設定は本体の不揮発性メモリに保存され，起動時に前回の設定が反映されます。
+
+コマンドが送られていない状態で C ボタン（一番右のボタン）を押すと，Set Free Move コマンドを送信します。サーボが固定されてしまっているときに押すと自由に動かせるようになります。
+
+※v0.0.2 時点では，起動時に LED が白に光るときと Free ボタンを押したときに，どちらもコマンドを直接`Serial2`に書き込む関係で，送信ではなく受信扱いになって右の丸が赤になります。
diff --git a/lib/MyCobot/MyCobot.cpp b/lib/MyCobot/MyCobot.cpp
@@ -6,6 +6,7 @@ const byte MyCobot::FRAME_HEADER = 0xfe;
 const byte MyCobot::FRAME_FOOTER = 0xfa;
 
 const byte MyCobot::CMD_SET_LED_LEN = 5;
+const byte MyCobot::CMD_SET_FREE_MOVE_LEN = 2;
 
 FrameState MyCobot::checkFrameState(const FrameState state, const int b)
 {

diff --git a/lib/MyCobot/MyCobot.h b/lib/MyCobot/MyCobot.h
@@ -10,6 +10,7 @@ namespace MyCobot
   extern const byte FRAME_FOOTER;
 
   extern const byte CMD_SET_LED_LEN;
+  extern const byte CMD_SET_FREE_MOVE_LEN;
 
   enum Command
   {

diff --git a/src/main.cpp b/src/main.cpp
@@ -5,7 +5,7 @@
 const char *PREFS_NAMESPACE = "transponder";
 const char *PREFS_IS_DUMPED = "is_dumped";
 
-const char *VERSION = "v0.0.1";
+const char *VERSION = "v0.0.2";
 const uint8_t VERSION_DATUM = TL_DATUM;
 const int32_t VERSION_X_POS = 5;
 const int32_t VERSION_Y_POS = 10;
@@ -77,6 +77,9 @@ const enum ButtonName DUMP_BUTTON_NAME = ButtonName::ButtonA;
 const char *DUMP_BUTTON_ON = "ON";
 const char *DUMP_BUTTON_OFF = "OFF";
 
+const char *FREE_MOVE_BUTTON_LABEL_NAME = "Free";
+const enum ButtonName FREE_MOVE_BUTTON_NAME = ButtonName::ButtonC;
+
 const byte ATOM_LED_R = 0xff;
 const byte ATOM_LED_G = 0xff;
 const byte ATOM_LED_B = 0xff;
@@ -87,6 +90,7 @@ const int SERIAL2_BAUD_RATE = 1000000; // Basic -> ATOM
 extern void setup(void);
 extern void loop(void);
 extern void setLED(const byte, const byte, const byte);
+extern void setFreeMove(void);
 extern void setTitle(const char *, const char *);
 extern void setCommandName(const int, const uint16_t);
 extern void setSend(const bool);
@@ -120,6 +124,8 @@ void setup(void)
 
   is_dumped = getDumped();
   setDumpButton(DUMP_BUTTON_NAME, is_dumped);
+
+  setButtonName(FREE_MOVE_BUTTON_NAME, FREE_MOVE_BUTTON_LABEL_NAME);
 }
 
 void loop(void)
@@ -132,11 +138,18 @@ void loop(void)
     setDumpButton(DUMP_BUTTON_NAME, is_dumped);
   }
 
-  setSend(false);
+  if (M5.BtnC.wasPressed())
+  {
+    if (frame_state == MyCobot::STATE_NONE)
+    {
+      setFreeMove();
+    }
+  }
+
+  setSend(Serial.available() > 0);
   while (Serial.available() > 0)
   {
     const int b = Serial.read();
-    setSend(true);
     Serial2.write(b);
     frame_state = MyCobot::checkFrameState(frame_state, b);
     if (frame_state == MyCobot::STATE_CMD)
@@ -149,10 +162,9 @@ void loop(void)
     }
   }
 
-  setRecv(false);
+  setRecv(Serial2.available() > 0);
   while (Serial2.available() > 0)
   {
-    setRecv(true);
     Serial.write(Serial2.read());
   }
 }
@@ -170,6 +182,16 @@ void setLED(const byte r, const byte g, const byte b)
   Serial2.flush();
 }
 
+void setFreeMove(void)
+{
+  Serial2.write(MyCobot::FRAME_HEADER);
+  Serial2.write(MyCobot::FRAME_HEADER);
+  Serial2.write(MyCobot::CMD_SET_FREE_MOVE_LEN);
+  Serial2.write(MyCobot::SET_FREE_MOVE);
+  Serial2.write(MyCobot::FRAME_FOOTER);
+  Serial2.flush();
+}
+
 void setTitle(const char *title, const char *version)
 {
   sprite.setColorDepth(8);