sumple program for angel linesensor. This program can prevent "out of bounds" while robot runs slow.
基板サイズ φ130mm 固定部はr5mmの円弧 厚さ1mm
使用パーツ
Nucleo stm32f303k8 1個
xh 3pin 1個
フォトトランジスタ(NJL7502L) 36個
超高輝度3mmウォームホワイトLED 36個
LED用抵抗(300Ω 1/4w) 18個
プルダウン用抵抗(1kΩ1/6w) 18個
可変抵抗100kΩ(3362P) 18個
シュミットトリガ(SN74HC14N) 3個
ピンヘッダ 15p
ICソケット 14p
組立て
XHのピン側が前(0°方向)です。 可変抵抗のシルクがあるほうが地面側です。 LEDとフォトトランジスタはシルクの向きが逆ですが頭が地面側にくるように取り付けます。 LEDとフォトトランジスタをホールにさして、上空側から見たとき足の長いほうが左側にくるように取り付けてください。(ロボット正面から見ると右側に長い方が来ます) LED用抵抗は、間隔が広い抵抗シルクがあるところに取り付けてください。 プルダウン用抵抗は,間隔が短い抵抗シルクがあるところに取り付けてください。 Nucleoは上空から見たとき左側にmicrousb端子(ロボット正面から見ると右側)がくるように取り付けてください。
シュミットトリガやnucleoがモーター等に干渉しないように気をつけてください。干渉しそうな場合は反対の面からつけてください。
使い方 入力電圧は5Vです。 アナログ値(0:ボール無, 0.33-3.3V:ボール有)で出力されます。 TJ3で読む場合(0%:ボール無, 6%-66%:ボール有)
メインマイコン側で360度に変換します。
mbedの場合(0-1) 400*(line.read() -0.1)
cstyleの場合(0-100) 6*(CN1 - 6)
ロボット正面を0°,時計回りに360°として、 変換した値の角度の方向に動きます。
mbedの場合
if(line.read() == 0){反応無し}
else{進行方向 = 400*(line.read()-0.1);}
Cstyleの場合はテーブル化したり場合分けして動かすと良いでしょう。
if(CN1 == 0){反応無し}
else if(6*(CN1 - 6) >= 350){motor(r100,r100,100,100);}
else if(6*(CN1 - 6) >= 330){motor(r100,r80,100,80);}
..etc
main.cpp@0:aa4a1eead8d4, 2018-04-01 (annotated)
- Committer:
- tajiri1999
- Date:
- Sun Apr 01 14:27:34 2018 +0000
- Revision:
- 0:aa4a1eead8d4
This sensor program was made for RCJ.; It cannot be run perfect.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tajiri1999 | 0:aa4a1eead8d4 | 1 | #include "mbed.h" |
tajiri1999 | 0:aa4a1eead8d4 | 2 | |
tajiri1999 | 0:aa4a1eead8d4 | 3 | /*Nucleo STM32F303k8で開発しました.*/ |
tajiri1999 | 0:aa4a1eead8d4 | 4 | DigitalIn Line0(A7);//0 |
tajiri1999 | 0:aa4a1eead8d4 | 5 | DigitalIn Line1(A1);//20 |
tajiri1999 | 0:aa4a1eead8d4 | 6 | DigitalIn Line2(A0);//40 |
tajiri1999 | 0:aa4a1eead8d4 | 7 | DigitalIn Line3(D13);//60 |
tajiri1999 | 0:aa4a1eead8d4 | 8 | DigitalIn Line4(D7);//80 |
tajiri1999 | 0:aa4a1eead8d4 | 9 | DigitalIn Line5(D10);//100 |
tajiri1999 | 0:aa4a1eead8d4 | 10 | DigitalIn Line6(D11);//120 |
tajiri1999 | 0:aa4a1eead8d4 | 11 | DigitalIn Line7(D12);//140 |
tajiri1999 | 0:aa4a1eead8d4 | 12 | DigitalIn Line8(D8);//160 |
tajiri1999 | 0:aa4a1eead8d4 | 13 | DigitalIn Line9(D9);//180 |
tajiri1999 | 0:aa4a1eead8d4 | 14 | DigitalIn Line10(D6);//200 |
tajiri1999 | 0:aa4a1eead8d4 | 15 | DigitalIn Line11(D5);//220 |
tajiri1999 | 0:aa4a1eead8d4 | 16 | DigitalIn Line12(D4);//240 |
tajiri1999 | 0:aa4a1eead8d4 | 17 | DigitalIn Line13(D0);//260 |
tajiri1999 | 0:aa4a1eead8d4 | 18 | DigitalIn Line14(D2);//280 |
tajiri1999 | 0:aa4a1eead8d4 | 19 | DigitalIn Line15(D3);//300 |
tajiri1999 | 0:aa4a1eead8d4 | 20 | DigitalIn Line16(A5);//320 |
tajiri1999 | 0:aa4a1eead8d4 | 21 | DigitalIn Line17(A6);//340 |
tajiri1999 | 0:aa4a1eead8d4 | 22 | |
tajiri1999 | 0:aa4a1eead8d4 | 23 | /*DegreeOutの出力が0Vのときライン反応無し,0.33V~3.3Vのときライン反応有*/ |
tajiri1999 | 0:aa4a1eead8d4 | 24 | AnalogOut DegreeOut(A3); |
tajiri1999 | 0:aa4a1eead8d4 | 25 | |
tajiri1999 | 0:aa4a1eead8d4 | 26 | |
tajiri1999 | 0:aa4a1eead8d4 | 27 | double Linefor(double x[18],double y[18]); |
tajiri1999 | 0:aa4a1eead8d4 | 28 | |
tajiri1999 | 0:aa4a1eead8d4 | 29 | int j; |
tajiri1999 | 0:aa4a1eead8d4 | 30 | const float Pi = 3.1415926; |
tajiri1999 | 0:aa4a1eead8d4 | 31 | double X[2]; |
tajiri1999 | 0:aa4a1eead8d4 | 32 | double Y[2]; |
tajiri1999 | 0:aa4a1eead8d4 | 33 | double Vx,Vy; |
tajiri1999 | 0:aa4a1eead8d4 | 34 | double Distance,degreebase; |
tajiri1999 | 0:aa4a1eead8d4 | 35 | double Degree[2]; |
tajiri1999 | 0:aa4a1eead8d4 | 36 | |
tajiri1999 | 0:aa4a1eead8d4 | 37 | |
tajiri1999 | 0:aa4a1eead8d4 | 38 | |
tajiri1999 | 0:aa4a1eead8d4 | 39 | |
tajiri1999 | 0:aa4a1eead8d4 | 40 | int main(){ |
tajiri1999 | 0:aa4a1eead8d4 | 41 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 42 | |
tajiri1999 | 0:aa4a1eead8d4 | 43 | |
tajiri1999 | 0:aa4a1eead8d4 | 44 | if(Line0 == 0){//0度にあるセンサーの反応があるとき |
tajiri1999 | 0:aa4a1eead8d4 | 45 | double xb[18] = {1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.76604,0.93969}; |
tajiri1999 | 0:aa4a1eead8d4 | 46 | double yb[18] = {0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202}; |
tajiri1999 | 0:aa4a1eead8d4 | 47 | degreebase = 0; |
tajiri1999 | 0:aa4a1eead8d4 | 48 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 49 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 50 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 51 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 52 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 53 | } |
tajiri1999 | 0:aa4a1eead8d4 | 54 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 55 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 56 | } |
tajiri1999 | 0:aa4a1eead8d4 | 57 | } |
tajiri1999 | 0:aa4a1eead8d4 | 58 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 59 | Degree[1] = (degreebase + Linefor(xb,yb) + 180);//180°足すのは求めた角度の反対側に進むため |
tajiri1999 | 0:aa4a1eead8d4 | 60 | if(Degree[1] >= 360){//定義域を0~360に変換 |
tajiri1999 | 0:aa4a1eead8d4 | 61 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 62 | } |
tajiri1999 | 0:aa4a1eead8d4 | 63 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 64 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 65 | } |
tajiri1999 | 0:aa4a1eead8d4 | 66 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 67 | } |
tajiri1999 | 0:aa4a1eead8d4 | 68 | } |
tajiri1999 | 0:aa4a1eead8d4 | 69 | } |
tajiri1999 | 0:aa4a1eead8d4 | 70 | else if(Line1 == 0){ //20 |
tajiri1999 | 0:aa4a1eead8d4 | 71 | double xb[18] = {0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660}; |
tajiri1999 | 0:aa4a1eead8d4 | 72 | double yb[18] = {-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278}; |
tajiri1999 | 0:aa4a1eead8d4 | 73 | degreebase = 20; |
tajiri1999 | 0:aa4a1eead8d4 | 74 | |
tajiri1999 | 0:aa4a1eead8d4 | 75 | |
tajiri1999 | 0:aa4a1eead8d4 | 76 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 77 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 78 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 79 | if(Vx < 0){//角度が反転した状態でラインから出たとき |
tajiri1999 | 0:aa4a1eead8d4 | 80 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 81 | } |
tajiri1999 | 0:aa4a1eead8d4 | 82 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 83 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 84 | } |
tajiri1999 | 0:aa4a1eead8d4 | 85 | } |
tajiri1999 | 0:aa4a1eead8d4 | 86 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 87 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 88 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 89 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 90 | } |
tajiri1999 | 0:aa4a1eead8d4 | 91 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 92 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 93 | } |
tajiri1999 | 0:aa4a1eead8d4 | 94 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 95 | } |
tajiri1999 | 0:aa4a1eead8d4 | 96 | } |
tajiri1999 | 0:aa4a1eead8d4 | 97 | } |
tajiri1999 | 0:aa4a1eead8d4 | 98 | else if(Line2 ==0){//40 |
tajiri1999 | 0:aa4a1eead8d4 | 99 | double xb[18] = {0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5}; |
tajiri1999 | 0:aa4a1eead8d4 | 100 | double yb[18] = {-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602}; |
tajiri1999 | 0:aa4a1eead8d4 | 101 | |
tajiri1999 | 0:aa4a1eead8d4 | 102 | degreebase = 40; |
tajiri1999 | 0:aa4a1eead8d4 | 103 | |
tajiri1999 | 0:aa4a1eead8d4 | 104 | |
tajiri1999 | 0:aa4a1eead8d4 | 105 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 106 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 107 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 108 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 109 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 110 | } |
tajiri1999 | 0:aa4a1eead8d4 | 111 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 112 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 113 | } |
tajiri1999 | 0:aa4a1eead8d4 | 114 | } |
tajiri1999 | 0:aa4a1eead8d4 | 115 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 116 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 117 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 118 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 119 | } |
tajiri1999 | 0:aa4a1eead8d4 | 120 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 121 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 122 | } |
tajiri1999 | 0:aa4a1eead8d4 | 123 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 124 | } |
tajiri1999 | 0:aa4a1eead8d4 | 125 | } |
tajiri1999 | 0:aa4a1eead8d4 | 126 | } |
tajiri1999 | 0:aa4a1eead8d4 | 127 | else if(Line3 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 128 | double xb[18] = {0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364}; |
tajiri1999 | 0:aa4a1eead8d4 | 129 | double yb[18] = {-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848}; |
tajiri1999 | 0:aa4a1eead8d4 | 130 | degreebase = 60; |
tajiri1999 | 0:aa4a1eead8d4 | 131 | |
tajiri1999 | 0:aa4a1eead8d4 | 132 | |
tajiri1999 | 0:aa4a1eead8d4 | 133 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 134 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 135 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 136 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 137 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 138 | } |
tajiri1999 | 0:aa4a1eead8d4 | 139 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 140 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 141 | } |
tajiri1999 | 0:aa4a1eead8d4 | 142 | } |
tajiri1999 | 0:aa4a1eead8d4 | 143 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 144 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 145 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 146 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 147 | } |
tajiri1999 | 0:aa4a1eead8d4 | 148 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 149 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 150 | } |
tajiri1999 | 0:aa4a1eead8d4 | 151 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 152 | } |
tajiri1999 | 0:aa4a1eead8d4 | 153 | } |
tajiri1999 | 0:aa4a1eead8d4 | 154 | } |
tajiri1999 | 0:aa4a1eead8d4 | 155 | else if(Line4 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 156 | double xb[18] = {0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364}; |
tajiri1999 | 0:aa4a1eead8d4 | 157 | double yb[18] = {-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848}; |
tajiri1999 | 0:aa4a1eead8d4 | 158 | degreebase = 80; |
tajiri1999 | 0:aa4a1eead8d4 | 159 | |
tajiri1999 | 0:aa4a1eead8d4 | 160 | |
tajiri1999 | 0:aa4a1eead8d4 | 161 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 162 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 163 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 164 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 165 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 166 | } |
tajiri1999 | 0:aa4a1eead8d4 | 167 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 168 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 169 | } |
tajiri1999 | 0:aa4a1eead8d4 | 170 | } |
tajiri1999 | 0:aa4a1eead8d4 | 171 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 172 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 173 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 174 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 175 | } |
tajiri1999 | 0:aa4a1eead8d4 | 176 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 177 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 178 | } |
tajiri1999 | 0:aa4a1eead8d4 | 179 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 180 | } |
tajiri1999 | 0:aa4a1eead8d4 | 181 | } |
tajiri1999 | 0:aa4a1eead8d4 | 182 | } |
tajiri1999 | 0:aa4a1eead8d4 | 183 | else if(Line5 ==0){//100 |
tajiri1999 | 0:aa4a1eead8d4 | 184 | double xb[18] = {-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5}; |
tajiri1999 | 0:aa4a1eead8d4 | 185 | double yb[18] = {-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602}; |
tajiri1999 | 0:aa4a1eead8d4 | 186 | degreebase = 100; |
tajiri1999 | 0:aa4a1eead8d4 | 187 | |
tajiri1999 | 0:aa4a1eead8d4 | 188 | |
tajiri1999 | 0:aa4a1eead8d4 | 189 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 190 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 191 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 192 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 193 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 194 | } |
tajiri1999 | 0:aa4a1eead8d4 | 195 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 196 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 197 | } |
tajiri1999 | 0:aa4a1eead8d4 | 198 | } |
tajiri1999 | 0:aa4a1eead8d4 | 199 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 200 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 201 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 202 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 203 | } |
tajiri1999 | 0:aa4a1eead8d4 | 204 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 205 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 206 | } |
tajiri1999 | 0:aa4a1eead8d4 | 207 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 208 | } |
tajiri1999 | 0:aa4a1eead8d4 | 209 | } |
tajiri1999 | 0:aa4a1eead8d4 | 210 | } |
tajiri1999 | 0:aa4a1eead8d4 | 211 | else if(Line6 ==0){//120 |
tajiri1999 | 0:aa4a1eead8d4 | 212 | double xb[18] = {-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604}; |
tajiri1999 | 0:aa4a1eead8d4 | 213 | double yb[18] = {-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278}; |
tajiri1999 | 0:aa4a1eead8d4 | 214 | degreebase = 120; |
tajiri1999 | 0:aa4a1eead8d4 | 215 | |
tajiri1999 | 0:aa4a1eead8d4 | 216 | |
tajiri1999 | 0:aa4a1eead8d4 | 217 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 218 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 219 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 220 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 221 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 222 | } |
tajiri1999 | 0:aa4a1eead8d4 | 223 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 224 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 225 | } |
tajiri1999 | 0:aa4a1eead8d4 | 226 | } |
tajiri1999 | 0:aa4a1eead8d4 | 227 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 228 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 229 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 230 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 231 | } |
tajiri1999 | 0:aa4a1eead8d4 | 232 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 233 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 234 | } |
tajiri1999 | 0:aa4a1eead8d4 | 235 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 236 | } |
tajiri1999 | 0:aa4a1eead8d4 | 237 | } |
tajiri1999 | 0:aa4a1eead8d4 | 238 | } |
tajiri1999 | 0:aa4a1eead8d4 | 239 | else if(Line7 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 240 | double xb[18] = {-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969}; |
tajiri1999 | 0:aa4a1eead8d4 | 241 | double yb[18] = {-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202}; |
tajiri1999 | 0:aa4a1eead8d4 | 242 | degreebase = 140; |
tajiri1999 | 0:aa4a1eead8d4 | 243 | |
tajiri1999 | 0:aa4a1eead8d4 | 244 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 245 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 246 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 247 | |
tajiri1999 | 0:aa4a1eead8d4 | 248 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 249 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 250 | } |
tajiri1999 | 0:aa4a1eead8d4 | 251 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 252 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 253 | } |
tajiri1999 | 0:aa4a1eead8d4 | 254 | |
tajiri1999 | 0:aa4a1eead8d4 | 255 | } |
tajiri1999 | 0:aa4a1eead8d4 | 256 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 257 | |
tajiri1999 | 0:aa4a1eead8d4 | 258 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 259 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 260 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 261 | } |
tajiri1999 | 0:aa4a1eead8d4 | 262 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 263 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 264 | } |
tajiri1999 | 0:aa4a1eead8d4 | 265 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 266 | } |
tajiri1999 | 0:aa4a1eead8d4 | 267 | } |
tajiri1999 | 0:aa4a1eead8d4 | 268 | } |
tajiri1999 | 0:aa4a1eead8d4 | 269 | else if(Line8 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 270 | double xb[18] = {-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1}; |
tajiri1999 | 0:aa4a1eead8d4 | 271 | double yb[18] = {-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0}; |
tajiri1999 | 0:aa4a1eead8d4 | 272 | degreebase = 160; |
tajiri1999 | 0:aa4a1eead8d4 | 273 | |
tajiri1999 | 0:aa4a1eead8d4 | 274 | |
tajiri1999 | 0:aa4a1eead8d4 | 275 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 276 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 277 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 278 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 279 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 280 | } |
tajiri1999 | 0:aa4a1eead8d4 | 281 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 282 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 283 | } |
tajiri1999 | 0:aa4a1eead8d4 | 284 | |
tajiri1999 | 0:aa4a1eead8d4 | 285 | } |
tajiri1999 | 0:aa4a1eead8d4 | 286 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 287 | |
tajiri1999 | 0:aa4a1eead8d4 | 288 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 289 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 290 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 291 | } |
tajiri1999 | 0:aa4a1eead8d4 | 292 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 293 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 294 | } |
tajiri1999 | 0:aa4a1eead8d4 | 295 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 296 | } |
tajiri1999 | 0:aa4a1eead8d4 | 297 | } |
tajiri1999 | 0:aa4a1eead8d4 | 298 | } |
tajiri1999 | 0:aa4a1eead8d4 | 299 | else if(Line9 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 300 | double xb[18] = {-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969}; |
tajiri1999 | 0:aa4a1eead8d4 | 301 | double yb[18] = {0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202}; |
tajiri1999 | 0:aa4a1eead8d4 | 302 | degreebase = 180; |
tajiri1999 | 0:aa4a1eead8d4 | 303 | |
tajiri1999 | 0:aa4a1eead8d4 | 304 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 305 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 306 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 307 | |
tajiri1999 | 0:aa4a1eead8d4 | 308 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 309 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 310 | } |
tajiri1999 | 0:aa4a1eead8d4 | 311 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 312 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 313 | } |
tajiri1999 | 0:aa4a1eead8d4 | 314 | |
tajiri1999 | 0:aa4a1eead8d4 | 315 | } |
tajiri1999 | 0:aa4a1eead8d4 | 316 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 317 | |
tajiri1999 | 0:aa4a1eead8d4 | 318 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 319 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 320 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 321 | } |
tajiri1999 | 0:aa4a1eead8d4 | 322 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 323 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 324 | } |
tajiri1999 | 0:aa4a1eead8d4 | 325 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 326 | } |
tajiri1999 | 0:aa4a1eead8d4 | 327 | } |
tajiri1999 | 0:aa4a1eead8d4 | 328 | } |
tajiri1999 | 0:aa4a1eead8d4 | 329 | else if(Line10 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 330 | double xb[18] = {-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604}; |
tajiri1999 | 0:aa4a1eead8d4 | 331 | double yb[18] = {0.342020,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278}; |
tajiri1999 | 0:aa4a1eead8d4 | 332 | degreebase = 200; |
tajiri1999 | 0:aa4a1eead8d4 | 333 | |
tajiri1999 | 0:aa4a1eead8d4 | 334 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 335 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 336 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 337 | |
tajiri1999 | 0:aa4a1eead8d4 | 338 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 339 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 340 | } |
tajiri1999 | 0:aa4a1eead8d4 | 341 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 342 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 343 | } |
tajiri1999 | 0:aa4a1eead8d4 | 344 | |
tajiri1999 | 0:aa4a1eead8d4 | 345 | } |
tajiri1999 | 0:aa4a1eead8d4 | 346 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 347 | |
tajiri1999 | 0:aa4a1eead8d4 | 348 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 349 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 350 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 351 | } |
tajiri1999 | 0:aa4a1eead8d4 | 352 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 353 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 354 | } |
tajiri1999 | 0:aa4a1eead8d4 | 355 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 356 | } |
tajiri1999 | 0:aa4a1eead8d4 | 357 | } |
tajiri1999 | 0:aa4a1eead8d4 | 358 | } |
tajiri1999 | 0:aa4a1eead8d4 | 359 | else if(Line11 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 360 | double xb[18] = {-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364,-0.5}; |
tajiri1999 | 0:aa4a1eead8d4 | 361 | double yb[18] = {0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848,0.86602}; |
tajiri1999 | 0:aa4a1eead8d4 | 362 | degreebase = 220; |
tajiri1999 | 0:aa4a1eead8d4 | 363 | |
tajiri1999 | 0:aa4a1eead8d4 | 364 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 365 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 366 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 367 | |
tajiri1999 | 0:aa4a1eead8d4 | 368 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 369 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 370 | } |
tajiri1999 | 0:aa4a1eead8d4 | 371 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 372 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 373 | } |
tajiri1999 | 0:aa4a1eead8d4 | 374 | |
tajiri1999 | 0:aa4a1eead8d4 | 375 | } |
tajiri1999 | 0:aa4a1eead8d4 | 376 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 377 | |
tajiri1999 | 0:aa4a1eead8d4 | 378 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 379 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 380 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 381 | } |
tajiri1999 | 0:aa4a1eead8d4 | 382 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 383 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 384 | } |
tajiri1999 | 0:aa4a1eead8d4 | 385 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 386 | } |
tajiri1999 | 0:aa4a1eead8d4 | 387 | } |
tajiri1999 | 0:aa4a1eead8d4 | 388 | } |
tajiri1999 | 0:aa4a1eead8d4 | 389 | else if(Line12 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 390 | double xb[18] = {-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364,-0.17364}; |
tajiri1999 | 0:aa4a1eead8d4 | 391 | double yb[18] = {0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848,0.9848}; |
tajiri1999 | 0:aa4a1eead8d4 | 392 | degreebase = 240; |
tajiri1999 | 0:aa4a1eead8d4 | 393 | |
tajiri1999 | 0:aa4a1eead8d4 | 394 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 395 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 396 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 397 | |
tajiri1999 | 0:aa4a1eead8d4 | 398 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 399 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 400 | } |
tajiri1999 | 0:aa4a1eead8d4 | 401 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 402 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 403 | } |
tajiri1999 | 0:aa4a1eead8d4 | 404 | |
tajiri1999 | 0:aa4a1eead8d4 | 405 | } |
tajiri1999 | 0:aa4a1eead8d4 | 406 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 407 | |
tajiri1999 | 0:aa4a1eead8d4 | 408 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 409 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 410 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 411 | } |
tajiri1999 | 0:aa4a1eead8d4 | 412 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 413 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 414 | } |
tajiri1999 | 0:aa4a1eead8d4 | 415 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 416 | } |
tajiri1999 | 0:aa4a1eead8d4 | 417 | } |
tajiri1999 | 0:aa4a1eead8d4 | 418 | } |
tajiri1999 | 0:aa4a1eead8d4 | 419 | else if(Line13 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 420 | double xb[18] = {-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5,0.17364}; |
tajiri1999 | 0:aa4a1eead8d4 | 421 | double yb[18] = {0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602,0.9848}; |
tajiri1999 | 0:aa4a1eead8d4 | 422 | degreebase = 260; |
tajiri1999 | 0:aa4a1eead8d4 | 423 | |
tajiri1999 | 0:aa4a1eead8d4 | 424 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 425 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 426 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 427 | |
tajiri1999 | 0:aa4a1eead8d4 | 428 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 429 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 430 | } |
tajiri1999 | 0:aa4a1eead8d4 | 431 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 432 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 433 | } |
tajiri1999 | 0:aa4a1eead8d4 | 434 | |
tajiri1999 | 0:aa4a1eead8d4 | 435 | } |
tajiri1999 | 0:aa4a1eead8d4 | 436 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 437 | |
tajiri1999 | 0:aa4a1eead8d4 | 438 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 439 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 440 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 441 | } |
tajiri1999 | 0:aa4a1eead8d4 | 442 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 443 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 444 | } |
tajiri1999 | 0:aa4a1eead8d4 | 445 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 446 | } |
tajiri1999 | 0:aa4a1eead8d4 | 447 | } |
tajiri1999 | 0:aa4a1eead8d4 | 448 | } |
tajiri1999 | 0:aa4a1eead8d4 | 449 | else if(Line14==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 450 | double xb[18] = {0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604,0.5}; |
tajiri1999 | 0:aa4a1eead8d4 | 451 | double yb[18] = {0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278,0.86602}; |
tajiri1999 | 0:aa4a1eead8d4 | 452 | degreebase = 280; |
tajiri1999 | 0:aa4a1eead8d4 | 453 | |
tajiri1999 | 0:aa4a1eead8d4 | 454 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 455 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 456 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 457 | |
tajiri1999 | 0:aa4a1eead8d4 | 458 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 459 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 460 | } |
tajiri1999 | 0:aa4a1eead8d4 | 461 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 462 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 463 | } |
tajiri1999 | 0:aa4a1eead8d4 | 464 | |
tajiri1999 | 0:aa4a1eead8d4 | 465 | } |
tajiri1999 | 0:aa4a1eead8d4 | 466 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 467 | |
tajiri1999 | 0:aa4a1eead8d4 | 468 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 469 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 470 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 471 | } |
tajiri1999 | 0:aa4a1eead8d4 | 472 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 473 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 474 | } |
tajiri1999 | 0:aa4a1eead8d4 | 475 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 476 | } |
tajiri1999 | 0:aa4a1eead8d4 | 477 | } |
tajiri1999 | 0:aa4a1eead8d4 | 478 | } |
tajiri1999 | 0:aa4a1eead8d4 | 479 | else if(Line15 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 480 | double xb[18] = {0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969,0.76604}; |
tajiri1999 | 0:aa4a1eead8d4 | 481 | double yb[18] = {0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202,0.64278}; |
tajiri1999 | 0:aa4a1eead8d4 | 482 | degreebase = 300; |
tajiri1999 | 0:aa4a1eead8d4 | 483 | |
tajiri1999 | 0:aa4a1eead8d4 | 484 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 485 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 486 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 487 | |
tajiri1999 | 0:aa4a1eead8d4 | 488 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 489 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 490 | } |
tajiri1999 | 0:aa4a1eead8d4 | 491 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 492 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 493 | } |
tajiri1999 | 0:aa4a1eead8d4 | 494 | |
tajiri1999 | 0:aa4a1eead8d4 | 495 | } |
tajiri1999 | 0:aa4a1eead8d4 | 496 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 497 | |
tajiri1999 | 0:aa4a1eead8d4 | 498 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 499 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 500 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 501 | } |
tajiri1999 | 0:aa4a1eead8d4 | 502 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 503 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 504 | } |
tajiri1999 | 0:aa4a1eead8d4 | 505 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 506 | } |
tajiri1999 | 0:aa4a1eead8d4 | 507 | } |
tajiri1999 | 0:aa4a1eead8d4 | 508 | } |
tajiri1999 | 0:aa4a1eead8d4 | 509 | else if(Line16 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 510 | double xb[18] = {0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1,0.93969}; |
tajiri1999 | 0:aa4a1eead8d4 | 511 | double yb[18] = {0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0,0.34202}; |
tajiri1999 | 0:aa4a1eead8d4 | 512 | degreebase = 320; |
tajiri1999 | 0:aa4a1eead8d4 | 513 | |
tajiri1999 | 0:aa4a1eead8d4 | 514 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 515 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 516 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 517 | |
tajiri1999 | 0:aa4a1eead8d4 | 518 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 519 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 520 | } |
tajiri1999 | 0:aa4a1eead8d4 | 521 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 522 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 523 | } |
tajiri1999 | 0:aa4a1eead8d4 | 524 | |
tajiri1999 | 0:aa4a1eead8d4 | 525 | } |
tajiri1999 | 0:aa4a1eead8d4 | 526 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 527 | |
tajiri1999 | 0:aa4a1eead8d4 | 528 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 529 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 530 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 531 | } |
tajiri1999 | 0:aa4a1eead8d4 | 532 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 533 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 534 | } |
tajiri1999 | 0:aa4a1eead8d4 | 535 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 536 | } |
tajiri1999 | 0:aa4a1eead8d4 | 537 | } |
tajiri1999 | 0:aa4a1eead8d4 | 538 | } |
tajiri1999 | 0:aa4a1eead8d4 | 539 | else if(Line17 ==0){ |
tajiri1999 | 0:aa4a1eead8d4 | 540 | double xb[18] = {0.93969,0.76604,0.5,0.17364,-0.17364,-0.5,-0.76604,-0.93969,-1,-0.93969,-0.76604,-0.5,-0.17364,0.17364,0.5,0.7660,0.93969,1}; |
tajiri1999 | 0:aa4a1eead8d4 | 541 | double yb[18] = {0.34202,0.64278,0.86602,0.9848,0.9848,0.86602,0.64278,0.34202,0,-0.34202,-0.64278,-0.86602,-0.9848,-0.9848,-0.86602,-0.64278,-0.34202,0}; |
tajiri1999 | 0:aa4a1eead8d4 | 542 | degreebase = 340; |
tajiri1999 | 0:aa4a1eead8d4 | 543 | |
tajiri1999 | 0:aa4a1eead8d4 | 544 | while(1){ |
tajiri1999 | 0:aa4a1eead8d4 | 545 | if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) |
tajiri1999 | 0:aa4a1eead8d4 | 546 | &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ |
tajiri1999 | 0:aa4a1eead8d4 | 547 | |
tajiri1999 | 0:aa4a1eead8d4 | 548 | if(Vx < 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 549 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 550 | } |
tajiri1999 | 0:aa4a1eead8d4 | 551 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 552 | break; |
tajiri1999 | 0:aa4a1eead8d4 | 553 | } |
tajiri1999 | 0:aa4a1eead8d4 | 554 | |
tajiri1999 | 0:aa4a1eead8d4 | 555 | } |
tajiri1999 | 0:aa4a1eead8d4 | 556 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 557 | |
tajiri1999 | 0:aa4a1eead8d4 | 558 | Degree[1] = (degreebase + Linefor(xb,yb) + 180); |
tajiri1999 | 0:aa4a1eead8d4 | 559 | if(Degree[1] >= 360){ |
tajiri1999 | 0:aa4a1eead8d4 | 560 | Degree[1] = Degree[1] - 360; |
tajiri1999 | 0:aa4a1eead8d4 | 561 | } |
tajiri1999 | 0:aa4a1eead8d4 | 562 | else if(Degree[1] <= 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 563 | Degree[1] = Degree[1] + 360; |
tajiri1999 | 0:aa4a1eead8d4 | 564 | } |
tajiri1999 | 0:aa4a1eead8d4 | 565 | DegreeOut = Degree[1]/400 + 0.1; |
tajiri1999 | 0:aa4a1eead8d4 | 566 | } |
tajiri1999 | 0:aa4a1eead8d4 | 567 | } |
tajiri1999 | 0:aa4a1eead8d4 | 568 | } |
tajiri1999 | 0:aa4a1eead8d4 | 569 | //センサーの反応が無いとき |
tajiri1999 | 0:aa4a1eead8d4 | 570 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 571 | DegreeOut = 0; |
tajiri1999 | 0:aa4a1eead8d4 | 572 | Degree[0] = 0; |
tajiri1999 | 0:aa4a1eead8d4 | 573 | |
tajiri1999 | 0:aa4a1eead8d4 | 574 | |
tajiri1999 | 0:aa4a1eead8d4 | 575 | } |
tajiri1999 | 0:aa4a1eead8d4 | 576 | } |
tajiri1999 | 0:aa4a1eead8d4 | 577 | } |
tajiri1999 | 0:aa4a1eead8d4 | 578 | |
tajiri1999 | 0:aa4a1eead8d4 | 579 | |
tajiri1999 | 0:aa4a1eead8d4 | 580 | |
tajiri1999 | 0:aa4a1eead8d4 | 581 | |
tajiri1999 | 0:aa4a1eead8d4 | 582 | |
tajiri1999 | 0:aa4a1eead8d4 | 583 | /*ラインセンサ角度取得*/ |
tajiri1999 | 0:aa4a1eead8d4 | 584 | double Linefor(double x[18],double y[18]){ |
tajiri1999 | 0:aa4a1eead8d4 | 585 | double preDistance = 0;//初期化 |
tajiri1999 | 0:aa4a1eead8d4 | 586 | bool Line[18] = {Line0,Line1,Line2,Line3,Line4,Line5,Line6,Line7,Line8,Line9,Line10,Line11,Line12,Line13,Line14,Line15,Line16,Line17}; |
tajiri1999 | 0:aa4a1eead8d4 | 587 | |
tajiri1999 | 0:aa4a1eead8d4 | 588 | /*3つい上の複数のセンサーが反応したとき最も離れた2点を取得*/ |
tajiri1999 | 0:aa4a1eead8d4 | 589 | for(int i = 0;i <= 17;i++){ |
tajiri1999 | 0:aa4a1eead8d4 | 590 | if(Line[i] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 591 | X[0] = x[i];//一点目のX座標Y座標代入 |
tajiri1999 | 0:aa4a1eead8d4 | 592 | Y[0] = y[i]; |
tajiri1999 | 0:aa4a1eead8d4 | 593 | X[1] = x[i]; |
tajiri1999 | 0:aa4a1eead8d4 | 594 | Y[1] = y[i]; |
tajiri1999 | 0:aa4a1eead8d4 | 595 | ////////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 596 | if(i+1 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 597 | j = i - 17; |
tajiri1999 | 0:aa4a1eead8d4 | 598 | } |
tajiri1999 | 0:aa4a1eead8d4 | 599 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 600 | j= i+1; |
tajiri1999 | 0:aa4a1eead8d4 | 601 | } |
tajiri1999 | 0:aa4a1eead8d4 | 602 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 603 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 604 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 605 | } |
tajiri1999 | 0:aa4a1eead8d4 | 606 | ///////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 607 | if(i-1 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 608 | j = i + 17; |
tajiri1999 | 0:aa4a1eead8d4 | 609 | } |
tajiri1999 | 0:aa4a1eead8d4 | 610 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 611 | j= i-1; |
tajiri1999 | 0:aa4a1eead8d4 | 612 | } |
tajiri1999 | 0:aa4a1eead8d4 | 613 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 614 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 615 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 616 | } |
tajiri1999 | 0:aa4a1eead8d4 | 617 | ///////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 618 | if(i+2 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 619 | j = i - 16; |
tajiri1999 | 0:aa4a1eead8d4 | 620 | } |
tajiri1999 | 0:aa4a1eead8d4 | 621 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 622 | j= i+2; |
tajiri1999 | 0:aa4a1eead8d4 | 623 | } |
tajiri1999 | 0:aa4a1eead8d4 | 624 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 625 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 626 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 627 | } |
tajiri1999 | 0:aa4a1eead8d4 | 628 | ///////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 629 | if(i-2 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 630 | j = i + 16; |
tajiri1999 | 0:aa4a1eead8d4 | 631 | } |
tajiri1999 | 0:aa4a1eead8d4 | 632 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 633 | j= i-2; |
tajiri1999 | 0:aa4a1eead8d4 | 634 | } |
tajiri1999 | 0:aa4a1eead8d4 | 635 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 636 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 637 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 638 | } |
tajiri1999 | 0:aa4a1eead8d4 | 639 | ///////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 640 | if(i+3 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 641 | j = i - 15; |
tajiri1999 | 0:aa4a1eead8d4 | 642 | } |
tajiri1999 | 0:aa4a1eead8d4 | 643 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 644 | j= i+3; |
tajiri1999 | 0:aa4a1eead8d4 | 645 | } |
tajiri1999 | 0:aa4a1eead8d4 | 646 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 647 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 648 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 649 | } |
tajiri1999 | 0:aa4a1eead8d4 | 650 | ////////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 651 | if(i-3 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 652 | j = i + 15; |
tajiri1999 | 0:aa4a1eead8d4 | 653 | } |
tajiri1999 | 0:aa4a1eead8d4 | 654 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 655 | j= i-3; |
tajiri1999 | 0:aa4a1eead8d4 | 656 | } |
tajiri1999 | 0:aa4a1eead8d4 | 657 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 658 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 659 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 660 | } |
tajiri1999 | 0:aa4a1eead8d4 | 661 | /////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 662 | if(i+4 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 663 | j = i - 14; |
tajiri1999 | 0:aa4a1eead8d4 | 664 | } |
tajiri1999 | 0:aa4a1eead8d4 | 665 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 666 | j= i+4; |
tajiri1999 | 0:aa4a1eead8d4 | 667 | } |
tajiri1999 | 0:aa4a1eead8d4 | 668 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 669 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 670 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 671 | } |
tajiri1999 | 0:aa4a1eead8d4 | 672 | //////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 673 | if(i-4 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 674 | j = i + 14; |
tajiri1999 | 0:aa4a1eead8d4 | 675 | } |
tajiri1999 | 0:aa4a1eead8d4 | 676 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 677 | j= i-4; |
tajiri1999 | 0:aa4a1eead8d4 | 678 | } |
tajiri1999 | 0:aa4a1eead8d4 | 679 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 680 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 681 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 682 | } |
tajiri1999 | 0:aa4a1eead8d4 | 683 | ///////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 684 | if(i+5 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 685 | j = i - 13; |
tajiri1999 | 0:aa4a1eead8d4 | 686 | } |
tajiri1999 | 0:aa4a1eead8d4 | 687 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 688 | j= i+5; |
tajiri1999 | 0:aa4a1eead8d4 | 689 | } |
tajiri1999 | 0:aa4a1eead8d4 | 690 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 691 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 692 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 693 | } |
tajiri1999 | 0:aa4a1eead8d4 | 694 | /////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 695 | if(i-5 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 696 | j = i + 13; |
tajiri1999 | 0:aa4a1eead8d4 | 697 | } |
tajiri1999 | 0:aa4a1eead8d4 | 698 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 699 | j= i-5; |
tajiri1999 | 0:aa4a1eead8d4 | 700 | } |
tajiri1999 | 0:aa4a1eead8d4 | 701 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 702 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 703 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 704 | } |
tajiri1999 | 0:aa4a1eead8d4 | 705 | //////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 706 | if(i+6 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 707 | j = i - 12; |
tajiri1999 | 0:aa4a1eead8d4 | 708 | } |
tajiri1999 | 0:aa4a1eead8d4 | 709 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 710 | j= i+6; |
tajiri1999 | 0:aa4a1eead8d4 | 711 | } |
tajiri1999 | 0:aa4a1eead8d4 | 712 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 713 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 714 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 715 | } |
tajiri1999 | 0:aa4a1eead8d4 | 716 | /////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 717 | if(i-6 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 718 | j = i + 12; |
tajiri1999 | 0:aa4a1eead8d4 | 719 | } |
tajiri1999 | 0:aa4a1eead8d4 | 720 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 721 | j= i-6; |
tajiri1999 | 0:aa4a1eead8d4 | 722 | } |
tajiri1999 | 0:aa4a1eead8d4 | 723 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 724 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 725 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 726 | } |
tajiri1999 | 0:aa4a1eead8d4 | 727 | ////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 728 | if(i+7 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 729 | j = i - 11; |
tajiri1999 | 0:aa4a1eead8d4 | 730 | } |
tajiri1999 | 0:aa4a1eead8d4 | 731 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 732 | j= i+7; |
tajiri1999 | 0:aa4a1eead8d4 | 733 | } |
tajiri1999 | 0:aa4a1eead8d4 | 734 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 735 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 736 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 737 | } |
tajiri1999 | 0:aa4a1eead8d4 | 738 | ////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 739 | if(i-7 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 740 | j = i + 11; |
tajiri1999 | 0:aa4a1eead8d4 | 741 | } |
tajiri1999 | 0:aa4a1eead8d4 | 742 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 743 | j= i-7; |
tajiri1999 | 0:aa4a1eead8d4 | 744 | } |
tajiri1999 | 0:aa4a1eead8d4 | 745 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 746 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 747 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 748 | } |
tajiri1999 | 0:aa4a1eead8d4 | 749 | ///////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 750 | if(i+8 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 751 | j = i - 10; |
tajiri1999 | 0:aa4a1eead8d4 | 752 | } |
tajiri1999 | 0:aa4a1eead8d4 | 753 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 754 | j= i+8; |
tajiri1999 | 0:aa4a1eead8d4 | 755 | } |
tajiri1999 | 0:aa4a1eead8d4 | 756 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 757 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 758 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 759 | } |
tajiri1999 | 0:aa4a1eead8d4 | 760 | /////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 761 | if(i-8 <= -1){ |
tajiri1999 | 0:aa4a1eead8d4 | 762 | j = i + 10; |
tajiri1999 | 0:aa4a1eead8d4 | 763 | } |
tajiri1999 | 0:aa4a1eead8d4 | 764 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 765 | j= i-8; |
tajiri1999 | 0:aa4a1eead8d4 | 766 | } |
tajiri1999 | 0:aa4a1eead8d4 | 767 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 768 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 769 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 770 | } |
tajiri1999 | 0:aa4a1eead8d4 | 771 | ////////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 772 | if(i+9 >= 18){ |
tajiri1999 | 0:aa4a1eead8d4 | 773 | j = i - 9; |
tajiri1999 | 0:aa4a1eead8d4 | 774 | } |
tajiri1999 | 0:aa4a1eead8d4 | 775 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 776 | j= i+9; |
tajiri1999 | 0:aa4a1eead8d4 | 777 | } |
tajiri1999 | 0:aa4a1eead8d4 | 778 | if(Line[j] == 0){ |
tajiri1999 | 0:aa4a1eead8d4 | 779 | X[1] = x[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 780 | Y[1] = y[j]; |
tajiri1999 | 0:aa4a1eead8d4 | 781 | } |
tajiri1999 | 0:aa4a1eead8d4 | 782 | //////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 783 | //////////////// |
tajiri1999 | 0:aa4a1eead8d4 | 784 | Distance = (X[1]-X[0])*(X[1]-X[0]) + (Y[1]-Y[0])*(Y[1]-Y[0]);//距離の計算 |
tajiri1999 | 0:aa4a1eead8d4 | 785 | |
tajiri1999 | 0:aa4a1eead8d4 | 786 | if(Distance > preDistance){//前ループで計算した距離と比較 |
tajiri1999 | 0:aa4a1eead8d4 | 787 | Vx = (X[0]+X[1])/2; |
tajiri1999 | 0:aa4a1eead8d4 | 788 | Vy = (Y[0]+Y[1])/2; |
tajiri1999 | 0:aa4a1eead8d4 | 789 | preDistance = Distance; |
tajiri1999 | 0:aa4a1eead8d4 | 790 | } |
tajiri1999 | 0:aa4a1eead8d4 | 791 | } |
tajiri1999 | 0:aa4a1eead8d4 | 792 | } |
tajiri1999 | 0:aa4a1eead8d4 | 793 | if(Distance == 0){//2点反応してないとき |
tajiri1999 | 0:aa4a1eead8d4 | 794 | return(Degree[0]); |
tajiri1999 | 0:aa4a1eead8d4 | 795 | } |
tajiri1999 | 0:aa4a1eead8d4 | 796 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 797 | if((Vx == 0) && (Vy == 0)){ ////VyVx が0で角度が出せないとき |
tajiri1999 | 0:aa4a1eead8d4 | 798 | return(Degree[0]); |
tajiri1999 | 0:aa4a1eead8d4 | 799 | } |
tajiri1999 | 0:aa4a1eead8d4 | 800 | else{ |
tajiri1999 | 0:aa4a1eead8d4 | 801 | Degree[0] = atan(Vy/Vx)*(180/Pi); |
tajiri1999 | 0:aa4a1eead8d4 | 802 | return(Degree[0]); |
tajiri1999 | 0:aa4a1eead8d4 | 803 | } |
tajiri1999 | 0:aa4a1eead8d4 | 804 | } |
tajiri1999 | 0:aa4a1eead8d4 | 805 | } |
tajiri1999 | 0:aa4a1eead8d4 | 806 | |
tajiri1999 | 0:aa4a1eead8d4 | 807 |