Revision 0:3c88c8c7b112, committed 2022-04-25

Comitter:

GOD_NAFU

Date:

Mon Apr 25 09:01:31 2022 +0000

Commit message:

2021A_umaRobo

Changed in this revision

diff -r 000000000000 -r 3c88c8c7b112 CERICA_2019.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CERICA_2019.lib	Mon Apr 25 09:01:31 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NIT-Fukui/code/CERICA_2019/#bba1ff6d196e

diff -r 000000000000 -r 3c88c8c7b112 HCSR04.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Mon Apr 25 09:01:31 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/prabhuvd/code/HCSR04/#71da0dbf4400

diff -r 000000000000 -r 3c88c8c7b112 Serial6050Yaw.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Serial6050Yaw.lib	Mon Apr 25 09:01:31 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NIT-Fukui/code/Serial6050Yaw/#413d60ebdcf2

diff -r 000000000000 -r 3c88c8c7b112 main.cpp
--- /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]);
+    }
+}
+

diff -r 000000000000 -r 3c88c8c7b112 mbed-os.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Mon Apr 25 09:01:31 2022 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#c9e63f14085f5751ff5ead79a7c0382d50a813a2

diff -r 000000000000 -r 3c88c8c7b112 utility.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/utility.h	Mon Apr 25 09:01:31 2022 +0000
@@ -0,0 +1,335 @@
+#include "hcsr04.h"
+#include "board.h"
+#include "Serial6050.h"
+#define PI 3.14159265359
+#define MATO 3
+
+CERICA cerica(FEP01);
+Serial6050 mpu(PC_10, PC_11, PC_12);
+RawSerial pc(USBTX, USBRX, 115200);
+//I2C i2c(PB_9,PB_8);
+
+DigitalOut debugLED(D13);
+DigitalIn button(PC_13);
+
+HCSR04 usensor(D4, D6); // 超音波センサ Trigger(DO), Echo(PWMIN)
+
+volatile char im920Flag;//無線通信
+double cm_Y,cm_X;
+unsigned int dist=0;
+int kyoriSiki =600;//本番は600
+double matoDis[MATO+1]= {0,0,0,0}; //的との距離
+//double matoDis[]= {0,150,200,160};
+double angle[MATO+1] {0,0,0,0};  //動く角度
+double pointX[MATO+1]= {0,0,0,0};  //的のx座標
+//double pointX[]= {50,50,50,50};
+double pointSum=0;
+double nnmDis[MATO+1]= {0,0,0,0}; //斜めの移動距離
+double rot=0;   //傾き
+volatile double motor[4]= {0}; //メモリアドレスを介して入出力を行うときにvolatile
+//int reach=200;   //弓の射程
+int reach=250;
+int encMode=0;  //0が x正 y負　1が x負 y正
+int sendTiming = 0;
+
+
+//無線関連-----------------------------------------------------
+void debugLEDbrinkThread()
+{
+    while(1) {
+        debugLED = !button.read();
+    }
+}
+
+void im920sSend(char status_)
+{
+    char data[1];
+    data[0] = status_;              //data you wanna send
+    i2c.write(0x08 << 1,data,1);
+    pc.printf("send to Arduino\r\n");
+}
+
+void commArduinoThread()
+{
+    while(1) {
+        if(im920Flag == 1) {
+            im920sSend(1);
+            im920Flag = 0;
+        }
+    }
+}
+void send()
+{
+    //busy = 1;
+
+    Thread LED;
+    LED.start(debugLEDbrinkThread);
+
+    Thread aaaaa;
+    aaaaa.start(commArduinoThread);
+
+    debugLED = 1;
+    wait_ms(1000);
+    debugLED = 0;
+    wait_ms(1000);
+
+    //pc.printf("prog start\r\n");
+
+    while (1) {
+        if(sendTiming == 1) { //user button
+            im920Flag = 1;
+            wait_ms(100);
+            sendTiming = 0;     //いるかわからない
+        }
+    }
+}
+//--------------------------------------------------------------
+
+void move(int spd, int dir)
+{
+    double adjMpu = mpu.read()*3;
+    float rad = dir / 180.0 * PI;
+    motor[0] = -spd * cos(rad + PI / 4)- adjMpu;
+    motor[1] = -spd * cos(rad - PI / 4)- adjMpu;
+    motor[2] = spd * cos(rad + PI / 4)- adjMpu;
+    motor[3] = spd * cos(rad - PI / 4)- adjMpu;
+
+    for(int i = 0; i<4 ; i++) {
+        cerica.Motor(i,motor[i]);
+    }
+}
+
+
+void encoder()
+{
+    while (true) {
+        switch(encMode) {
+            case 0:
+                cm_Y = -6.0 * PI * -cerica.GetY() / (double)720.0; //単位cm
+                cm_X = 6.0 * PI * -cerica.GetX() / (double)720.0;
+            case 1:
+                cm_Y = 6.0 * PI * -cerica.GetY() / (double)720.0; //単位cm
+                cm_X = -6.0 * PI * -cerica.GetX() / (double)720.0;
+        }
+
+        if (cerica.SW(0) == true) {
+            pc.printf("  X ---> %.2lf     Y ---> %.2lf\r\n", cm_X, cm_Y);
+        }
+    }
+}
+
+void mpuR()
+{
+    while(1) {
+        rot=mpu.read();
+        if (cerica.SW(1) == true) {
+            pc.printf("rot = %lf\r\n", rot);
+        }
+        if (cerica.SW(2) == true) {
+            pc.printf("cm:%d\r\n", dist );
+        }
+    }
+}
+
+
+
+
+void moveEnc(int spd, double dir, double distance)//移動用関数---------------------------
+{
+    cerica.ResetY();
+    cerica.ResetX();
+    pc.printf("-----------------------------\r\n");
+    double rad = dir / 180.0 * PI;
+    pc.printf("dir:%f  dis:%f  rad=%f\r\n",dir,distance,rad);
+    /*motor[0] = -spd * cos(rad + PI / 4) + adjMpu;
+    motor[1] = -spd * cos(rad - PI / 4) + adjMpu;
+    motor[2] = spd * cos(rad + PI / 4) + adjMpu;
+    motor[3] = spd * cos(rad - PI / 4) + adjMpu;
+    for(int i = 0; i<4 ; i++) {
+        cerica.Motor(i,motor[i]);
+    }*/
+    /*for(int i=0; i<4; i++) {
+        pc.printf("motor[%d] : %f\n\r",i, motor[i]);
+    }*/
+
+    //ここからエンコーダー処理
+    bool keyX = true, keyY = true;
+    double load_x = distance * cos(rad); //ラジアン値にすること
+    double load_y = distance * sin(rad);
+
+    if (load_x > 0) {
+        load_x = floor(load_x);//小数点切り捨て
+    } else {
+        load_x = ceil(load_x);//小数点切り上げ
+    }
+    if (load_y > 0) {
+        load_y = floor(load_y);
+    } else {
+        load_y = ceil(load_y);
+    }
+    pc.printf("loadX--->%.1f   loadY--->%.1f\r\n",load_x,load_y);
+
+    while(keyX==true||keyY==true) {
+        double adjMpu = mpu.read()*3;
+        motor[0] = -spd * cos(rad + PI / 4) - adjMpu;
+        motor[1] = -spd * cos(rad - PI / 4) - adjMpu;
+        motor[2] = spd * cos(rad + PI / 4) - adjMpu;
+        motor[3] = spd * cos(rad - PI / 4) - adjMpu;
+
+        for(int i = 0; i<4 ; i++) {
+            cerica.Motor(i,motor[i]);
+        }
+        if(load_x==0||load_x==-0) {
+            keyX=false;
+        }
+        if(load_y==0||load_y==-0) {
+            keyY=false;
+        }
+
+        if(load_x>0) {
+            if (cm_X >= load_x) {
+                keyX = false;
+            }
+        } else {
+            if(cm_X<=load_x) {
+                keyX = false;
+            }
+        }
+        if(load_y>0) {
+            if (cm_Y >= load_y) {
+                keyY = false;
+            }
+        } else {
+            if(cm_Y<=load_y) {
+                keyY = false;
+            }
+        }
+    }
+
+    /* do {//keyが両方trueのとき繰り返す
+         if(load_x==0) {
+             keyX=false;
+         }
+         if(load_y==0) {
+             keyY=false;
+         }
+
+         if(load_x>0) {
+             if (cm_X >= load_x) {
+                 keyX = false;
+                 //printf("get to load_x!\r\n");
+             }
+         } else {
+             if(cm_X<=load_x) {
+                 keyX = false;
+                 //printf("get to load_x!\r\n");
+             }
+         }
+         if(load_y>0) {
+             if (cm_Y >= load_y) {
+                 keyY = false;
+                 //printf("get to load_y!\r\n");
+             }
+         } else {
+             if(cm_Y<=load_y) {
+                 keyY = false;
+                 //printf("get to load_y!\r\n");
+             }
+         }
+     } while (keyX == true || keyY == true);
+     printf("get to GOAL!!------------\n");*/
+    cerica.ResetY();
+    cerica.ResetX();
+}
+void oto(int val)
+{
+    //cerica.Play(rand() % val);
+    cerica.Play(val);
+    wait(0.3);
+    cerica.Stop();
+}
+
+void rotation(int spd,int dir)
+{
+    mpu.reset();
+    for(int i=0; i<4; i++) {
+        pc.printf("motor[%d] : %d\n\r",i, spd);
+        cerica.Motor(i,spd);
+    }
+    pc.printf("rotating\r\n");
+    while(1) {
+        if(dir<=rot) {
+            for(int i=0; i<4; i++) {
+                pc.printf("motor[%d] : 0\n\r",i);
+                //cerica.Motor(i,0);
+            }
+            break;
+        }
+    }
+    mpu.reset();
+    pc.printf("rotation end\r\n");
+}
+
+void stop()
+{
+    for(int i=0; i<4; i++) {
+        pc.printf("motor[%d] : 0\n\r",i);
+        cerica.Motor(i,0);
+    }
+}
+
+void selfShot()
+{
+    while(1) {
+        if(!button) {
+            oto(40);
+            sendTiming=1;
+        }
+    }
+}
+
+void signal()
+{
+    while (true) {
+        cerica.Send();
+    }
+}
+
+/*void Init()
+{
+    int tone = 0;
+
+    // Initialize
+    for (int i = 0; i < 5; i++)
+        cerica.LED(i, 1);
+
+    boardInit();
+    cerica.ResetY();
+    cerica.ResetX();
+
+    //Thread Start
+    Thread move;
+    move.start(signal);
+
+    Thread mpu;
+    mpu.start(mpuR);
+
+    Thread enc;
+    enc.start(encoder);
+
+    Thread wireless;
+    wireless.start(send);
+
+
+    cerica.Play(tone), wait(0.1), cerica.Stop(), tone++, wait(0.1);
+    printf("MPU6050 Start\r\n");
+
+
+    cerica.Play(tone), wait(0.1), cerica.Stop(), tone++, wait(0.1);
+    printf("MPU6050 Start\r\n");
+
+    for (int i = 0; i < 5; i++)
+        cerica.LED(i, 0);
+
+    wait(0.2);
+}*/