Diff: main.cpp

Revision:

10:886268a42090

Parent:

9:8dd49bc48d59

Child:

11:10d3dc96c469

diff -r 8dd49bc48d59 -r 886268a42090 main.cpp
--- a/main.cpp	Mon Oct 10 11:03:17 2022 +0000
+++ b/main.cpp	Tue Oct 11 08:25:15 2022 +0000
@@ -62,6 +62,9 @@
 
 int TargetAngle = 0;
 
+int sum_1 = 0;
+int sum_2 = 0;
+
 double bldcspeed = 0.8;
 
 int16_t angle[4] = {0};//エンコーダ受信格納
@@ -73,7 +76,7 @@
 PID angl(10.0, 5.0, 0.0000005, 0.01);
  
 //プロトタイプ宣言
-void chassis();         //足回りの制御・ジャイロ・テラターム
+void chassis();         //機体自体の制御
 void spin(double ang);  //PID
 int  pm(double num);    //正負判定
  
@@ -127,7 +130,7 @@
         pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
         
         mycon.getData(data);
-        for (int i=0, tmp=1; i<8; i++) {
+/*        for (int i=0, tmp=1; i<8; i++) {
             pw = pow((float)2,i);
             b[i] = (int)((data[0] & tmp)/pw);
             pc.printf("%d ", b[i]);
@@ -139,6 +142,28 @@
             pc.printf("%d ", b[i]);
             tmp *= 2;
         }
+*/
+        for (int i=0, tmp=1; i<8; i++) {
+            pw = pow((float)2,i);
+            b[i] = (int)((data[0] & tmp)/pw);
+//          pc.printf("%d ", b[i]);          上のポインタの式が分からんので無理やり10進数に変える
+            sum_1 += b[i]*pow((float)2,i+1);
+            tmp *= 2;
+        }
+        pc.printf("%3d ",sum_1);
+        /*初期化*/
+        sum_1 = 0;
+        
+        for (int i=8, tmp=1, j=0; i<16; i++, j++) {
+            pw = pow((float)2,j);
+            b[i] = (int)((data[1] & tmp)/pw);
+//          pc.printf("%d ", b[i]);             
+            sum_2 += b[i]*pow((float)2,i-7);
+            tmp *= 2;
+        }
+        pc.printf("%3d ",sum_2);
+        /*初期化*/
+        sum_2 = 0;
         pc.printf(" | ");
         
         for (int i=0; i<4; i++) {
@@ -178,7 +203,10 @@
         for(int i=0,j=0;i<4;i++,j+=2){
             angle[i] = pulse[j] << 8 | pulse[j+1];  
         }
-        pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);            
+        pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);
+        if(abs(stick[2])<10){
+            pc.printf("TA:%.2f pid:%.2f T-p:%.2f",TargetAngle,-angl.compute(),TargetAngle-posiTheta);
+        }
         pc.printf("\r\n");
  
         //ブラシレスモーター
@@ -299,7 +327,7 @@
     posiTheta = posi.getTheta()*(180.0/M_PI);
     angl.setProcessValue(posiTheta);
     //for(int i=4; i<12; i++) motor[i].setSpeed(0);旋回時以外はPID判定なのでsetSpeed(0)だと機構が動かなくなるので多分このコードいらないです。
-    pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angl.compute(),posiTheta,TargetAngle-posiTheta);
+    //pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angl.compute(),posiTheta,TargetAngle-posiTheta);
   
 }
 int pm(double num){