Diff: main.cpp

Revision:

1:5132f966db08

Parent:

0:5d4705b2893c

Child:

2:d84346eaa720

--- a/main.cpp	Fri Oct 07 10:25:24 2022 +0000
+++ b/main.cpp	Sat Oct 08 06:46:05 2022 +0000
@@ -6,21 +6,21 @@
 #include "math.h"
 #include "OmniPosition.h"
 #include "PID.h"
-
+ 
 #define PI 3.141592653589
-
+ 
 Bcon mycon(fepTX, fepRX, fepad);
 Serial pc(pcTX, pcRX, 115200);
 Serial RS485(PC_10,PC_11,115200);
-
-
+ 
+ 
 uint8_t b[8];
 int16_t stick[4];
 double engine[4];
 double speed[6];
 double spin_power;
 double posiX , posiY , posiTheta;
-
+ 
 DigitalOut stop(A3);
 ikarashiMDC motor[] = {
     ikarashiMDC(0,0,SM,&RS485),
@@ -28,19 +28,19 @@
     ikarashiMDC(0,2,SM,&RS485),
     ikarashiMDC(0,3,SM,&RS485),
 };
-
+ 
 OmniWheel omni(4);
 OmniPosition posi(mwTX, mwRX);
-
+ 
 PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
-
+ 
 /*プロトタイプ宣言*/
 void chassis();//足回りの制御・ジャイロ・テラターム
 void spin(double ang);//PID
-
+ 
 int main(void){
     mycon.StartReceive();
-
+ 
     omni.wheel[0].setRadian(PI * 1.0 / 4.0);
     omni.wheel[1].setRadian(PI * 3.0 / 4.0);
     omni.wheel[2].setRadian(PI * 5.0 / 4.0);
@@ -57,11 +57,11 @@
     }
     
 }
-
+ 
 void chassis(){
         /*非常停止*/
         //stop = 1;
-
+ 
         /*ジャイロ読み取り*/
         posiX = posi.getX();
         posiY = posi.getY();
@@ -75,17 +75,14 @@
         pc.printf(" | ");
         for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
         pc.printf(" | ");
+        pc.printf("spinning... %.2f|%.3f",posiTheta,-angle.compute());
         if (mycon.status) pc.printf("received\r\n");
-        else pc.printf("anything error...\r\n");
-
-        for(int i=0; i<4; i++){
-            speed[i] = 0;
+        else{ pc.printf("anything error...\r\n");
+            for( int i=0; i<4; i++){
+                motor[i].setSpeed(0);
+            }
         }
         
-        /*notcontoroler*/
-        for(int i=0; i<4; i++){
-            motor[i].setSpeed(0);
-        }
         /*全方位*/
         for(int i=0; i<4; i++){
             if(abs(stick[i]) > 10){
@@ -94,26 +91,15 @@
                 engine[i] = 0;
             }
         }
-
-        /*旋回*/
-        if(abs(stick[2]) > 10){
-            spin_power = engine[2];
-        }else{
-            spin_power = 0;
-        }
-        
-        omni.computeXY(engine[0],engine[1],-spin_power);
-        for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
-
-
-        for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
         /*PID*/
         /*スパゲッティコードで申し訳ないです...*/
         if(abs(stick[2]) < 10){
         if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
             spin(0);
+            pc.printf("a");
         }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
             spin(15);
+            pc.printf("b");
         }else if(((posiTheta>=22.5)&&(posiTheta<37.5))&&((posiTheta>31)||(posiTheta<29))){
             spin(30);
         }else if(((posiTheta>=37.5)&&(posiTheta<52.5))&&((posiTheta>46)||(posiTheta<44))){
@@ -161,17 +147,28 @@
         }else{
         }
         }
+ 
+        /*旋回*/
+        if(abs(stick[2]) > 10){
+            spin_power = engine[2];
+        }else{
+            spin_power = angle.compute();
+        }
+        
+        omni.computeXY(engine[0],engine[1],-spin_power);
+        for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
+ 
+ 
+        for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
+        
 }
 void spin(double ang)
 {
     angle.setSetPoint(ang);
-        while(1) {
         stop = 1;
         posiTheta = posi.getTheta()*(180.0/M_PI);
         angle.setProcessValue(posiTheta);
-        pc.printf("spinning... |%d|%.2f|%.3f\r\n",ang,posiTheta,-angle.compute());
-        for(int i=0; i<4; i++) motor[i].setSpeed(-angle.compute());
+        pc.printf("ang:%.2f fabs:%.2f posi:%.2f\r\n",ang,fabs(ang-posiTheta),posiTheta);
         //for(int i=4; i<8; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
-        if ((fabs(ang - posiTheta) < 1)||(fabs(ang - posiTheta)> 7.5)) return;
-    }
+    
 }
\ No newline at end of file