Diff: main.cpp

Revision:

1:31f47d3fa8cd

Parent:

0:b0ca7b23bdb5

Child:

2:f856cbeb5940

--- a/main.cpp	Sun Oct 09 08:59:22 2022 +0000
+++ b/main.cpp	Mon Oct 10 05:43:42 2022 +0000
@@ -12,6 +12,7 @@
 #include "cmath"
 
 #define PI 3.141592653589
+#define maxSpeed 0.4
 
 DigitalIn userButton(USER_BUTTON);
 AnalogIn VOLUME(A0);
@@ -33,46 +34,72 @@
     ikarashiMDC(0,3,SM,&RS485), //asi
     ikarashiMDC(1,0,SM,&RS485), //全体昇降1
     ikarashiMDC(1,1,SM,&RS485), //全体昇降2
-    ikarashiMDC(1,2,SM,&RS485), //井上左昇降
-    ikarashiMDC(1,3,SM,&RS485), //井上右昇降
-    ikarashiMDC(2,0,SM,&RS485),
-    ikarashiMDC(2,1,SM,&RS485),
-    ikarashiMDC(2,2,SM,&RS485),
-    ikarashiMDC(2,3,SM,&RS485),
+    ikarashiMDC(1,2,SM,&RS485), //フジモン機構
+    ikarashiMDC(1,3,SM,&RS485), //フジモン機構
+    ikarashiMDC(2,0,SM,&RS485), //井上左昇降
+    ikarashiMDC(2,1,SM,&RS485), //井上右昇降
+    ikarashiMDC(2,2,SM,&RS485), //井上左前後
+    ikarashiMDC(2,3,SM,&RS485), //井上右前後
 };
 
 Servo pwm_imput1(BLDC1);//ブラシレス宣言
 Servo pwm_imput2(BLDC2);
 Servo pwm_imput3(BLDC3);
 
-OmniWheel omni(4);//足回り宣言
+//abe 足回り
+double engine[4];
+double spin_power;
+double posiX , posiY , posiTheta;
+int TargetAngle = 0;
+int StartAngle = 0;
+
+OmniWheel omni(4);
 OmniPosition posi(sub1TX, sub1RX);
 
+PID angle(10.0, 5.0, 0.0000005, 0.01);
+
+//プロトタイプ宣言
+void chassis();         //足回りの制御・ジャイロ・テラターム
+void spin(double ang);  //PID
+int  pm(double num);    //正負判定
+
 Timer t;
 
+int16_t trigger[4];
+uint8_t b[16];
+int16_t stick[4];
+uint8_t data[128];
+int pw;
+
+double speed[12] = {0};
+
 int main() 
 {
     t.start();
-    
-    double bldcspeed = 0.8;
+
+    int16_t volume[3];
+    uint8_t toggle[4];
+    uint8_t timeout;
     
-//    double spin_power;       //足回り宣言
-//    double posiX , posiY , posiTheta;
+    double bldcspeed = 0.6;
     
     rcv.setHeaders(0xff,0xff);
     rcv.startReceive(4); //SerialMultiByte受信
     
     mycon.StartReceive(); //コントローラー受信・宣言
-    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[12] = {0};
+    //足回り宣言
+    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);
+
     
     int16_t angle[4] = {0};//エンコーダ受信格納
     uint8_t pulse[8] = {0};
@@ -81,73 +108,60 @@
         {
         stop = toggle[0];
         
-        rcv.getData(pulse); //エンコーダ受信
-        for(int i=0,j=0;i<4;i++,j+=2){
-            angle[i] = pulse[j] << 8 + pulse[j+1];  
-        }
-        pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);            
-        pc.printf("\r\n");
-         
-#if ControllerMode   
-        for (int i=0; i<16; i++) b[i] = mycon.getButton(i);
-        for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
-        for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i);
-        
-//        for (int i=0; i<16; i++) pc.printf("%d ", b[i]);
-//        pc.printf(" | ");
-//        for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
-//        pc.printf(" | ");
-//        for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]);
-//        pc.printf(" | ");
-#else
-        mycon.getData(data);
+        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]);
+//            pc.printf("%d ", b[i]);
             tmp *= 2;
         }
         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]);
+//            pc.printf("%d ", b[i]);
             tmp *= 2;
         }
-        pc.printf(" | ");
-        
+//        pc.printf(" | ");
         for (int i=0; i<4; i++) {
             stick[i] = data[i+2];
-            pc.printf("%3d ", stick[i]);
+//            pc.printf("%3d ", stick[i]);
         }
-        pc.printf(" | ");
-        
+//        pc.printf(" | ");
         for (int i=0; i<2; i++) {
             trigger[i] = data[i+6];
-            pc.printf("%3d ", trigger[i]);
+//            pc.printf("%3d ", trigger[i]);
         }
