NIT Fukui / Mbed OS 2021Arobo_UMAPYOI

2021 Ateam auto

Dependencies:   Serial6050Yaw HCSR04 CERICA_2019

main.cpp@0:3c88c8c7b112, 2022-04-25 (annotated)

Committer:
GOD_NAFU
Date:
Mon Apr 25 09:01:31 2022 +0000
Revision:
0:3c88c8c7b112
2021A_umaRobo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GOD_NAFU 0:3c88c8c7b112 1 #include "mbed.h"
GOD_NAFU 0:3c88c8c7b112 2 #include "utility.h"
GOD_NAFU 0:3c88c8c7b112 3 #define MATO 3
GOD_NAFU 0:3c88c8c7b112 4 #define PI 3.14159
GOD_NAFU 0:3c88c8c7b112 5 int fSpd;
GOD_NAFU 0:3c88c8c7b112 6
GOD_NAFU 0:3c88c8c7b112 7 void startSpd()
GOD_NAFU 0:3c88c8c7b112 8 {
GOD_NAFU 0:3c88c8c7b112 9 for(fSpd=0; fSpd<=160; fSpd+=5) {
GOD_NAFU 0:3c88c8c7b112 10 wait_ms(15);
GOD_NAFU 0:3c88c8c7b112 11 }
GOD_NAFU 0:3c88c8c7b112 12 }
GOD_NAFU 0:3c88c8c7b112 13
GOD_NAFU 0:3c88c8c7b112 14 void dis() //距離測定関数-----------------------------------
GOD_NAFU 0:3c88c8c7b112 15 {
GOD_NAFU 0:3c88c8c7b112 16 pc.baud(115200);
GOD_NAFU 0:3c88c8c7b112 17 //的との位置を把握
GOD_NAFU 0:3c88c8c7b112 18 int a = 3;
GOD_NAFU 0:3c88c8c7b112 19 while(matoDis[1]==0 || matoDis[2]==0 || matoDis[3]==0) {
GOD_NAFU 0:3c88c8c7b112 20 while(1) {
GOD_NAFU 0:3c88c8c7b112 21 move(fSpd,180); //ひだり
GOD_NAFU 0:3c88c8c7b112 22 usensor.start();
GOD_NAFU 0:3c88c8c7b112 23 dist = usensor.get_dist_cm();
GOD_NAFU 0:3c88c8c7b112 24 wait_ms(10);
GOD_NAFU 0:3c88c8c7b112 25 /*for(int i=0; i<4; i++) {
GOD_NAFU 0:3c88c8c7b112 26 pc.printf("motor[%d] : %f ",i, motor[i]);
GOD_NAFU 0:3c88c8c7b112 27 }
GOD_NAFU 0:3c88c8c7b112 28 pc.printf("\r\n");*/
GOD_NAFU 0:3c88c8c7b112 29
GOD_NAFU 0:3c88c8c7b112 30 if(dist < kyoriSiki && dist != 0) {
GOD_NAFU 0:3c88c8c7b112 31 matoDis[a] = dist;//的との距離
GOD_NAFU 0:3c88c8c7b112 32 pointX[a] = -cm_X - pointSum;//的がある場所のX方向の距離
GOD_NAFU 0:3c88c8c7b112 33 pointSum += pointX[a];
GOD_NAFU 0:3c88c8c7b112 34 oto(10);
GOD_NAFU 0:3c88c8c7b112 35 // pointX[a]=-cm_X;
GOD_NAFU 0:3c88c8c7b112 36 // cerica.ResetX();
GOD_NAFU 0:3c88c8c7b112 37 pc.printf("matoDis[%d]:%f\r\n",a,matoDis[a]);
GOD_NAFU 0:3c88c8c7b112 38 pc.printf("pointX[%d]:%f\r\n",a,pointX[a]);
GOD_NAFU 0:3c88c8c7b112 39
GOD_NAFU 0:3c88c8c7b112 40 while(1) {
GOD_NAFU 0:3c88c8c7b112 41 bbb:
GOD_NAFU 0:3c88c8c7b112 42 move(fSpd,180); //ひだり
GOD_NAFU 0:3c88c8c7b112 43 usensor.start();
GOD_NAFU 0:3c88c8c7b112 44 dist = usensor.get_dist_cm();
GOD_NAFU 0:3c88c8c7b112 45 wait_ms(10);
GOD_NAFU 0:3c88c8c7b112 46 if(dist>kyoriSiki) {
GOD_NAFU 0:3c88c8c7b112 47 while(1) {
GOD_NAFU 0:3c88c8c7b112 48 move(fSpd,180); //ひだり
GOD_NAFU 0:3c88c8c7b112 49 usensor.start();
GOD_NAFU 0:3c88c8c7b112 50 dist = usensor.get_dist_cm();
GOD_NAFU 0:3c88c8c7b112 51 wait_ms(10);
GOD_NAFU 0:3c88c8c7b112 52 if(dist<kyoriSiki && dist!=0) {
GOD_NAFU 0:3c88c8c7b112 53 goto bbb;
GOD_NAFU 0:3c88c8c7b112 54 }
GOD_NAFU 0:3c88c8c7b112 55 a--;
GOD_NAFU 0:3c88c8c7b112 56 pc.printf("a=%d\n\r",a);
GOD_NAFU 0:3c88c8c7b112 57 goto aaa;
GOD_NAFU 0:3c88c8c7b112 58 }
GOD_NAFU 0:3c88c8c7b112 59 }
GOD_NAFU 0:3c88c8c7b112 60 }
GOD_NAFU 0:3c88c8c7b112 61 }
GOD_NAFU 0:3c88c8c7b112 62 }
GOD_NAFU 0:3c88c8c7b112 63 aaa:
GOD_NAFU 0:3c88c8c7b112 64 // pc.printf("matoDis[%d]:%f\r\n",a-1,matoDis[a-1]);
GOD_NAFU 0:3c88c8c7b112 65 }
GOD_NAFU 0:3c88c8c7b112 66 a=1;
GOD_NAFU 0:3c88c8c7b112 67 wait_ms(300); //測り終わった後 秒進む（要調整）
GOD_NAFU 0:3c88c8c7b112 68 stop();
GOD_NAFU 0:3c88c8c7b112 69 pointX[0]=-cm_X - pointSum;//kokokokokokokokoko
GOD_NAFU 0:3c88c8c7b112 70 //pointX[0]=-cm_X;
GOD_NAFU 0:3c88c8c7b112 71 for(int i=0; i<=MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 72 pc.printf("matoDis[%d]:%f\r\n",i,matoDis[i]);
GOD_NAFU 0:3c88c8c7b112 73 pc.printf("pointX[%d]:%f\r\n",i,pointX[i]);
GOD_NAFU 0:3c88c8c7b112 74 }
GOD_NAFU 0:3c88c8c7b112 75 wait_ms(1000);
GOD_NAFU 0:3c88c8c7b112 76
GOD_NAFU 0:3c88c8c7b112 77 //回転
GOD_NAFU 0:3c88c8c7b112 78 rotation(80,170);
GOD_NAFU 0:3c88c8c7b112 79 stop();
GOD_NAFU 0:3c88c8c7b112 80 encMode=1;
GOD_NAFU 0:3c88c8c7b112 81 cerica.ResetX();
GOD_NAFU 0:3c88c8c7b112 82 cerica.ResetY();
GOD_NAFU 0:3c88c8c7b112 83
GOD_NAFU 0:3c88c8c7b112 84
GOD_NAFU 0:3c88c8c7b112 85 //角度とか
GOD_NAFU 0:3c88c8c7b112 86 angle[0]=atan((matoDis[1]-reach)/pointX[0]);
GOD_NAFU 0:3c88c8c7b112 87 for(int i=1; i<MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 88 angle[i]=atan((matoDis[i+1]-matoDis[i])/pointX[i]);
GOD_NAFU 0:3c88c8c7b112 89 }
GOD_NAFU 0:3c88c8c7b112 90 angle[MATO]=-atan((matoDis[MATO]-reach)/pointX[MATO]);
GOD_NAFU 0:3c88c8c7b112 91
GOD_NAFU 0:3c88c8c7b112 92 pc.printf("------------------------\r\n");
GOD_NAFU 0:3c88c8c7b112 93 for(int i=0; i<=MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 94 pc.printf("matoDis[%d]:%f\n\r",i,matoDis[i]);
GOD_NAFU 0:3c88c8c7b112 95 }
GOD_NAFU 0:3c88c8c7b112 96 pc.printf("------------------------\r\n");
GOD_NAFU 0:3c88c8c7b112 97
GOD_NAFU 0:3c88c8c7b112 98
GOD_NAFU 0:3c88c8c7b112 99 for(int i=0; i<=MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 100 pc.printf("pointX[%d]:%f\n\r",i,pointX[i]);
GOD_NAFU 0:3c88c8c7b112 101 }
GOD_NAFU 0:3c88c8c7b112 102 //反転のずれ直す
GOD_NAFU 0:3c88c8c7b112 103 for(int i=0; i<=MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 104 angle[i]=180/PI*(angle[i]+PI);
GOD_NAFU 0:3c88c8c7b112 105 pc.printf("angle[%d]:%f\n\r",i,angle[i]);
GOD_NAFU 0:3c88c8c7b112 106 }
GOD_NAFU 0:3c88c8c7b112 107 pointX[0]-=85;
GOD_NAFU 0:3c88c8c7b112 108 pointX[3]-=70;
GOD_NAFU 0:3c88c8c7b112 109
GOD_NAFU 0:3c88c8c7b112 110
GOD_NAFU 0:3c88c8c7b112 111 //進む距離
GOD_NAFU 0:3c88c8c7b112 112 //pc.printf("nakami:%d\r\n",(matoDis[1]-reach)*(matoDis[1]-reach)+pointX[0]*pointX[0]);
GOD_NAFU 0:3c88c8c7b112 113 nnmDis[0]=sqrt((matoDis[1]-reach)*(matoDis[1]-reach)+pointX[0]*pointX[0]);
GOD_NAFU 0:3c88c8c7b112 114 for(int i=1; i<MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 115 nnmDis[i]=sqrt((matoDis[i]-matoDis[i+1])*(matoDis[i]-matoDis[i+1])+pointX[i]*pointX[i]);
GOD_NAFU 0:3c88c8c7b112 116 }
GOD_NAFU 0:3c88c8c7b112 117 nnmDis[MATO]=sqrt((matoDis[MATO]-reach)*(matoDis[MATO]-reach)+pointX[MATO]*pointX[MATO]);
GOD_NAFU 0:3c88c8c7b112 118
GOD_NAFU 0:3c88c8c7b112 119 for(int i=0; i<=MATO; i++) {
GOD_NAFU 0:3c88c8c7b112 120 // nnmDis[i]=180/PI*angle[i];
GOD_NAFU 0:3c88c8c7b112 121 pc.printf("nnmDis[%d]:%f\n\r",i,nnmDis[i]);
GOD_NAFU 0:3c88c8c7b112 122 }
GOD_NAFU 0:3c88c8c7b112 123
GOD_NAFU 0:3c88c8c7b112 124 }
GOD_NAFU 0:3c88c8c7b112 125
GOD_NAFU 0:3c88c8c7b112 126
GOD_NAFU 0:3c88c8c7b112 127 //main------------------------------
GOD_NAFU 0:3c88c8c7b112 128 int main()
GOD_NAFU 0:3c88c8c7b112 129 {
GOD_NAFU 0:3c88c8c7b112 130 mpu.init();
GOD_NAFU 0:3c88c8c7b112 131 cerica.ResetY();
GOD_NAFU 0:3c88c8c7b112 132 cerica.ResetX();
GOD_NAFU 0:3c88c8c7b112 133
GOD_NAFU 0:3c88c8c7b112 134 Thread move;
GOD_NAFU 0:3c88c8c7b112 135 move.start(signal);
GOD_NAFU 0:3c88c8c7b112 136
GOD_NAFU 0:3c88c8c7b112 137 Thread m;
GOD_NAFU 0:3c88c8c7b112 138 m.start(mpuR);
GOD_NAFU 0:3c88c8c7b112 139
GOD_NAFU 0:3c88c8c7b112 140 Thread enc;
GOD_NAFU 0:3c88c8c7b112 141 enc.start(encoder);
GOD_NAFU 0:3c88c8c7b112 142
GOD_NAFU 0:3c88c8c7b112 143 Thread bow;
GOD_NAFU 0:3c88c8c7b112 144 bow.start(selfShot);
GOD_NAFU 0:3c88c8c7b112 145
GOD_NAFU 0:3c88c8c7b112 146 Thread wireless;
GOD_NAFU 0:3c88c8c7b112 147 wireless.start(send);
GOD_NAFU 0:3c88c8c7b112 148
GOD_NAFU 0:3c88c8c7b112 149 Thread slow;
GOD_NAFU 0:3c88c8c7b112 150
GOD_NAFU 0:3c88c8c7b112 151 boardInit();
GOD_NAFU 0:3c88c8c7b112 152
GOD_NAFU 0:3c88c8c7b112 153 oto(20);
GOD_NAFU 0:3c88c8c7b112 154
GOD_NAFU 0:3c88c8c7b112 155 pc.printf("start------------------------\r\n");
GOD_NAFU 0:3c88c8c7b112 156 //合体
GOD_NAFU 0:3c88c8c7b112 157 moveEnc(100,-90,70); //本番距離変える
GOD_NAFU 0:3c88c8c7b112 158 wait_ms(1500);
GOD_NAFU 0:3c88c8c7b112 159 pc.printf("Left move");
GOD_NAFU 0:3c88c8c7b112 160 //x方向に移動しながら距離測定
GOD_NAFU 0:3c88c8c7b112 161 slow.start(startSpd);
GOD_NAFU 0:3c88c8c7b112 162 dis();
GOD_NAFU 0:3c88c8c7b112 163 wait_ms(1000);
GOD_NAFU 0:3c88c8c7b112 164 //じぐざくいどう
GOD_NAFU 0:3c88c8c7b112 165 for(int i=0; i<=3; i++) {
GOD_NAFU 0:3c88c8c7b112 166 moveEnc(140,angle[i],nnmDis[i]);
GOD_NAFU 0:3c88c8c7b112 167 sendTiming = 1;
GOD_NAFU 0:3c88c8c7b112 168 stop();
GOD_NAFU 0:3c88c8c7b112 169 //撃つタイミング送信
GOD_NAFU 0:3c88c8c7b112 170 wait_ms(1000);
GOD_NAFU 0:3c88c8c7b112 171 }
GOD_NAFU 0:3c88c8c7b112 172
GOD_NAFU 0:3c88c8c7b112 173 for(int i=0; i<4; i++) {
GOD_NAFU 0:3c88c8c7b112 174 pc.printf("motor[%d] : 0\n\r",i);
GOD_NAFU 0:3c88c8c7b112 175 cerica.Motor(i,0);
GOD_NAFU 0:3c88c8c7b112 176 }
GOD_NAFU 0:3c88c8c7b112 177 for(int i=0; i<3; i++) {
GOD_NAFU 0:3c88c8c7b112 178 pc.printf("matoDis[%d]:%f\r\n",i,matoDis[i]);
GOD_NAFU 0:3c88c8c7b112 179 }
GOD_NAFU 0:3c88c8c7b112 180 }
GOD_NAFU 0:3c88c8c7b112 181