Diff: main.cpp

Revision:

6:d4b82ba4836a

Parent:

5:885bffdceaa2

Child:

7:778eaeae8128

--- a/main.cpp	Sun Oct 09 06:34:58 2022 +0000
+++ b/main.cpp	Sun Oct 09 10:17:42 2022 +0000
@@ -18,12 +18,18 @@
 uint8_t b[16];
 int16_t stick[4];
 int16_t trigger[4];
+int16_t volume[3];
+uint8_t toggle[4];
+uint8_t timeout;
+uint8_t data[128];
+int pw;
 double engine[4];
 double speed[6];
 double spin_power;
 double posiX , posiY , posiTheta;
- 
-DigitalOut stop(A3);
+int TargetAngle = 0;
+int StartAngle = 0;
+DigitalOut stop(PA_5);
 ikarashiMDC motor[] = {
     ikarashiMDC(0,0,SM,&RS485),
     ikarashiMDC(0,1,SM,&RS485),
@@ -55,7 +61,7 @@
     angle.setMode(1);
     angle.setSetPoint(0);
     while(1){
-        stop = 1;
+        stop = toggle[0];
         chassis();
     }
     
@@ -63,7 +69,6 @@
  
 void chassis(){
         
-        int TargetAngle = 0;
  
         /*ジャイロ読み取り*/
         posiX = posi.getX();
@@ -71,40 +76,59 @@
         posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
         pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
         /*コントローラー受信*/
-        for (int i=0; i<16; i++) b[i] = mycon.getButton(i);     
-        for (int i=0; i<4; i++){
-             stick[i] = mycon.getStick(i);
-             trigger[i] = mycon.getTrigger(i);
+        mycon.getData(data);
+        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]);
+            tmp *= 2;
+        }
+        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]);
+            tmp *= 2;
+        }
+        pc.printf(" | ");
+        
+        for (int i=0; i<4; i++) {
+            stick[i] = data[i+2];
+            pc.printf("%3d ", stick[i]);
         }
-        for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
+        pc.printf(" | ");
+        
+        for (int i=0; i<2; i++) {
+            trigger[i] = data[i+6];
+            pc.printf("%3d ", trigger[i]);
+        }
         pc.printf(" | ");
-        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
+        
+        for (int i=0; i<3; i++) {
+            volume[i] = data[i+9];
+            pc.printf("%3d ", volume[i]);
+        }
         pc.printf(" | ");
-        for (int i=0; i<4; i++) pc.printf("%3d ", trigger[i]);
+        
+        for (int i=0; i<4; i++) {
+            toggle[i] = data[i+12];
+            pc.printf("%d ", toggle[i]);
+        }
+        pc.printf(" | ");
+        
+        timeout = data[8];
+        pc.printf("%3d ", timeout);
+        pc.printf(" | ");
+        
         if(mycon.getStatus()) pc.printf("received\r\n");
         else{ pc.printf("anything error...\r\n");
             for( int i=0; i<4; i++){
                 motor[i].setSpeed(0);
             }
         }
-        
-        /*全方位*/
-        for(int i=0; i<4; i++){
-            if(abs(stick[i]) > 10){
-                if(trigger[1]<10) trigger[1] = 0;
-                engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
-            }else if(b[i]%2){
-                if(trigger[1]<10) trigger[1] = 0;
-                //b[1]のとき左向き（負）b[3]のとき右向き（正）
-                engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
-            }else if(b[i]%2==0){
-                if(trigger[1]<10) trigger[1] = 0;
-                //b[0]のとき上向き（正）b[2]のとき下向き（負）
-                engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
-            }else{
-                engine[i] = 0;
-            }
+        for(int i=0;i<4;i++){
+            stick[i] = stick[i] - 128;
         }
+
         /*PID*/
         /*スパゲッティコードで申し訳ないです...*/
         if(abs(stick[2]) < 10){/*
@@ -167,15 +191,39 @@
             else if((posiTheta == i) ||(posiTheta == i+1) ||(posiTheta == i-1)){
             }
         }*/
+//            if(fabs(posiTheta)<8){
+//                spin(StartAngle);
+//            }
+//            if(fabs(TargetAngle-posiTheta)<8){
+//                spin(TargetAngle);
+//                }
             if(fabs(TargetAngle-posiTheta)>8){
                 TargetAngle += 15*pm(posiTheta-TargetAngle);
                 if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
                     TargetAngle += -360*pm(TargetAngle);
                 }
-                spin(TargetAngle);
             }
+            spin(TargetAngle);
         }
- 
+        /*全方位*/
+        for(int i=0; i<4; i++){
+            if(abs(stick[i]) > 10){
+                if(trigger[1]<10) trigger[1] = 0;
+                engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
+            }else if(b[i]%2){
+                if(trigger[1]<10) trigger[1] = 0;
+                //b[1]のとき左向き（負）b[3]のとき右向き（正）
+                engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
+                engine[1] = 0;
+            }else if(b[i]%2==0){
+                if(trigger[1]<10) trigger[1] = 0;
+                //b[0]のとき上向き（正）b[2]のとき下向き（負）
+                engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
+                engine[0] = 0;
+            }else{
+                engine[i] = 0;
+                }
+        }
         /*旋回*/
         if(abs(stick[2]) > 10){
             spin_power = engine[2];
@@ -195,7 +243,7 @@
     angle.setSetPoint(ang);
     posiTheta = posi.getTheta()*(180.0/M_PI);
     angle.setProcessValue(posiTheta);
-    pc.printf("ang:%.2f pid:%.2f posi:%.2f\r\n",ang,-angle.compute(),posiTheta);
+    pc.printf("ang:%.2f pid:%.2f    posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
     //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す    
 }
 int pm(double num){