Diff: main.cpp

Revision:

0:5d4705b2893c

Child:

1:5132f966db08

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 07 10:25:24 2022 +0000
@@ -0,0 +1,177 @@
+#include "mbed.h"
+#include "ikarashiMDC.h"
+#include "controller.h"
+#include "pinconfig.h"
+#include "omni_wheel.h"
+#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),
+    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 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 = 1;
+        chassis();
+    }
+    
+}
+
+void chassis(){
+        /*非常停止*/
+        //stop = 1;
+
+        /*ジャイロ読み取り*/
+        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);
+        /*コントローラー受信*/
+        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<8; i++) pc.printf("%d ", b[i]);
+        pc.printf(" | ");
+        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
+        pc.printf(" | ");
+        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;
+        }
+        
+        /*notcontoroler*/
+        for(int i=0; i<4; i++){
+            motor[i].setSpeed(0);
+        }
+        /*全方位*/
+        for(int i=0; i<4; i++){
+            if(abs(stick[i]) > 10){
+            engine[i] = 0.4*(stick[i]/128.0);
+            }else{
+                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);
+        }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
+            spin(15);
+        }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))){
+            spin(45);
+        }else if(((posiTheta>=52.5)&&(posiTheta<67.5))&&((posiTheta>61)||(posiTheta<59))){
+            spin(60);
+        }else if(((posiTheta>=67.5)&&(posiTheta<82.5))&&((posiTheta>76)||(posiTheta<74))){
+            spin(75);
+        }else if(((posiTheta>=82.5)&&(posiTheta<97.5))&&((posiTheta>91)||(posiTheta<89))){
+            spin(90);
+        }else if(((posiTheta>=97.5)&&(posiTheta<112.5))&&((posiTheta>106)||(posiTheta<104))){
+            spin(105);
+        }else if(((posiTheta>=112.5)&&(posiTheta<127.5))&&((posiTheta>121)||(posiTheta<119))){
+            spin(120);
+        }else if(((posiTheta>=127.5)&&(posiTheta<142.5))&&((posiTheta>136)||(posiTheta<134))){
+            spin(135);
+        }else if(((posiTheta>=142.5)&&(posiTheta<157.5))&&((posiTheta>151)||(posiTheta<149))){
+            spin(150);
+        }else if(((posiTheta>=157.5)&&(posiTheta<172.5))&&((posiTheta>166)||(posiTheta<164))){
+            spin(165);
+        }else if(((posiTheta>=172.5)&&(posiTheta<=179))||((posiTheta<=-172.5)&&(posiTheta>=-179))){
+            spin(180);
+        }else if(((posiTheta<=-157.5)&&(posiTheta>-172.5))&&((posiTheta>-164)||(posiTheta<-166))){
+            spin(-165);
+        }else if(((posiTheta<=-142.5)&&(posiTheta>-157.5))&&((posiTheta>-149)||(posiTheta<-151))){
+            spin(-150);
+        }else if(((posiTheta<=-127.5)&&(posiTheta>-142.5))&&((posiTheta>-134)||(posiTheta<-136))){
+            spin(-135);
+        }else if(((posiTheta<=-112.5)&&(posiTheta>-127.5))&&((posiTheta>-119)||(posiTheta<-121))){
+            spin(-120);
+        }else if(((posiTheta<=-97.5)&&(posiTheta>-112.5))&&((posiTheta>-104)||(posiTheta<-106))){
+            spin(-105);
+        }else if(((posiTheta<=-82.5)&&(posiTheta>-90.5))&&((posiTheta>-89)||(posiTheta<-91))){
+            spin(-90);
+        }else if(((posiTheta<=-67.5)&&(posiTheta>-82.5))&&((posiTheta>-74)||(posiTheta<-76))){
+            spin(-75);
+        }else if(((posiTheta<=-52.5)&&(posiTheta>-67.5))&&((posiTheta>-59)||(posiTheta<-61))){
+            spin(-60);
+        }else if(((posiTheta<=-37.5)&&(posiTheta>-52.5))&&((posiTheta>-44)||(posiTheta<-46))){
+            spin(-45);
+        }else if(((posiTheta<=-22.5)&&(posiTheta>-37.5))&&((posiTheta>-29)||(posiTheta<-30))){
+            spin(-30);
+        }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){
+            spin(-15);
+        }else{
+        }
+        }
+}
+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());
+        //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