![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
Diff: main.cpp
- Revision:
- 0:aa4a1eead8d4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Apr 01 14:27:34 2018 +0000 @@ -0,0 +1,807 @@ + #include "mbed.h" + + /*Nucleo STM32F303k8で開発しました.*/ +DigitalIn Line0(A7);//0 +DigitalIn Line1(A1);//20 +DigitalIn Line2(A0);//40 +DigitalIn Line3(D13);//60 +DigitalIn Line4(D7);//80 +DigitalIn Line5(D10);//100 +DigitalIn Line6(D11);//120 +DigitalIn Line7(D12);//140 +DigitalIn Line8(D8);//160 +DigitalIn Line9(D9);//180 +DigitalIn Line10(D6);//200 +DigitalIn Line11(D5);//220 +DigitalIn Line12(D4);//240 +DigitalIn Line13(D0);//260 +DigitalIn Line14(D2);//280 +DigitalIn Line15(D3);//300 +DigitalIn Line16(A5);//320 +DigitalIn Line17(A6);//340 + +/*DegreeOutの出力が0Vのときライン反応無し,0.33V~3.3Vのときライン反応有*/ +AnalogOut DegreeOut(A3); + + +double Linefor(double x[18],double y[18]); + +int j; +const float Pi = 3.1415926; +double X[2]; +double Y[2]; +double Vx,Vy; +double Distance,degreebase; +double Degree[2]; + + + + +int main(){ + while(1){ + + + if(Line0 == 0){//0度にあるセンサーの反応があるとき + 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}; + 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}; + degreebase = 0; + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180);//180°足すのは求めた角度の反対側に進むため + if(Degree[1] >= 360){//定義域を0~360に変換 + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line1 == 0){ //20 + 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}; + 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}; + degreebase = 20; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){//角度が反転した状態でラインから出たとき + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line2 ==0){//40 + 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}; + 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}; + + degreebase = 40; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line3 ==0){ + 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}; + 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}; + degreebase = 60; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line4 ==0){ + 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}; + 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}; + degreebase = 80; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line5 ==0){//100 + 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}; + 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}; + degreebase = 100; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line6 ==0){//120 + 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}; + 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}; + degreebase = 120; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + } + else{ + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line7 ==0){ + 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}; + 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}; + degreebase = 140; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line8 ==0){ + 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}; + 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}; + degreebase = 160; + + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line9 ==0){ + 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}; + 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}; + degreebase = 180; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line10 ==0){ + 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}; + 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}; + degreebase = 200; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line11 ==0){ + 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}; + 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}; + degreebase = 220; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line12 ==0){ + 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}; + 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}; + degreebase = 240; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line13 ==0){ + 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}; + 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}; + degreebase = 260; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line14==0){ + 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}; + 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}; + degreebase = 280; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line15 ==0){ + 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}; + 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}; + degreebase = 300; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line16 ==0){ + 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}; + 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}; + degreebase = 320; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + else if(Line17 ==0){ + 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}; + 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}; + degreebase = 340; + + while(1){ + if((Line0==1)&&(Line1==1)&&(Line2==1)&&(Line3==1)&&(Line4==1)&&(Line5==1)&&(Line6==1)&&(Line7==1)&&(Line8==1)&&(Line9==1) + &&(Line10==1)&&(Line11==1)&&(Line12==1)&&(Line13==1)&&(Line14==1)&&(Line15==1)&&(Line16==1)&&(Line17==1)){ + + if(Vx < 0){ + DegreeOut = Degree[1]/400 + 0.1; + } + else{ + break; + } + + } + else{ + + Degree[1] = (degreebase + Linefor(xb,yb) + 180); + if(Degree[1] >= 360){ + Degree[1] = Degree[1] - 360; + } + else if(Degree[1] <= 0){ + Degree[1] = Degree[1] + 360; + } + DegreeOut = Degree[1]/400 + 0.1; + } + } + } + //センサーの反応が無いとき + else{ + DegreeOut = 0; + Degree[0] = 0; + + + } + } +} + + + + + +/*ラインセンサ角度取得*/ +double Linefor(double x[18],double y[18]){ + double preDistance = 0;//初期化 + bool Line[18] = {Line0,Line1,Line2,Line3,Line4,Line5,Line6,Line7,Line8,Line9,Line10,Line11,Line12,Line13,Line14,Line15,Line16,Line17}; + + /*3つい上の複数のセンサーが反応したとき最も離れた2点を取得*/ + for(int i = 0;i <= 17;i++){ + if(Line[i] == 0){ + X[0] = x[i];//一点目のX座標Y座標代入 + Y[0] = y[i]; + X[1] = x[i]; + Y[1] = y[i]; + ////////////////////// + if(i+1 >= 18){ + j = i - 17; + } + else{ + j= i+1; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ///////////////////// + if(i-1 <= -1){ + j = i + 17; + } + else{ + j= i-1; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ///////////////////// + if(i+2 >= 18){ + j = i - 16; + } + else{ + j= i+2; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ///////////////////// + if(i-2 <= -1){ + j = i + 16; + } + else{ + j= i-2; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ///////////////////// + if(i+3 >= 18){ + j = i - 15; + } + else{ + j= i+3; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ////////////////////// + if(i-3 <= -1){ + j = i + 15; + } + else{ + j= i-3; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + /////////////////// + if(i+4 >= 18){ + j = i - 14; + } + else{ + j= i+4; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + //////////////////// + if(i-4 <= -1){ + j = i + 14; + } + else{ + j= i-4; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ///////////////////// + if(i+5 >= 18){ + j = i - 13; + } + else{ + j= i+5; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + /////////////////// + if(i-5 <= -1){ + j = i + 13; + } + else{ + j= i-5; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + //////////////////// + if(i+6 >= 18){ + j = i - 12; + } + else{ + j= i+6; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + /////////////////// + if(i-6 <= -1){ + j = i + 12; + } + else{ + j= i-6; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ////////////////// + if(i+7 >= 18){ + j = i - 11; + } + else{ + j= i+7; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ////////////////// + if(i-7 <= -1){ + j = i + 11; + } + else{ + j= i-7; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ///////////////// + if(i+8 >= 18){ + j = i - 10; + } + else{ + j= i+8; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + /////////////// + if(i-8 <= -1){ + j = i + 10; + } + else{ + j= i-8; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + ////////////////// + if(i+9 >= 18){ + j = i - 9; + } + else{ + j= i+9; + } + if(Line[j] == 0){ + X[1] = x[j]; + Y[1] = y[j]; + } + //////////////// + //////////////// + Distance = (X[1]-X[0])*(X[1]-X[0]) + (Y[1]-Y[0])*(Y[1]-Y[0]);//距離の計算 + + if(Distance > preDistance){//前ループで計算した距離と比較 + Vx = (X[0]+X[1])/2; + Vy = (Y[0]+Y[1])/2; + preDistance = Distance; + } + } + } + if(Distance == 0){//2点反応してないとき + return(Degree[0]); + } + else{ + if((Vx == 0) && (Vy == 0)){ ////VyVx が0で角度が出せないとき + return(Degree[0]); + } + else{ + Degree[0] = atan(Vy/Vx)*(180/Pi); + return(Degree[0]); + } + } +} + +