yikestone/Embedded_codes
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Embedded_codes/ir_maze_solver/sketch_jan05/sketch_jan05.ino
yikestone c932efe7ff initial
2018-02-18 14:02:33 +05:30

306 lines
7.7 KiB
C++
Raw Blame History

int A[11][11];
int b[6][2]={{7,3},{0,0}};
int ch[20][2];int nch=-1;
int nb=2,ib=0,M[18],im;
int ix,iy;
int X,Y,D;
int in[10];
int ma[7]={0,0,0,0,0,0,0};
int mi[7]={0,500,500,0,500,500,0};
int md[]={8,7,9,3,4,5};
int cv;int m,pcv;
float kp=0.19,ki=0.00008,kd=0.05;
int a;
int mov(){long tt=millis();
int bbb=0;int bb1=0;int b=0;int aa=0;int perr;long cerr=0;int pwma=0,pwmb=0,pwmm=180;
while((b==0)||(in[7]==0)||(in[9]==0)){
takeInput();long tt1=millis();int cc=0;
if((in[0]==1)||(in[6]==1)&&((tt1-tt)>100)&&((tt1-tt)<1900)){bbb=1;aa=1;}
if((bbb==1)&&in[7]==1||in[9]==1) bb1=1;
if(bbb!=1&&in[7]==0&&in[9]==0)b=1;
if(bb1==1&&in[7]==0&&in[9]==0)b=1;
if((b==1)&&((in[7]==1)||(in[9]==1))){speedControl(0,0);return in[8];}
if(in[0]==0&&in[6]==0) pwmm=180;
setValues();
int err = 0 - cv;
cerr+=err;
float pwm = err*kp + cerr*ki + (err-perr)*kd;
perr=err;
if(aa==1){pwm=0;aa=0;}
pwma=pwmm-pwm;pwmb=pwmm+pwm;
pwma=(pwma>pwmm)?pwmm:pwma;
pwmb=(pwmb>pwmm)?pwmm:pwmb;
pwma=(pwma<0)?0:pwma;
pwmb=(pwmb<0)?0:pwmb;
digitalWrite(md[0],HIGH);
digitalWrite(md[1],LOW);
digitalWrite(md[4],LOW);
digitalWrite(md[5],HIGH);
speedControl(pwma,pwmb);}
speedControl(0,0);
return in[8];
}
void setup(){
D=1;X=1;Y=1;im=-1;
Serial.begin(9600);
pinMode(13,OUTPUT);pinMode(10,INPUT);
pinMode(11,INPUT);pinMode(12,INPUT);
pinMode(4,OUTPUT);pinMode(5,OUTPUT);//motor B
pinMode(7,OUTPUT);pinMode(8,OUTPUT);//motor A
pinMode(9,OUTPUT);//motor A pwm
pinMode(3,OUTPUT);//motor B pwm
digitalWrite(13,HIGH);
digitalWrite(13,HIGH);
for(int i=0;i<500;i++)
{
takeInput();
digitalWrite(md[0],HIGH);
digitalWrite(md[1],LOW);
digitalWrite(md[4],HIGH);
digitalWrite(md[5],LOW);
speedControl(255,255);
}
for(int i=0;i<1000;i++)
{takeInput();
digitalWrite(md[0],LOW);
digitalWrite(md[1],HIGH);
digitalWrite(md[4],LOW);
digitalWrite(md[5],HIGH);
speedControl(255,255);
}
while(in[3]==0){
takeInput();
digitalWrite(md[0],HIGH);
digitalWrite(md[1],LOW);
digitalWrite(md[4],HIGH);
digitalWrite(md[5],LOW);
speedControl(255,255);
}
speedControl(0,0);
digitalWrite(md[0],HIGH);
digitalWrite(md[1],LOW);
digitalWrite(md[4],LOW);
digitalWrite(md[5],HIGH);
digitalWrite(13,LOW);
ix=0;iy=0;
for(int i=0;i<6;i++){printf("%d,%d\n",b[i][0]+1,b[i][1]+1);}
for(int i=0;i<11;i++){
A[i][0]=1000;A[0][i]=1000;A[10][i]=1000;A[i][10]=1000;
}
mov();
}
void calc(){
for(int i=1;i<10;i++)
for(int j=1;j<10;j++)
if(A[i][j]!=500)
A[i][j]=abs(b[ib][0]-i+1)+abs(b[ib][1]-j+1);
}
void takeInput(){
in[0]=digitalRead(12);
in[1]=analogRead(18);
in[2]=analogRead(19);
in[3]=digitalRead(11);
in[4]=analogRead(20);
in[5]=analogRead(21);
in[6]=digitalRead(10);
for(int i=1;i<6;i++){
if(i==3)continue;
if(in[i]>ma[i])
{ma[i]=in[i];}
else
if(in[i]<mi[i])
mi[i]=in[i];
in[i]=map(in[i],mi[i],ma[i],0,255);
}
in[7]=analogRead(15);in[7]=((in[7]>500)?1:0);
in[8]=analogRead(16);in[8]=((in[8]>500)?1:0);
in[9]=analogRead(17);in[9]=((in[9]>150)?1:0);
}
void setValues(){
if(in[3]==1)m=(-in[1]*3-in[2]+in[4]+in[5]*3);
if((in[1]>60||in[2]>60||in[4]>60||in[5]>60||in[3]==1))
cv =m*in[3]
+(1-in[3])*(((in[1]+in[2])>(in[4]+in[5]))?(m-(+(255-in[1])*3+(255-in[2]))):((255-in[4])+(255-in[5])*3+m));
else cv=1800*(pcv/abs(pcv));
pcv=cv;
}
void turn(int c){
int e=0;
while(e==0||(in[3]==0))
{
takeInput();
if(in[3]==0)e=1;
if(c){
digitalWrite(md[0],HIGH);
digitalWrite(md[1],LOW);
digitalWrite(md[4],HIGH);
digitalWrite(md[5],LOW);
}
else{
digitalWrite(md[0],LOW);
digitalWrite(md[1],HIGH);
digitalWrite(md[4],LOW);
digitalWrite(md[5],HIGH);
}
speedControl(200,200);
}
if(c){D--;if(D==0)D=4;}
else{D++;if(D==5)D=1;}
speedControl(0,0);
digitalWrite(md[0],HIGH);
digitalWrite(md[1],LOW);
digitalWrite(md[4],LOW);
digitalWrite(md[5],HIGH);
}
void loop(){
sp();
if(!go())
for(int i=0;i<50;i++){
digitalWrite(6,HIGH);
delay(100);
digitalWrite(6,LOW);
delay(100);
ib++;}
if(ib==nb)delay(100000);
}
int go(){
for(int i=0;i<=im;i++){if(goto_(M[i]))return -1;}
return 0;
}
void setp(int d){
switch(d){
case 1:Y++;break;
case 2:X++;break;
case 3:Y--;break;
case 4:X--;break;
}
}
int goto_(int d){
if(d==D){if(!mov()){setp(D);turn(0);turn(0);mov();ch[++nch][0]=X;ch[nch][1]=Y;A[X][Y]=500;setp(D);return -1;}else {setp(D);return 0;}}
if((abs(d-D)==2)){turn(1);turn(1);if(!mov()){setp(D);turn(0);turn(0);mov();ch[++nch][0]=X;ch[nch][1]=Y;A[X][Y]=500;setp(D);return -1;}else setp(D);return 0;}
if((d==4&&D==1)||(d<D)){turn(1);if(!mov()){setp(D);turn(0);turn(0);mov();ch[++nch][0]=X;ch[nch][1]=Y;A[X][Y]=500;setp(D);return -1;}else setp(D);return 0;}
if((d>D)||(D==4&&d==1)){turn(0);if(!mov()){setp(D);turn(0);turn(0);mov();ch[++nch][0]=X;ch[nch][1]=Y;A[X][Y]=500;setp(D);return -1;}else setp(D);return 0;}
}
void sp(){
ix=X;iy=Y;im=-1;
calc();
int i=0,j=0,k=0;
while(!((b[ib][0]==(ix-1))&&(b[ib][1]==(iy-1)))){
A[ix][iy]+=2;
nextd(min(A[ix][iy+1],min(A[ix-1][iy],min(A[ix+1][iy],A[ix][iy-1]))));
for(k=0;k<nch;k++){
if((ix==ch[k][0])&&(iy==ch[k][1])){
A[ix][iy]=500;
switch(M[im]){
case 1:A[ix][--iy]+=2;break;
case 2:A[--ix][iy]+=2;break;
case 3:A[ix][++iy]+=2;break;
case 4:A[++ix][iy]+=2;break;
}
im--;break;
}
}
}
for(i=1;i<im;i++){
if(abs(M[i]-M[i-1])==2){for(j=i-1;j<=im-2;j++)M[j]=M[j+2];im-=2;i=((i<2)?0:(i-2));}
else if(abs(M[i+1]-M[i-1])==2){M[i-1]=M[i];for(j=i;j<=im-2;j++)M[j]=M[j+2];im-=2;i=((i<2)?0:(i-2));}
}
}
void nextd(int t1){
if(im!=0){
switch(M[im]){
case 1:if(t1==A[ix][iy+1]){iy++;M[++im]=1;return;}break;
case 2:if(t1==A[ix+1][iy]){ix++;M[++im]=2;return;}break;
case 3:if(t1==A[ix][iy-1]){iy--;M[++im]=3;return;}break;
case 4:if(t1==A[ix-1][iy]){ix--;M[++im]=4;return;}break;
}
}
switch(D){
case 1:
if(t1==A[ix][iy+1]){
iy++;M[++im]=1;return;
}
if(t1==A[ix+1][iy]){
ix++;M[++im]=2;return;
}
if(t1==A[ix-1][iy]){
ix--;M[++im]=4;return;
}
if(t1==A[ix][iy-1]){
iy--;M[++im]=3;return;
}break;
case 2:
if(t1==A[ix+1][iy]){
ix++;M[++im]=2;return;
}
if(t1==A[ix][iy+1]){
iy++;M[++im]=1;return;
}
if(t1==A[ix][iy-1]){
iy--;M[++im]=3;return;
}
if(t1==A[ix-1][iy]){
ix--;M[++im]=4;return;
}
break;
case 3:
if(t1==A[ix][iy-1]){
iy--;M[++im]=3;return;
}break;
if(t1==A[ix+1][iy]){
ix++;M[++im]=2;return;
}
if(t1==A[ix-1][iy]){
ix--;M[++im]=4;return;
}
if(t1==A[ix][iy+1]){
iy++;M[++im]=1;return;
}
case 4:
if(t1==A[ix-1][iy]){
ix--;M[++im]=4;return;
}
if(t1==A[ix][iy-1]){
iy--;M[++im]=3;return;
}
if(t1==A[ix][iy+1]){
iy++;M[++im]=1;return;
}
if(t1==A[ix+1][iy]){
ix++;M[++im]=2;return;
}
break;
}
}
void speedControl(int a, int b){
analogWrite(9, map(a,0,255,0,145));
analogWrite(3, map(b,0,255,0,150));
}
Reference in New Issue View Git Blame Copy Permalink