Revision 2:278a4347fd69, committed 2020-10-11

Comitter:

shibazakiwataru

Date:

Sun Oct 11 08:47:33 2020 +0000

Parent:

1:b4ca9ec7b90b

Commit message:

shibazaki_proguram_ashi

Changed in this revision

--- a/main.cpp	Sat Oct 10 02:36:16 2020 +0000
+++ b/main.cpp	Sun Oct 11 08:47:33 2020 +0000
@@ -4,8 +4,8 @@
 //1272552550b00000001\n受信例
 
 #define ruto_3 1.73205080757//√3
-#define MAX_QPPS1 40960
-#define MAX_QPPS2 40960
+#define MAX_QPPS1 40960/3
+#define MAX_QPPS2 40960/3
 #define ADRS1 130
 #define ADRS2 129
 #define TIME 0.02
@@ -27,7 +27,6 @@
 char motoR;//モーター回転数
 char button;//ボタン信号 上半身へ送信
 char reciv_c;
-int motoXV,motoYV,motoRV;
 
 string reciv_str = "";//trasからtransへ変更予定
 
@@ -39,8 +38,8 @@
     led3 = 0;
     led4 = 0;
     if(reciv_c == '\n'){//末尾改行コード（\n）感知
-        motoX = reciv_str[0];
-        motoY = reciv_str[1];
+        motoY = reciv_str[0];
+        motoX = reciv_str[1];
         motoR = reciv_str[2];
         button = reciv_str[3];
         reciv_str = "";
@@ -54,26 +53,27 @@
 }
 void aUSBtx(){//書き込み処理
     pc.printf("=%d,%d,%d",motoX,motoY,motoR);
-    pc.printf(",%d",button);
+    //pc.printf(",%d",button);
     recivsome.putc(button);//上半身送信
     recivsome.putc('\n');
     pc.printf("\n");
-    motoXV = motoX - 127;
-    motoYV = motoY - 127;
-    motoRV = motoR - 127;
+    int motoXV = motoX - 127;
+    int motoYV = motoY - 127;
+    int motoRV = motoR - 127;
     
-    int V1 = motoXV+motoRV;
-    int V2 = motoXV/-2 + ruto_3*motoYV/2 + motoRV;//V2,V3の計算(三平方の定理)
-    int V3 = motoXV/-2 - ruto_3*motoYV/2 + motoRV;
+    double V1 = (double)motoXV + (double)motoRV;
+    double V2 = -0.5*(double)motoXV + ruto_3*0.5*(double)motoYV + (double)motoRV;
+    double V3 = -0.5*(double)motoXV - ruto_3*0.5*(double)motoYV + (double)motoRV;
+    pc.printf("v1=%d,v2=%d,v3=%d",int(V1),int(V2),int(V3));
     led1 = 0;
     led2 = 0;
     led3 = 1;
     led4 = 0;
 }
-void Roboclaw_send(int V1,int V2,int V3){
-    MD.SpeedM1(ADRS1,V1*int(MAX_QPPS1));
-    MD.SpeedM2(ADRS2,V2*int(MAX_QPPS2));
-    MD.SpeedM1(ADRS2,V3*int(MAX_QPPS2));
+void Roboclaw_send(double V1, double V2, double V3){
+    MD.SpeedM1(ADRS1,int(V3*MAX_QPPS1));
+    MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1));
+    MD.SpeedM1(ADRS2,int(V1*MAX_QPPS2));
     led1 = 0;
     led2 = 0;
     led3 = 0;