Diff: main.cpp

Revision:

0:b7dbdc0b19f3

diff -r 000000000000 -r b7dbdc0b19f3 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 10 08:01:10 2022 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "FEP_RX22.h"
+#include "pinconfig.h"
+#include "omni_wheel.h"
+#include "math.h"
+#include "OmniPosition.h"
+#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[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 speed[6];
+double engine[4];
+double spin_power;
+double posiX , posiY , posiTheta;
+int TargetAngle = 0;
+int StartAngle = 0;
+int sum_1 = 0;
+int sum_2 = 0;
+DigitalOut stop(PA_5);
+ikarashiMDC motor[] = {
+    ikarashiMDC(0,0,SM,&RS485),
+    ikarashiMDC(0,1,SM,&RS485),
+    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  pm(double num);    //正負判定
+ 
+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);
+    omni.wheel[3].setRadian(PI * 7.0 / 4.0);
+    
+    angle.setInputLimits(-180,180);
+    angle.setOutputLimits(-0.4,0.4);
+    angle.setBias(0);
+    angle.setMode(1);
+    angle.setSetPoint(0);
+    while(1){
+        stop = toggle[0];
+        chassis();
+    }
+    
+}
+ 
+void chassis(){
+        
+ 
+        /*ジャイロ読み取り*/
+        posiX = posi.getX();
+        posiY = posi.getY();
+        posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
+        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++) {
+            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);
+        pc.printf(" | ");
+        /*初期化*/
+        sum_2 = 0;
+        pc.printf(" | ");
+        
+        for (int i=0; i<4; i++) {
+            stick[i] = (data[i+2] - 128)*(-1);
+            pc.printf("%3d ", stick[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<3; i++) {
+            volume[i] = data[i+9];
+            pc.printf("%3d ", volume[i]);
+        }
+        pc.printf(" | ");
+        
+        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);
+            }
+        }
+ 
+        /*PID*/
+        if(abs(stick[2]) < 10){
+        /*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);
+                if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
+                    TargetAngle += -360*pm(TargetAngle);
+                }
+            }
+            spin(TargetAngle);
+        }
+        /*全方位*/
+        for(int i=0; i<4; i++){
+                if(abs(stick[i]) > 10){
+                    engine[i] = maxSpeed*(stick[i]/128.0);
+                }else if(b[0]){
+                    engine[1] = maxSpeed*(trigger[0]/225.0);
+                    engine[0] = 0;
+                }else if(b[1]){
+                    engine[0] = -maxSpeed*(trigger[0]/225.0);
+                    engine[1] = 0;
+                }else if(b[2]){
+                    engine[1] = -maxSpeed*(trigger[0]/225.0);
+                    engine[0] = 0;
+                }else if(b[3]){
+                    engine[0] = maxSpeed*(trigger[0]/255.0);
+                    engine[1] = 0;
+                }else{
+                    engine[i] = 0;
+                }
+        }
+        /*旋回*/
+        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);
+    posiTheta = posi.getTheta()*(180.0/M_PI);
+    angle.setProcessValue(posiTheta);
+    //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す  
+    pc.printf("ang:%.2f pid:%.2f    posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
+  
+}
+int pm(double num){
+    return((num>0)-(num<0));
+}
+            
\ No newline at end of file