Diff: main.cpp

Revision:

2:f10d06f31141

Parent:

1:1f21cff06302

--- a/main.cpp	Wed Mar 17 17:54:54 2021 +0000
+++ b/main.cpp	Thu Jun 10 06:13:50 2021 +0000
@@ -1,6 +1,6 @@
 // Nucleo_CAN_master for AIRo-4.1
 // Created by Atsushi Kakogawa, 2020.03.20
-// Modified by ****** ******, 2020.**.**
+// Modified by Yoshimichi Oka, 2021.06.09
 // Department of Robotics, Ritsumeikan University, Japan
 
 #include "mbed.h"
@@ -90,12 +90,17 @@
                 buf[i] = pc.getc();
                 if (buf[i] == '\n') {
                     
-                    // ***** Angle control mode *****
+                    // ***** Angle control mode (yaw joints)*****
                     // send(ID, mode(0), dir(0 or 1), 3rd digit, 2rd digit, 1rd digit)
                     if (buf[i-5] == 'A'){ 
                          //if (buf[i-4]-0x30==4){                      //id=4には前部リニアPOTの値を送信
-                         send(buf[i-4]-0x30,2,0,buf[i-3],buf[i-2],buf[i-1]);
+                         send(buf[i-4]-0x30,2,0,buf[i-3],buf[i-2],buf[i-1]);  //脱力制御
                          //}
+                    
+                    // ***** Yaw angle control mode (for T-branch) *****
+                    // send(ID, mode(1), dir(0), NC, NC, NC)
+                    } else if (buf[i-5] == 'T') {
+                        send(buf[i-4]-0x30,3,0,buf[i-3],buf[i-2],buf[i-1]);  //先頭モジュールのみ角度制御，他は脱力制御
                         
                     // ***** Receive mode (single data) *****
                     // send(ID, mode(1), dir(0), NC, NC, NC)