ハヤト タジリ
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
- Committer:
- tajiri1999
- Date:
- 2018-04-01
- Revision:
- 0:aa4a1eead8d4
File content as of revision 0:aa4a1eead8d4:
#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]);
}
}
}