涼太郎 中村 / Mbed 2 deprecated f3rc2

めいん

Dependencies:   mbed

header.h@2:b204cf2f9b60, 2016-08-31 (annotated)

Committer:
choutin
Date:
Wed Aug 31 11:09:45 2016 +0000
Revision:
2:b204cf2f9b60
Parent:
1:a1e592eca305 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
choutin 0:df2659fd8031 1 //headerではPINを直接扱わない応用関数を扱う
choutin 0:df2659fd8031 2
choutin 0:df2659fd8031 3 #include <math.h>
choutin 0:df2659fd8031 4 #include"mbed.h"
choutin 0:df2659fd8031 5 #include "locate.h"
choutin 0:df2659fd8031 6 #include"prime.h"
choutin 0:df2659fd8031 7
choutin 0:df2659fd8031 8 const int speed=100;//基本速度
choutin 0:df2659fd8031 9 const float allowdegree = 0.02;//角度許容誤差
choutin 0:df2659fd8031 10 const int speedscaler = 10;//角度と速度を変換
choutin 0:df2659fd8031 11 const int minspeed = 50;//最小速度
choutin 0:df2659fd8031 12 const int allowlength=30;
choutin 0:df2659fd8031 13
choutin 0:df2659fd8031 14
choutin 2:b204cf2f9b60 15 void turn(int degree)
choutin 2:b204cf2f9b60 16 {
choutin 2:b204cf2f9b60 17 update(-right.getPulses(), left.getPulses());
choutin 2:b204cf2f9b60 18 float pTheta = coordinateTheta();
choutin 2:b204cf2f9b60 19
choutin 2:b204cf2f9b60 20 float rad = degree * PI / 180;
choutin 2:b204cf2f9b60 21 if(degree>=0) {
choutin 2:b204cf2f9b60 22 move(30, -30);
choutin 2:b204cf2f9b60 23 while(1) {
choutin 2:b204cf2f9b60 24 update(-right.getPulses(), left.getPulses());
choutin 2:b204cf2f9b60 25 float nTheta = coordinateTheta();
choutin 2:b204cf2f9b60 26 if(pTheta-nTheta < -rad ){
choutin 2:b204cf2f9b60 27 move(0,0);
choutin 2:b204cf2f9b60 28 break;
choutin 2:b204cf2f9b60 29 }
choutin 2:b204cf2f9b60 30 }
choutin 2:b204cf2f9b60 31 } else if(degree<0) {
choutin 2:b204cf2f9b60 32 move(-30, 30);
choutin 2:b204cf2f9b60 33 while(1) {
choutin 2:b204cf2f9b60 34 update(-right.getPulses(), left.getPulses());
choutin 2:b204cf2f9b60 35 float nTheta = coordinateTheta();
choutin 2:b204cf2f9b60 36
choutin 2:b204cf2f9b60 37 if(pTheta-nTheta > rad ){
choutin 2:b204cf2f9b60 38 move(0,0);
choutin 2:b204cf2f9b60 39 break;
choutin 2:b204cf2f9b60 40 }
choutin 2:b204cf2f9b60 41 }
choutin 2:b204cf2f9b60 42 }
choutin 2:b204cf2f9b60 43 }
choutin 0:df2659fd8031 44
choutin 0:df2659fd8031 45
choutin 0:df2659fd8031 46
choutin 0:df2659fd8031 47 void moveto(int targetx, int targety){
choutin 0:df2659fd8031 48 int x, y;
choutin 0:df2659fd8031 49 float theta, phi;
choutin 0:df2659fd8031 50 int onoff = 1;
choutin 0:df2659fd8031 51
choutin 0:df2659fd8031 52 //よく使うかたまり
choutin 2:b204cf2f9b60 53 update(-right.getPulses(), left.getPulses());
choutin 0:df2659fd8031 54 x = coordinateX();
choutin 0:df2659fd8031 55 y = coordinateY();
choutin 0:df2659fd8031 56 theta = coordinateTheta();//自己位置取得
choutin 0:df2659fd8031 57 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
choutin 0:df2659fd8031 58
choutin 0:df2659fd8031 59
choutin 0:df2659fd8031 60 //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
choutin 0:df2659fd8031 61 while (1){
choutin 2:b204cf2f9b60 62 update(-right.getPulses(), left.getPulses());
choutin 0:df2659fd8031 63 x = coordinateX();
choutin 0:df2659fd8031 64 y = coordinateY();
choutin 0:df2659fd8031 65 theta = coordinateTheta();
choutin 0:df2659fd8031 66 phi = atan(double(targety - y) / double(targetx - x));
choutin 0:df2659fd8031 67
choutin 0:df2659fd8031 68 move((-1)*speed, speed);
choutin 0:df2659fd8031 69 if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
choutin 0:df2659fd8031 70 }
choutin 0:df2659fd8031 71
choutin 0:df2659fd8031 72 //角度を合わせながら直進
choutin 0:df2659fd8031 73 while (onoff){
choutin 2:b204cf2f9b60 74 update(-right.getPulses(), left.getPulses());
choutin 0:df2659fd8031 75 x = coordinateX();
choutin 0:df2659fd8031 76 y = coordinateY();
choutin 0:df2659fd8031 77 theta = coordinateTheta();
choutin 0:df2659fd8031 78 phi = atan(double(targety - y) / double(targetx - x));
choutin 0:df2659fd8031 79
choutin 0:df2659fd8031 80 move(speed + 10 * (phi - theta), speed); //この10はradを回転数に影響させるスカラー。右輪のみで方向を調節するが、
choutin 0:df2659fd8031 81
choutin 0:df2659fd8031 82 if (targetx - allowlength < x&&x < targetx + allowlength){
choutin 0:df2659fd8031 83 //x座標が合ったら
choutin 0:df2659fd8031 84 if (targety - allowlength < y&&y < targety + allowlength){
choutin 0:df2659fd8031 85 onoff = 0;//whileから抜ける
choutin 0:df2659fd8031 86 }
choutin 0:df2659fd8031 87 }
choutin 0:df2659fd8031 88 }
choutin 0:df2659fd8031 89 //位置合わせ完了(最終的にはx,yどっちか先に合わせたい)
choutin 0:df2659fd8031 90 }
choutin 0:df2659fd8031 91
choutin 0:df2659fd8031 92
choutin 0:df2659fd8031 93
choutin 0:df2659fd8031 94 void back(){
choutin 0:df2659fd8031 95
choutin 0:df2659fd8031 96 int x1, y1, x2, y2,distance;
choutin 0:df2659fd8031 97
choutin 0:df2659fd8031 98 x1 = coordinateX();
choutin 0:df2659fd8031 99 y1= coordinateY();
choutin 0:df2659fd8031 100
choutin 0:df2659fd8031 101
choutin 0:df2659fd8031 102 move((-1)*speed, (-1) *speed);
choutin 0:df2659fd8031 103
choutin 0:df2659fd8031 104 while (1){
choutin 2:b204cf2f9b60 105 update(-right.getPulses(), left.getPulses());
choutin 0:df2659fd8031 106 x2 = coordinateX();
choutin 0:df2659fd8031 107 y2 = coordinateY();
choutin 0:df2659fd8031 108 distance = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
choutin 0:df2659fd8031 109
choutin 0:df2659fd8031 110 if (distance > 22500){
choutin 0:df2659fd8031 111 move(0, 0);
choutin 0:df2659fd8031 112 break; }
choutin 0:df2659fd8031 113 }
choutin 0:df2659fd8031 114
choutin 0:df2659fd8031 115
choutin 0:df2659fd8031 116 }
choutin 0:df2659fd8031 117
choutin 0:df2659fd8031 118
choutin 0:df2659fd8031 119 void fixto(int targetx,int targety){//必ずy正方向からとることとする
choutin 0:df2659fd8031 120 int onoff=1,lr=0;
choutin 0:df2659fd8031 121 const int X=20;//左右の位置調整。回転数をどれだけ増減させるかをこの定数に入れる
choutin 0:df2659fd8031 122
choutin 0:df2659fd8031 123 while (onoff){
choutin 0:df2659fd8031 124 //(right,left)=(off,off),(on,off),(off,on),(on,on)
choutin 0:df2659fd8031 125 // 0 1 2 3
choutin 0:df2659fd8031 126 lr = sensor();
choutin 0:df2659fd8031 127
choutin 0:df2659fd8031 128 switch (lr){
choutin 0:df2659fd8031 129
choutin 0:df2659fd8031 130 case 0:
choutin 0:df2659fd8031 131 break;
choutin 0:df2659fd8031 132 case 1:
choutin 0:df2659fd8031 133 move(speed-X , speed+X);
choutin 0:df2659fd8031 134 break;
choutin 0:df2659fd8031 135 case 2:
choutin 0:df2659fd8031 136 move(speed+X , speed-X);
choutin 0:df2659fd8031 137 break;
choutin 0:df2659fd8031 138 case 3:
choutin 0:df2659fd8031 139 move(speed,speed);
choutin 0:df2659fd8031 140 break;
choutin 0:df2659fd8031 141 default:
choutin 0:df2659fd8031 142 break;
choutin 0:df2659fd8031 143 }
choutin 0:df2659fd8031 144
choutin 0:df2659fd8031 145 }
choutin 0:df2659fd8031 146 }
choutin 0:df2659fd8031 147
choutin 0:df2659fd8031 148
choutin 0:df2659fd8031 149
choutin 0:df2659fd8031 150
choutin 0:df2659fd8031 151
choutin 0:df2659fd8031 152 void lift(int level) {//段数を指定して回転させる。
choutin 0:df2659fd8031 153 const int K=33;//階数と回転角度を反映させる比例点定数
choutin 0:df2659fd8031 154
choutin 0:df2659fd8031 155 step(K*level);
choutin 0:df2659fd8031 156
choutin 0:df2659fd8031 157
choutin 0:df2659fd8031 158 }
choutin 0:df2659fd8031 159
choutin 0:df2659fd8031 160 void release(void){
choutin 0:df2659fd8031 161
choutin 0:df2659fd8031 162 step(100);
choutin 0:df2659fd8031 163
choutin 0:df2659fd8031 164 }
choutin 0:df2659fd8031 165
choutin 0:df2659fd8031 166