ラインセンサのプログラム 機体中心からライン中心までの距離を返します 16門が2列 「理論上」動く(未検証)
Dependencies: mbed
main.cpp
- Committer:
- aoikoizumi
- Date:
- 2019-03-27
- Revision:
- 0:6042935abd0f
File content as of revision 0:6042935abd0f:
#include "mbed.h" DigitalOut pulse_1(D13); DigitalOut reset_1(D12); AnalogIn reader_1(PA_0); DigitalIn button(PC_13); //AnalogIn reader_1(PB_0); //DigitalOut pulse_2(NC); //DigitalOut reset_2(NC); //AnalogIn reader_2(NC); double sensor_1[16]; double sensor_2[16]; int i; int j; int lx; int ly; int state_1[16]; int state_2[16]; double a;//migigawa ue double b;//hidarigawa ue double c;//migigawa shita double d;//hidarigawa shita double line_dist; int fail; void check_line_1(); void check_line_2(); void doukaku(); void distance_line(); int main() { printf("start\r\n"); reset_1=1; //reset_2=1; pulse_1=0; //pulse_2=0; wait(0.1); check_line_1(); // check_line_2(); //doukaku(); } void check_line_1() { reset_1=0; wait(0.002); pulse_1=1; wait(0.002); pulse_1=0; reset_1=1; for(i=0; i<16; i++) { if(i==0)sensor_1[0]=reader_1.read()*1.5+0.05; else if(i==1)sensor_1[1]=reader_1.read()+0.09; else if(i==2)sensor_1[2]=reader_1.read()+0.07; else if(i==13)sensor_1[13]=reader_1.read()+0.09; else if(i==14)sensor_1[14]=reader_1.read()+0.09; else if(i==15)sensor_1[15]=reader_1.read()*1.25+0.08; else sensor_1[i]=reader_1.read(); pulse_1=1; wait(0.0005); pulse_1=0; wait(0.0005); //printf("%d",i ); if(i%8==7) { printf("%f\r\n",sensor_1[i]); } else { printf("%f ",sensor_1[i]); } } } /* void check_line_2() { reset_2=0; wait(0.002); pulse_2=1; wait(0.002); pulse_2=0; reset_2=1; for(j=0; j<16; j++) { if(j==0)sensor_2[0]=reader_2.read()*1.5+0.05; else if(j==1)sensor_2[1]=reader_2.read()+0.09; else if(j==2)sensor_2[2]=reader_2.read()+0.07; else if(j==13)sensor_2[13]=reader_2.read()+0.09; else if(j==14)sensor_2[14]=reader_2.read()+0.09; else if(j==15)sensor_2[15]=reader_2.read()*1.25+0.08; else sensor_2[j]=reader_2.read(); pulse_2=1; wait(0.0005); pulse_2=0; wait(0.0005); //printf("%d",j ); if(j%8==7) { printf("%f\r\n",sensor_2[j]); } else { printf("%f ",sensor_2[j]); } } } */ /* void doukaku() { pulse_1=0; reset_1=1; while(1) { if(!button) { pulse_1=1; wait(0.1); pulse_1=0; wait(0.1); } } } */ void color_adj_green() { for(i=0; i<16; i++) {//閾値 if(sensor_1[i]>0.8)state_1[i]=0;//white else if(sensor_1[i]>0.4)state_1[i]=1;// else if(sensor_1[i]>0.1)state_1[i]=2;//green else state_1[i]=3; } for(j=0; j<16; j++) {//閾値 if(sensor_2[j]>0.8)state_2[j]=0; else if(sensor_2[j]>0.4)state_2[j]=1; else if(sensor_2[j]>0.1)state_2[j]=2; else state_2[j]=3; } for(i=0; i<16; i++) {//データ補正 if(state_1[i]==1) { //境界上 if(i!=0&&i!=15&&state_1[i+1]==0&&state_1[i-1]==0) { //エラー値の除去(white) state_1[i]=0; } else if(i!=0&&i!=15&&state_1[i+1]==2&&state_1[i-1]==2) { //エラー値の除去(green) state_1[i]=2; } } if(state_1[i]==3) { //エラー値 if(i!=0&&i!=15&&state_1[i+1]==0&&state_1[i-1]==0) { //エラー値の除去(white) state_1[i]=0; } else if(i!=0&&i!=15&&state_1[i+1]==2&&state_1[i-1]==2) { //エラー値の除去(green) state_1[i]=2; } else if(i!=0&&i!=15&&state_1[i+1]==1&&state_1[i-1]==2) { //エラー値の除去(green) state_1[i]=2; } else if(i!=0&&i!=15&&state_1[i+1]==2&&state_1[i-1]==1) { //エラー値の除去(green) state_1[i]=2; } else if(i!=0&&i!=15&&state_1[i+1]==1&&state_1[i-1]==0) { //エラー値の除去(green) state_1[i]=0; } else if(i!=0&&i!=15&&state_1[i+1]==0&&state_1[i-1]==1) { //エラー値の除去(green) state_1[i]=0; } } } for(j=0; j<16; j++) {//閾値 if(sensor_2[j]>0.8)state_2[j]=0; else if(sensor_2[j]>0.4)state_2[j]=1; else if(sensor_2[j]>0.1)state_2[j]=2; else state_2[j]=3; } for(j=0; j<16; j++) {//データ補正 if(state_1[j]==1) { //境界上 if(j!=0&&j!=15&&state_1[j+1]==0&&state_1[j-1]==0) { //エラー値の除去(white) state_1[j]=0; } else if(j!=0&&j!=15&&state_1[j+1]==2&&state_1[j-1]==2) { //エラー値の除去(green) state_1[j]=2; } } if(state_1[j]==3) { //エラー値 if(j!=0&&j!=15&&state_1[j+1]==0&&state_1[j-1]==0) { //エラー値の除去(white) state_1[j]=0; } else if(j!=0&&j!=15&&state_1[j+1]==2&&state_1[j-1]==2) { //エラー値の除去(green) state_1[j]=2; } else if(j!=0&&j!=15&&state_1[j+1]==1&&state_1[j-1]==2) { //エラー値の除去(green) state_1[j]=2; } else if(j!=0&&j!=15&&state_1[j+1]==2&&state_1[j-1]==1) { //エラー値の除去(green) state_1[j]=2; } else if(j!=0&&j!=15&&state_1[j+1]==1&&state_1[j-1]==0) { //エラー値の除去(green) state_1[j]=0; } else if(j!=0&&j!=15&&state_1[j+1]==0&&state_1[j-1]==1) { //エラー値の除去(green) state_1[j]=0; } } } } void dist_line() { a=100; b=100; c=100; d=100; fail=0; if(state_1[i]==1) { if(i>=0&&i<3) { //境界をaまたはcと定義 if(a==100)a=90.25-15*i; else if(c==100)c=90.25-15*i; else fail=1; //エラー回の除去 } if(i>=3&&i<8) { if(a==100)a=76.5-10*i; else if(c==100)c=76.5-10*i; else fail=1; } if(i>=8&&i<13) { if(a==100)a=73.5-10*i; else if(c==100)c=73.5-10*i; else fail=1; } if(i>=13&&i<16) { if(a==100)a=133.75-15*i; else if(c==100)c=133.75-15*i; else fail=1; } } else if(state_1[i]==0) { //白線上 if(i!=0&&state_1[i-1]==2) { if(i>=0&&i<3) { //境界をaまたはcと定義 if(a==100)a=90.25-15*i; else if(c==100)c=90.25-15*i; else fail=1; //エラー回の除去 } if(i>=3&&i<8) { if(a==100)a=76.5-10*i; else if(c==100)c=76.5-10*i; else fail=1; } if(i>=8&&i<13) { if(a==100)a=73.5-10*i; else if(c==100)c=73.5-10*i; else fail=1; } if(i>=13&&i<16) { if(a==100)a=133.75-15*i; else if(c==100)c=133.75-15*i; else fail=1; } } if(state_1[j]==1) { if(j>=0&&j<3) { //境界をbまたはdと定義 if(b==100)b=90.25-15*j; else if(d==100)d=90.25-15*j; else fail=1; //エラー回の除去 } if(j>=3&&j<8) { if(b==100)b=76.5-10*j; else if(d==100)d=76.5-10*j; else fail=1; } if(j>=8&&j<13) { if(b==100)b=73.5-10*j; else if(d==100)d=73.5-10*j; else fail=1; } if(j>=13&&j<16) { if(b==100)b=133.75-15*j; else if(d==100)d=133.75-15*j; else fail=1; } } else if(state_1[j]==0) { //白線上 if(j!=0&&state_1[j-1]==2) { if(j>=0&&j<3) { //境界をbまたはdと定義 if(b==100)b=90.25-15*j; else if(d==100)d=90.25-15*j; else fail=1; //エラー回の除去 } if(j>=3&&j<8) { if(b==100)b=76.5-10*j; else if(d==100)d=76.5-10*j; else fail=1; } if(j>=8&&j<13) { if(b==100)b=73.5-10*j; else if(d==100)d=73.5-10*j; else fail=1; } if(j>=13&&j<16) { if(b==100)b=133.75-15*j; else if(d==100)d=133.75-15*j; else fail=1; } } } } } void calc_line() { if(fail!=0) { } else { double theta; theta=atan((a-b+c-d)*ly/2); line_dist=(a+b+c+d)*cos(theta)/4; } }