春ロボ1班(元F3RC4班+)
ラインセンサのプログラム 機体中心からライン中心までの距離を返します 16門が2列 「理論上」動く(未検証)
Dependencies: mbed
Revision 0:6042935abd0f, committed 2019-03-27
- Comitter:
- aoikoizumi
- Date:
- Wed Mar 27 04:01:47 2019 +0000
- Commit message:
- A
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 6042935abd0f main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 27 04:01:47 2019 +0000
@@ -0,0 +1,295 @@
+#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;
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 6042935abd0f mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Mar 27 04:01:47 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file