-        pc.printf(" | ");
-        
+//        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("%3d ", toggle[i]);
+//            pc.printf("%3d ", volume[i]);
         }
         pc.printf(" | ");
-        
+        for (int i=0; i<4; i++) {
+            toggle[i] = data[i+12];
+//            pc.printf("%3d ", toggle[i]);
+        }
+//        pc.printf(" | ");
         timeout = data[8];
         pc.printf("%3d ", timeout);
-        pc.printf(" | ");
+//        pc.printf(" | ");
+        if (mycon.getStatus()) pc.printf("receive");
+        else{pc.printf("anything error..."); 
+                for( int i=0; i<12; i++){
+                motor[i].setSpeed(0);
+            }
+        }
         
         
-#endif
-        if (mycon.getStatus()) pc.printf("receive");
-        else pc.printf("anything error..."); 
-    
+        /*阿部君の素晴らしい天才的な足回り関数*/
+        chassis();
+        
+        rcv.getData(pulse); //エンコーダ受信
+        for(int i=0,j=0;i<4;i++,j+=2){
+            angle[i] = pulse[j] << 8 | pulse[j+1];  
+        }
+        pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);            
+        pc.printf("\r\n");
 
         //ブラシレスモーター
         pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
@@ -158,34 +172,136 @@
 //        pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
 //                  ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
         
+        /*井上機構ON,OFF*/
+        if(toggle[1]){
+            speed[10] = -0.9;
+        }else{
+            speed[10] = 0;
+        }
+        if(toggle[1] && b[15]){
+            speed[10] = 0.4;
+        }
+        
+        if(toggle[3]){
+            speed[11] = -0.9;
+        }else{
+            speed[11] = 0;
+        }
+        if(toggle[3] && b[15]){
+            speed[11] = 0.4;
+        }
+        
+        /*フジモン機構*/
+        if(toggle[2]){
+            speed[6] = 0.6;
+            speed[7] = 0.6;   
+        }else{
+            speed[6] = 0;
+            speed[7] = 0;            
+        }
+        
         /*展開昇降*/
         if(b[5] != 0){
-            speed[4] = 0.3;
-            speed[5] = 0.3;
+            speed[4] = 0.5;
+            speed[5] = 0.5;
         }else if(b[4] != 0){
-            speed[4] = -0.3;
-            speed[5] = -0.3;
+            speed[4] = -0.35;
+            speed[5] = -0.35;
         }else{
             speed[4] = 0;
             speed[5] = 0;
         }
         //機構昇降
-        if(b[9] != 0){
-            speed[6] = 0.3;
-        }else if(b[11] != 0){
-            speed[7] = 0.3;
-        }else if(b[13] != 0){    
-            speed[6] = -0.3;
-        }else if(b[14] != 0){    
-            speed[7] = -0.3;
+        if(b[9]){
+            speed[8] = -0.35;
+        }else
+        if(b[13]){    
+            speed[8] = 0.4;
+        }
+        if(b[11]){
+            speed[9] = -0.35;
+        }else
+        if(b[14]){    
+            speed[9] = 0.4;
+        }
+        if((b[9]!=1) && (b[13]!=1)){
+            speed[8]=0;
+        }
+        if((b[11]!=1) && (b[14]!=1)){
+            speed[9]=0;
+        }
+        for(int i=0; i<12; i++) pc.printf("%.2f ",speed[i]);
+        for(int i=4; i<12; i++) motor[i].setSpeed(speed[i]);
+    }
+        
+}        
+
+void chassis(){
+    
+        mycon.getData(data);
+        for (int i=0; i<4; i++) {
+            stick[i] = data[i+2];
+        }
+
+        /*ジャイロ読み取り*/
+        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);
+
+        if(abs(stick[2]) < 10){
+            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){
+                if(trigger[1]<10) trigger[1] = 0;
+                engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
+            }else if(b[i]%2){
+                if(trigger[1]<10) trigger[1] = 0;
+                //b[1]のとき左向き（負）b[3]のとき右向き（正）
+                engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
+                engine[1] = 0;
+            }else if(b[i]%2==0){
+                if(trigger[1]<10) trigger[1] = 0;
+                //b[0]のとき上向き（正）b[2]のとき下向き（負）
+                engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
+                engine[0] = 0;
+            }else{
+                engine[i] = 0;
+                }
+        }
+        /*旋回*/
+        if(abs(stick[2]) > 10){
+            spin_power = engine[2];
         }else{
-            speed[6] = 0;
-            speed[7] = 0;
+            spin_power = angle.compute();
         }
         
-        for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);
-        
+        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]);
         
-    }
-        
-}        
\ No newline at end of file
+}
+
+
+void spin(double ang)
+{
+    angle.setSetPoint(ang);
+    posiTheta = posi.getTheta()*(180.0/M_PI);
+    angle.setProcessValue(posiTheta);
+    pc.printf("ang:%.2f pid:%.2f    posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-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