Diff: main.cpp

Revision:

0:3c88c8c7b112

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 25 09:01:31 2022 +0000
@@ -0,0 +1,181 @@
+#include "mbed.h"
+#include "utility.h"
+#define MATO 3
+#define PI 3.14159
+int fSpd;
+
+void startSpd()
+{
+    for(fSpd=0; fSpd<=160; fSpd+=5) {
+        wait_ms(15);
+    }
+}
+
+void dis() //距離測定関数-----------------------------------
+{
+    pc.baud(115200);
+    //的との位置を把握
+    int a = 3;
+    while(matoDis[1]==0 || matoDis[2]==0 || matoDis[3]==0) {
+        while(1) {
+            move(fSpd,180);    //ひだり
+            usensor.start();
+            dist = usensor.get_dist_cm();
+            wait_ms(10);
+            /*for(int i=0; i<4; i++) {
+                pc.printf("motor[%d] : %f   ",i, motor[i]);
+            }
+            pc.printf("\r\n");*/
+
+            if(dist < kyoriSiki && dist != 0) {
+                matoDis[a] = dist;//的との距離
+                pointX[a] = -cm_X - pointSum;//的がある場所のX方向の距離
+                pointSum += pointX[a];
+                oto(10);
+                //  pointX[a]=-cm_X;
+                //  cerica.ResetX();
+                pc.printf("matoDis[%d]:%f\r\n",a,matoDis[a]);
+                pc.printf("pointX[%d]:%f\r\n",a,pointX[a]);
+
+                while(1) {
+bbb:
+                    move(fSpd,180);    //ひだり
+                    usensor.start();
+                    dist = usensor.get_dist_cm();
+                    wait_ms(10);
+                    if(dist>kyoriSiki) {
+                        while(1) {
+                            move(fSpd,180);    //ひだり
+                            usensor.start();
+                            dist = usensor.get_dist_cm();
+                            wait_ms(10);
+                            if(dist<kyoriSiki && dist!=0) {
+                                goto bbb;
+                            }
+                            a--;
+                            pc.printf("a=%d\n\r",a);
+                            goto aaa;
+                        }
+                    }
+                }
+            }
+        }
+aaa:
+        // pc.printf("matoDis[%d]:%f\r\n",a-1,matoDis[a-1]);
+    }
+    a=1;
+    wait_ms(300);  //測り終わった後 秒進む（要調整）
+    stop();
+    pointX[0]=-cm_X - pointSum;//kokokokokokokokoko
+    //pointX[0]=-cm_X;
+    for(int i=0; i<=MATO; i++) {
+        pc.printf("matoDis[%d]:%f\r\n",i,matoDis[i]);
+        pc.printf("pointX[%d]:%f\r\n",i,pointX[i]);
+    }
+    wait_ms(1000);
+
+    //回転
+    rotation(80,170);
+    stop();
+    encMode=1;
+    cerica.ResetX();
+    cerica.ResetY();
+
+
+    //角度とか
+    angle[0]=atan((matoDis[1]-reach)/pointX[0]);
+    for(int i=1; i<MATO; i++) {
+        angle[i]=atan((matoDis[i+1]-matoDis[i])/pointX[i]);
+    }
+    angle[MATO]=-atan((matoDis[MATO]-reach)/pointX[MATO]);
+
+    pc.printf("------------------------\r\n");
+    for(int i=0; i<=MATO; i++) {
+        pc.printf("matoDis[%d]:%f\n\r",i,matoDis[i]);
+    }
+    pc.printf("------------------------\r\n");
+
+
+    for(int i=0; i<=MATO; i++) {
+        pc.printf("pointX[%d]:%f\n\r",i,pointX[i]);
+    }
+    //反転のずれ直す
+    for(int i=0; i<=MATO; i++) {
+        angle[i]=180/PI*(angle[i]+PI);
+        pc.printf("angle[%d]:%f\n\r",i,angle[i]);
+    }
+    pointX[0]-=85;
+    pointX[3]-=70;
+
+
+    //進む距離
+    //pc.printf("nakami:%d\r\n",(matoDis[1]-reach)*(matoDis[1]-reach)+pointX[0]*pointX[0]);
+    nnmDis[0]=sqrt((matoDis[1]-reach)*(matoDis[1]-reach)+pointX[0]*pointX[0]);
+    for(int i=1; i<MATO; i++) {
+        nnmDis[i]=sqrt((matoDis[i]-matoDis[i+1])*(matoDis[i]-matoDis[i+1])+pointX[i]*pointX[i]);
+    }
+    nnmDis[MATO]=sqrt((matoDis[MATO]-reach)*(matoDis[MATO]-reach)+pointX[MATO]*pointX[MATO]);
+
+    for(int i=0; i<=MATO; i++) {
+        // nnmDis[i]=180/PI*angle[i];
+        pc.printf("nnmDis[%d]:%f\n\r",i,nnmDis[i]);
+    }
+
+}
+
+
+//main------------------------------
+int main()
+{
+    mpu.init();
+    cerica.ResetY();
+    cerica.ResetX();
+
+    Thread move;
+    move.start(signal);
+
+    Thread m;
+    m.start(mpuR);
+
+    Thread enc;
+    enc.start(encoder);
+    
+    Thread bow;
+    bow.start(selfShot);
+
+    Thread wireless;
+    wireless.start(send);
+    
+    Thread slow;
+    
+    boardInit();
+
+    oto(20);
+
+    pc.printf("start------------------------\r\n");
+    //合体
+    moveEnc(100,-90,70);    //本番距離変える
+    wait_ms(1500);
+    pc.printf("Left move");
+    //x方向に移動しながら距離測定
+    slow.start(startSpd);
+    dis();
+    wait_ms(1000);
+    //じぐざくいどう
+    for(int i=0; i<=3; i++) {
+        moveEnc(140,angle[i],nnmDis[i]);
+        sendTiming = 1;
+        stop();
+        //撃つタイミング送信
+        wait_ms(1000);
+    }
+
+    for(int i=0; i<4; i++) {
+        pc.printf("motor[%d] : 0\n\r",i);
+        cerica.Motor(i,0);
+    }
+    for(int i=0; i<3; i++) {
+        pc.printf("matoDis[%d]:%f\r\n",i,matoDis[i]);
+    }
+}
+