Diff: main.cpp

Revision:

2:d84346eaa720

Parent:

1:5132f966db08

Child:

3:4c0c8046c3a7

--- a/main.cpp	Sat Oct 08 06:46:05 2022 +0000
+++ b/main.cpp	Sat Oct 08 14:33:24 2022 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "ikarashiMDC.h"
-#include "controller.h"
+#include "FEP_RX22.h"
 #include "pinconfig.h"
 #include "omni_wheel.h"
 #include "math.h"
@@ -9,13 +9,14 @@
  
 #define PI 3.141592653589
  
-Bcon mycon(fepTX, fepRX, fepad);
+FEP_RX22 mycon(fepTX, fepRX, fepad);
 Serial pc(pcTX, pcRX, 115200);
 Serial RS485(PC_10,PC_11,115200);
  
  
 uint8_t b[8];
 int16_t stick[4];
+int16_t trigger[4];
 double engine[4];
 double speed[6];
 double spin_power;
@@ -37,6 +38,7 @@
 /*プロトタイプ宣言*/
 void chassis();//足回りの制御・ジャイロ・テラターム
 void spin(double ang);//PID
+int  pm(double num); //正負判定
  
 int main(void){
     mycon.StartReceive();
@@ -61,6 +63,7 @@
 void chassis(){
         /*非常停止*/
         //stop = 1;
+        int TargetAngle = 0;
  
         /*ジャイロ読み取り*/
         posiX = posi.getX();
@@ -69,14 +72,16 @@
         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<4; i++) stick[i] = mycon.getStick(i);
-        
+        for (int i=0; i<4; i++){
+             stick[i] = mycon.getStick(i);
+             trigger[i] = mycon.getTrigger(i);
+        }
         for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
         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");
+        for (int i=0; i<4; i++) pc.printf("%3d ", trigger[i]);
+        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);
@@ -86,14 +91,14 @@
         /*全方位*/
         for(int i=0; i<4; i++){
             if(abs(stick[i]) > 10){
-            engine[i] = 0.4*(stick[i]/128.0);
+            engine[i] = 0.4*(stick[i]/128.0)*(trigger[1]/256.0);
             }else{
                 engine[i] = 0;
             }
         }
         /*PID*/
         /*スパゲッティコードで申し訳ないです...*/
-        if(abs(stick[2]) < 10){
+        if(abs(stick[2]) < 10){/*
         if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
             spin(0);
             pc.printf("a");
@@ -145,7 +150,21 @@
         }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){
             spin(-15);
         }else{
-        }
+        }*/
+        /*for(int i=-180;i<=180;i+=15){
+            if(((posiTheta>=i-7.5)&&(posiTheta<i+7.5))&&((posiTheta>i+1)||(posiTheta<i-1))){
+            spin(i);
+            }
+            else if((posiTheta == i) ||(posiTheta == i+1) ||(posiTheta == i-1)){
+            }
+        }*/
+            if(fabs(TargetAngle-posiTheta)>8){
+                TargetAngle += 15*pm(posiTheta-TargetAngle);
+                spin(TargetAngle);
+                if(abs(TargetAngle)==180){
+                    TargetAngle += -360*pm(TargetAngle);
+                }
+            }
         }
  
         /*旋回*/
@@ -165,10 +184,11 @@
 void spin(double ang)
 {
     angle.setSetPoint(ang);
-        stop = 1;
-        posiTheta = posi.getTheta()*(180.0/M_PI);
-        angle.setProcessValue(posiTheta);
-        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);射出機構とかのモーターが出てきたときに[//]を消す
-    
+    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);
+    //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す    
+}
+int pm(double num){
+    return((num>0)-(num<0));
 }
\ No newline at end of file