春ロボ1班(元F3RC4班+)
ラインセンサのプログラム 機体中心からライン中心までの距離を返します 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;
}
}