Diff: main.cpp

Revision:

3:4c0c8046c3a7

Parent:

2:d84346eaa720

Child:

4:82bc689e183e

--- a/main.cpp	Sat Oct 08 14:33:24 2022 +0000
+++ b/main.cpp	Sun Oct 09 05:26:55 2022 +0000
@@ -8,13 +8,14 @@
 #include "PID.h"
  
 #define PI 3.141592653589
+#define maxSpeed 0.4
  
 FEP_RX22 mycon(fepTX, fepRX, fepad);
 Serial pc(pcTX, pcRX, 115200);
 Serial RS485(PC_10,PC_11,115200);
  
  
-uint8_t b[8];
+uint8_t b[16];
 int16_t stick[4];
 int16_t trigger[4];
 double engine[4];
@@ -36,9 +37,9 @@
 PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
  
 /*プロトタイプ宣言*/
-void chassis();//足回りの制御・ジャイロ・テラターム
-void spin(double ang);//PID
-int  pm(double num); //正負判定
+void chassis();         //足回りの制御・ジャイロ・テラターム
+void spin(double ang);  //PID
+int  pm(double num);    //正負判定
  
 int main(void){
     mycon.StartReceive();
@@ -61,8 +62,7 @@
 }
  
 void chassis(){
-        /*非常停止*/
-        //stop = 1;
+        
         int TargetAngle = 0;
  
         /*ジャイロ読み取り*/
@@ -71,7 +71,7 @@
         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<8; i++) b[i] = mycon.getButton(i);     
+        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);
@@ -91,7 +91,12 @@
         /*全方位*/
         for(int i=0; i<4; i++){
             if(abs(stick[i]) > 10){
-            engine[i] = 0.4*(stick[i]/128.0)*(trigger[1]/256.0);
+                if(trigger[1]<10) trigger[1] = 0;
+                engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/256.0);
+            }else if(b[i]%2){
+                engine[0] = maxSpeed*pm(i-2);
+            }else if(b[i]%2==0){
+                engine[1] = -maxSpeed*pm(i-1);
             }else{
                 engine[i] = 0;
             }
@@ -186,7 +191,7 @@
     angle.setSetPoint(ang);
     posiTheta = posi.getTheta()*(180.0/M_PI);
     angle.setProcessValue(posiTheta);
-    pc.printf("/r/nang:%.2f pid:%.2f posi:%.2f\r\n",ang,-angle.compute(),posiTheta);
+    pc.printf("ang:%.2f pid:%.2f posi:%.2f\r\n",ang,-angle.compute(),posiTheta);
     //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す    
 }
 int pm(double num){