春ロボ1班(元F3RC4班+) / Mbed 2 deprecated line_sensor

ラインセンサのプログラム 機体中心からライン中心までの距離を返します 16門が2列 「理論上」動く(未検証)

Dependencies:   mbed

main.cpp@0:6042935abd0f, 2019-03-27 (annotated)

Committer:
aoikoizumi
Date:
Wed Mar 27 04:01:47 2019 +0000
Revision:
0:6042935abd0f
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aoikoizumi 0:6042935abd0f 1 #include "mbed.h"
aoikoizumi 0:6042935abd0f 2
aoikoizumi 0:6042935abd0f 3 DigitalOut pulse_1(D13);
aoikoizumi 0:6042935abd0f 4 DigitalOut reset_1(D12);
aoikoizumi 0:6042935abd0f 5 AnalogIn reader_1(PA_0);
aoikoizumi 0:6042935abd0f 6 DigitalIn button(PC_13);
aoikoizumi 0:6042935abd0f 7 //AnalogIn reader_1(PB_0);
aoikoizumi 0:6042935abd0f 8 //DigitalOut pulse_2(NC);
aoikoizumi 0:6042935abd0f 9 //DigitalOut reset_2(NC);
aoikoizumi 0:6042935abd0f 10 //AnalogIn reader_2(NC);
aoikoizumi 0:6042935abd0f 11 double sensor_1[16];
aoikoizumi 0:6042935abd0f 12 double sensor_2[16];
aoikoizumi 0:6042935abd0f 13 int i;
aoikoizumi 0:6042935abd0f 14 int j;
aoikoizumi 0:6042935abd0f 15 int lx;
aoikoizumi 0:6042935abd0f 16 int ly;
aoikoizumi 0:6042935abd0f 17 int state_1[16];
aoikoizumi 0:6042935abd0f 18 int state_2[16];
aoikoizumi 0:6042935abd0f 19 double a;//migigawa ue
aoikoizumi 0:6042935abd0f 20 double b;//hidarigawa ue
aoikoizumi 0:6042935abd0f 21 double c;//migigawa shita
aoikoizumi 0:6042935abd0f 22 double d;//hidarigawa shita
aoikoizumi 0:6042935abd0f 23 double line_dist;
aoikoizumi 0:6042935abd0f 24 int fail;
aoikoizumi 0:6042935abd0f 25
aoikoizumi 0:6042935abd0f 26 void check_line_1();
aoikoizumi 0:6042935abd0f 27 void check_line_2();
aoikoizumi 0:6042935abd0f 28 void doukaku();
aoikoizumi 0:6042935abd0f 29 void distance_line();
aoikoizumi 0:6042935abd0f 30
aoikoizumi 0:6042935abd0f 31 int main()
aoikoizumi 0:6042935abd0f 32 {
aoikoizumi 0:6042935abd0f 33
aoikoizumi 0:6042935abd0f 34 printf("start\r\n");
aoikoizumi 0:6042935abd0f 35 reset_1=1;
aoikoizumi 0:6042935abd0f 36 //reset_2=1;
aoikoizumi 0:6042935abd0f 37 pulse_1=0;
aoikoizumi 0:6042935abd0f 38 //pulse_2=0;
aoikoizumi 0:6042935abd0f 39 wait(0.1);
aoikoizumi 0:6042935abd0f 40 check_line_1();
aoikoizumi 0:6042935abd0f 41 // check_line_2();
aoikoizumi 0:6042935abd0f 42 //doukaku();
aoikoizumi 0:6042935abd0f 43 }
aoikoizumi 0:6042935abd0f 44
aoikoizumi 0:6042935abd0f 45 void check_line_1()
aoikoizumi 0:6042935abd0f 46 {
aoikoizumi 0:6042935abd0f 47 reset_1=0;
aoikoizumi 0:6042935abd0f 48 wait(0.002);
aoikoizumi 0:6042935abd0f 49 pulse_1=1;
aoikoizumi 0:6042935abd0f 50 wait(0.002);
aoikoizumi 0:6042935abd0f 51 pulse_1=0;
aoikoizumi 0:6042935abd0f 52 reset_1=1;
aoikoizumi 0:6042935abd0f 53 for(i=0; i<16; i++) {
aoikoizumi 0:6042935abd0f 54 if(i==0)sensor_1[0]=reader_1.read()*1.5+0.05;
aoikoizumi 0:6042935abd0f 55 else if(i==1)sensor_1[1]=reader_1.read()+0.09;
aoikoizumi 0:6042935abd0f 56 else if(i==2)sensor_1[2]=reader_1.read()+0.07;
aoikoizumi 0:6042935abd0f 57 else if(i==13)sensor_1[13]=reader_1.read()+0.09;
aoikoizumi 0:6042935abd0f 58 else if(i==14)sensor_1[14]=reader_1.read()+0.09;
aoikoizumi 0:6042935abd0f 59 else if(i==15)sensor_1[15]=reader_1.read()*1.25+0.08;
aoikoizumi 0:6042935abd0f 60 else sensor_1[i]=reader_1.read();
aoikoizumi 0:6042935abd0f 61 pulse_1=1;
aoikoizumi 0:6042935abd0f 62 wait(0.0005);
aoikoizumi 0:6042935abd0f 63 pulse_1=0;
aoikoizumi 0:6042935abd0f 64 wait(0.0005);
aoikoizumi 0:6042935abd0f 65 //printf("%d",i );
aoikoizumi 0:6042935abd0f 66 if(i%8==7) {
aoikoizumi 0:6042935abd0f 67 printf("%f\r\n",sensor_1[i]);
aoikoizumi 0:6042935abd0f 68 } else {
aoikoizumi 0:6042935abd0f 69 printf("%f ",sensor_1[i]);
aoikoizumi 0:6042935abd0f 70 }
aoikoizumi 0:6042935abd0f 71 }
aoikoizumi 0:6042935abd0f 72 }
aoikoizumi 0:6042935abd0f 73 /*
aoikoizumi 0:6042935abd0f 74 void check_line_2()
aoikoizumi 0:6042935abd0f 75 {
aoikoizumi 0:6042935abd0f 76 reset_2=0;
aoikoizumi 0:6042935abd0f 77 wait(0.002);
aoikoizumi 0:6042935abd0f 78 pulse_2=1;
aoikoizumi 0:6042935abd0f 79 wait(0.002);
aoikoizumi 0:6042935abd0f 80 pulse_2=0;
aoikoizumi 0:6042935abd0f 81 reset_2=1;
aoikoizumi 0:6042935abd0f 82 for(j=0; j<16; j++) {
aoikoizumi 0:6042935abd0f 83 if(j==0)sensor_2[0]=reader_2.read()*1.5+0.05;
aoikoizumi 0:6042935abd0f 84 else if(j==1)sensor_2[1]=reader_2.read()+0.09;
aoikoizumi 0:6042935abd0f 85 else if(j==2)sensor_2[2]=reader_2.read()+0.07;
aoikoizumi 0:6042935abd0f 86 else if(j==13)sensor_2[13]=reader_2.read()+0.09;
aoikoizumi 0:6042935abd0f 87 else if(j==14)sensor_2[14]=reader_2.read()+0.09;
aoikoizumi 0:6042935abd0f 88 else if(j==15)sensor_2[15]=reader_2.read()*1.25+0.08;
aoikoizumi 0:6042935abd0f 89 else sensor_2[j]=reader_2.read();
aoikoizumi 0:6042935abd0f 90 pulse_2=1;
aoikoizumi 0:6042935abd0f 91 wait(0.0005);
aoikoizumi 0:6042935abd0f 92 pulse_2=0;
aoikoizumi 0:6042935abd0f 93 wait(0.0005);
aoikoizumi 0:6042935abd0f 94 //printf("%d",j );
aoikoizumi 0:6042935abd0f 95 if(j%8==7) {
aoikoizumi 0:6042935abd0f 96 printf("%f\r\n",sensor_2[j]);
aoikoizumi 0:6042935abd0f 97 } else {
aoikoizumi 0:6042935abd0f 98 printf("%f ",sensor_2[j]);
aoikoizumi 0:6042935abd0f 99 }
aoikoizumi 0:6042935abd0f 100 }
aoikoizumi 0:6042935abd0f 101 }
aoikoizumi 0:6042935abd0f 102 */
aoikoizumi 0:6042935abd0f 103 /*
aoikoizumi 0:6042935abd0f 104 void doukaku()
aoikoizumi 0:6042935abd0f 105 {
aoikoizumi 0:6042935abd0f 106 pulse_1=0;
aoikoizumi 0:6042935abd0f 107 reset_1=1;
aoikoizumi 0:6042935abd0f 108 while(1) {
aoikoizumi 0:6042935abd0f 109 if(!button) {
aoikoizumi 0:6042935abd0f 110 pulse_1=1;
aoikoizumi 0:6042935abd0f 111 wait(0.1);
aoikoizumi 0:6042935abd0f 112 pulse_1=0;
aoikoizumi 0:6042935abd0f 113 wait(0.1);
aoikoizumi 0:6042935abd0f 114 }
aoikoizumi 0:6042935abd0f 115 }
aoikoizumi 0:6042935abd0f 116 }
aoikoizumi 0:6042935abd0f 117 */
aoikoizumi 0:6042935abd0f 118 void color_adj_green()
aoikoizumi 0:6042935abd0f 119 {
aoikoizumi 0:6042935abd0f 120 for(i=0; i<16; i++) {//閾値
aoikoizumi 0:6042935abd0f 121 if(sensor_1[i]>0.8)state_1[i]=0;//white
aoikoizumi 0:6042935abd0f 122 else if(sensor_1[i]>0.4)state_1[i]=1;//
aoikoizumi 0:6042935abd0f 123 else if(sensor_1[i]>0.1)state_1[i]=2;//green
aoikoizumi 0:6042935abd0f 124 else state_1[i]=3;
aoikoizumi 0:6042935abd0f 125 }
aoikoizumi 0:6042935abd0f 126 for(j=0; j<16; j++) {//閾値
aoikoizumi 0:6042935abd0f 127 if(sensor_2[j]>0.8)state_2[j]=0;
aoikoizumi 0:6042935abd0f 128 else if(sensor_2[j]>0.4)state_2[j]=1;
aoikoizumi 0:6042935abd0f 129 else if(sensor_2[j]>0.1)state_2[j]=2;
aoikoizumi 0:6042935abd0f 130 else state_2[j]=3;
aoikoizumi 0:6042935abd0f 131 }
aoikoizumi 0:6042935abd0f 132 for(i=0; i<16; i++) {//データ補正
aoikoizumi 0:6042935abd0f 133 if(state_1[i]==1) { //境界上
aoikoizumi 0:6042935abd0f 134 if(i!=0&&i!=15&&state_1[i+1]==0&&state_1[i-1]==0) { //エラー値の除去(white)
aoikoizumi 0:6042935abd0f 135 state_1[i]=0;
aoikoizumi 0:6042935abd0f 136 } else if(i!=0&&i!=15&&state_1[i+1]==2&&state_1[i-1]==2) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 137 state_1[i]=2;
aoikoizumi 0:6042935abd0f 138 }
aoikoizumi 0:6042935abd0f 139 }
aoikoizumi 0:6042935abd0f 140 if(state_1[i]==3) { //エラー値
aoikoizumi 0:6042935abd0f 141 if(i!=0&&i!=15&&state_1[i+1]==0&&state_1[i-1]==0) { //エラー値の除去(white)
aoikoizumi 0:6042935abd0f 142 state_1[i]=0;
aoikoizumi 0:6042935abd0f 143 } else if(i!=0&&i!=15&&state_1[i+1]==2&&state_1[i-1]==2) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 144 state_1[i]=2;
aoikoizumi 0:6042935abd0f 145 } else if(i!=0&&i!=15&&state_1[i+1]==1&&state_1[i-1]==2) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 146 state_1[i]=2;
aoikoizumi 0:6042935abd0f 147 } else if(i!=0&&i!=15&&state_1[i+1]==2&&state_1[i-1]==1) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 148 state_1[i]=2;
aoikoizumi 0:6042935abd0f 149 } else if(i!=0&&i!=15&&state_1[i+1]==1&&state_1[i-1]==0) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 150 state_1[i]=0;
aoikoizumi 0:6042935abd0f 151 } else if(i!=0&&i!=15&&state_1[i+1]==0&&state_1[i-1]==1) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 152 state_1[i]=0;
aoikoizumi 0:6042935abd0f 153 }
aoikoizumi 0:6042935abd0f 154 }
aoikoizumi 0:6042935abd0f 155 }
aoikoizumi 0:6042935abd0f 156 for(j=0; j<16; j++) {//閾値
aoikoizumi 0:6042935abd0f 157 if(sensor_2[j]>0.8)state_2[j]=0;
aoikoizumi 0:6042935abd0f 158 else if(sensor_2[j]>0.4)state_2[j]=1;
aoikoizumi 0:6042935abd0f 159 else if(sensor_2[j]>0.1)state_2[j]=2;
aoikoizumi 0:6042935abd0f 160 else state_2[j]=3;
aoikoizumi 0:6042935abd0f 161 }
aoikoizumi 0:6042935abd0f 162 for(j=0; j<16; j++) {//データ補正
aoikoizumi 0:6042935abd0f 163 if(state_1[j]==1) { //境界上
aoikoizumi 0:6042935abd0f 164 if(j!=0&&j!=15&&state_1[j+1]==0&&state_1[j-1]==0) { //エラー値の除去(white)
aoikoizumi 0:6042935abd0f 165 state_1[j]=0;
aoikoizumi 0:6042935abd0f 166 } else if(j!=0&&j!=15&&state_1[j+1]==2&&state_1[j-1]==2) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 167 state_1[j]=2;
aoikoizumi 0:6042935abd0f 168 }
aoikoizumi 0:6042935abd0f 169 }
aoikoizumi 0:6042935abd0f 170 if(state_1[j]==3) { //エラー値
aoikoizumi 0:6042935abd0f 171 if(j!=0&&j!=15&&state_1[j+1]==0&&state_1[j-1]==0) { //エラー値の除去(white)
aoikoizumi 0:6042935abd0f 172 state_1[j]=0;
aoikoizumi 0:6042935abd0f 173 } else if(j!=0&&j!=15&&state_1[j+1]==2&&state_1[j-1]==2) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 174 state_1[j]=2;
aoikoizumi 0:6042935abd0f 175 } else if(j!=0&&j!=15&&state_1[j+1]==1&&state_1[j-1]==2) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 176 state_1[j]=2;
aoikoizumi 0:6042935abd0f 177 } else if(j!=0&&j!=15&&state_1[j+1]==2&&state_1[j-1]==1) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 178 state_1[j]=2;
aoikoizumi 0:6042935abd0f 179 } else if(j!=0&&j!=15&&state_1[j+1]==1&&state_1[j-1]==0) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 180 state_1[j]=0;
aoikoizumi 0:6042935abd0f 181 } else if(j!=0&&j!=15&&state_1[j+1]==0&&state_1[j-1]==1) { //エラー値の除去(green)
aoikoizumi 0:6042935abd0f 182 state_1[j]=0;
aoikoizumi 0:6042935abd0f 183 }
aoikoizumi 0:6042935abd0f 184 }
aoikoizumi 0:6042935abd0f 185 }
aoikoizumi 0:6042935abd0f 186 }
aoikoizumi 0:6042935abd0f 187 void dist_line()
aoikoizumi 0:6042935abd0f 188 {
aoikoizumi 0:6042935abd0f 189 a=100;
aoikoizumi 0:6042935abd0f 190 b=100;
aoikoizumi 0:6042935abd0f 191 c=100;
aoikoizumi 0:6042935abd0f 192 d=100;
aoikoizumi 0:6042935abd0f 193 fail=0;
aoikoizumi 0:6042935abd0f 194 if(state_1[i]==1) {
aoikoizumi 0:6042935abd0f 195 if(i>=0&&i<3) { //境界をaまたはcと定義
aoikoizumi 0:6042935abd0f 196 if(a==100)a=90.25-15*i;
aoikoizumi 0:6042935abd0f 197 else if(c==100)c=90.25-15*i;
aoikoizumi 0:6042935abd0f 198 else fail=1; //エラー回の除去
aoikoizumi 0:6042935abd0f 199 }
aoikoizumi 0:6042935abd0f 200 if(i>=3&&i<8) {
aoikoizumi 0:6042935abd0f 201 if(a==100)a=76.5-10*i;
aoikoizumi 0:6042935abd0f 202 else if(c==100)c=76.5-10*i;
aoikoizumi 0:6042935abd0f 203 else fail=1;
aoikoizumi 0:6042935abd0f 204 }
aoikoizumi 0:6042935abd0f 205 if(i>=8&&i<13) {
aoikoizumi 0:6042935abd0f 206 if(a==100)a=73.5-10*i;
aoikoizumi 0:6042935abd0f 207 else if(c==100)c=73.5-10*i;
aoikoizumi 0:6042935abd0f 208 else fail=1;
aoikoizumi 0:6042935abd0f 209 }
aoikoizumi 0:6042935abd0f 210 if(i>=13&&i<16) {
aoikoizumi 0:6042935abd0f 211 if(a==100)a=133.75-15*i;
aoikoizumi 0:6042935abd0f 212 else if(c==100)c=133.75-15*i;
aoikoizumi 0:6042935abd0f 213 else fail=1;
aoikoizumi 0:6042935abd0f 214 }
aoikoizumi 0:6042935abd0f 215 } else if(state_1[i]==0) { //白線上
aoikoizumi 0:6042935abd0f 216 if(i!=0&&state_1[i-1]==2) {
aoikoizumi 0:6042935abd0f 217 if(i>=0&&i<3) { //境界をaまたはcと定義
aoikoizumi 0:6042935abd0f 218 if(a==100)a=90.25-15*i;
aoikoizumi 0:6042935abd0f 219 else if(c==100)c=90.25-15*i;
aoikoizumi 0:6042935abd0f 220 else fail=1; //エラー回の除去
aoikoizumi 0:6042935abd0f 221 }
aoikoizumi 0:6042935abd0f 222 if(i>=3&&i<8) {
aoikoizumi 0:6042935abd0f 223 if(a==100)a=76.5-10*i;
aoikoizumi 0:6042935abd0f 224 else if(c==100)c=76.5-10*i;
aoikoizumi 0:6042935abd0f 225 else fail=1;
aoikoizumi 0:6042935abd0f 226 }
aoikoizumi 0:6042935abd0f 227 if(i>=8&&i<13) {
aoikoizumi 0:6042935abd0f 228 if(a==100)a=73.5-10*i;
aoikoizumi 0:6042935abd0f 229 else if(c==100)c=73.5-10*i;
aoikoizumi 0:6042935abd0f 230 else fail=1;
aoikoizumi 0:6042935abd0f 231 }
aoikoizumi 0:6042935abd0f 232 if(i>=13&&i<16) {
aoikoizumi 0:6042935abd0f 233 if(a==100)a=133.75-15*i;
aoikoizumi 0:6042935abd0f 234 else if(c==100)c=133.75-15*i;
aoikoizumi 0:6042935abd0f 235 else fail=1;
aoikoizumi 0:6042935abd0f 236 }
aoikoizumi 0:6042935abd0f 237 }
aoikoizumi 0:6042935abd0f 238
aoikoizumi 0:6042935abd0f 239 if(state_1[j]==1) {
aoikoizumi 0:6042935abd0f 240 if(j>=0&&j<3) { //境界をbまたはdと定義
aoikoizumi 0:6042935abd0f 241 if(b==100)b=90.25-15*j;
aoikoizumi 0:6042935abd0f 242 else if(d==100)d=90.25-15*j;
aoikoizumi 0:6042935abd0f 243 else fail=1; //エラー回の除去
aoikoizumi 0:6042935abd0f 244 }
aoikoizumi 0:6042935abd0f 245 if(j>=3&&j<8) {
aoikoizumi 0:6042935abd0f 246 if(b==100)b=76.5-10*j;
aoikoizumi 0:6042935abd0f 247 else if(d==100)d=76.5-10*j;
aoikoizumi 0:6042935abd0f 248 else fail=1;
aoikoizumi 0:6042935abd0f 249 }
aoikoizumi 0:6042935abd0f 250 if(j>=8&&j<13) {
aoikoizumi 0:6042935abd0f 251 if(b==100)b=73.5-10*j;
aoikoizumi 0:6042935abd0f 252 else if(d==100)d=73.5-10*j;
aoikoizumi 0:6042935abd0f 253 else fail=1;
aoikoizumi 0:6042935abd0f 254 }
aoikoizumi 0:6042935abd0f 255 if(j>=13&&j<16) {
aoikoizumi 0:6042935abd0f 256 if(b==100)b=133.75-15*j;
aoikoizumi 0:6042935abd0f 257 else if(d==100)d=133.75-15*j;
aoikoizumi 0:6042935abd0f 258 else fail=1;
aoikoizumi 0:6042935abd0f 259 }
aoikoizumi 0:6042935abd0f 260 } else if(state_1[j]==0) { //白線上
aoikoizumi 0:6042935abd0f 261 if(j!=0&&state_1[j-1]==2) {
aoikoizumi 0:6042935abd0f 262 if(j>=0&&j<3) { //境界をbまたはdと定義
aoikoizumi 0:6042935abd0f 263 if(b==100)b=90.25-15*j;
aoikoizumi 0:6042935abd0f 264 else if(d==100)d=90.25-15*j;
aoikoizumi 0:6042935abd0f 265 else fail=1; //エラー回の除去
aoikoizumi 0:6042935abd0f 266 }
aoikoizumi 0:6042935abd0f 267 if(j>=3&&j<8) {
aoikoizumi 0:6042935abd0f 268 if(b==100)b=76.5-10*j;
aoikoizumi 0:6042935abd0f 269 else if(d==100)d=76.5-10*j;
aoikoizumi 0:6042935abd0f 270 else fail=1;
aoikoizumi 0:6042935abd0f 271 }
aoikoizumi 0:6042935abd0f 272 if(j>=8&&j<13) {
aoikoizumi 0:6042935abd0f 273 if(b==100)b=73.5-10*j;
aoikoizumi 0:6042935abd0f 274 else if(d==100)d=73.5-10*j;
aoikoizumi 0:6042935abd0f 275 else fail=1;
aoikoizumi 0:6042935abd0f 276 }
aoikoizumi 0:6042935abd0f 277 if(j>=13&&j<16) {
aoikoizumi 0:6042935abd0f 278 if(b==100)b=133.75-15*j;
aoikoizumi 0:6042935abd0f 279 else if(d==100)d=133.75-15*j;
aoikoizumi 0:6042935abd0f 280 else fail=1;
aoikoizumi 0:6042935abd0f 281 }
aoikoizumi 0:6042935abd0f 282 }
aoikoizumi 0:6042935abd0f 283 }
aoikoizumi 0:6042935abd0f 284 }
aoikoizumi 0:6042935abd0f 285 }
aoikoizumi 0:6042935abd0f 286 void calc_line()
aoikoizumi 0:6042935abd0f 287 {
aoikoizumi 0:6042935abd0f 288 if(fail!=0) {
aoikoizumi 0:6042935abd0f 289
aoikoizumi 0:6042935abd0f 290 } else {
aoikoizumi 0:6042935abd0f 291 double theta;
aoikoizumi 0:6042935abd0f 292 theta=atan((a-b+c-d)*ly/2);
aoikoizumi 0:6042935abd0f 293 line_dist=(a+b+c+d)*cos(theta)/4;
aoikoizumi 0:6042935abd0f 294 }
aoikoizumi 0:6042935abd0f 295 }