312 lines
7.8 KiB
C++
312 lines
7.8 KiB
C++
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(int aaaa){
|
|
long tt=millis();
|
|
int b=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();
|
|
if(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;
|
|
if(aaaa==0&&abs(pwm)>100&&((tt1-tt)>80)&&((tt1-tt)<1900)){turn(0);D++;if(D==5)D=1;return mov(1);}
|
|
perr=err;
|
|
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(){
|
|
/*A[1][3]=500;
|
|
A[6][3]=500;
|
|
A[4][2]=500;
|
|
A[3][1]=500;
|
|
A[3][6]=500;
|
|
A[7][7]=500;
|
|
A[2][4]=500;
|
|
*/
|
|
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(0);
|
|
}
|
|
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){
|
|
int t=0;
|
|
if(d==D){if(!(t=mov(0))){setp(D);turn(0);turn(0);mov(0);ch[++nch][0]=X;ch[nch][1]=Y;A[X][Y]=500;setp(D);return -1;}else {if(t==-1)setp(D);return 0;}}
|
|
if((abs(d-D)==2)){turn(1);turn(1);if(!(t=mov(0))){setp(D);turn(0);turn(0);mov(0);ch[++nch][0]=X;A[X][Y]=500;ch[nch][1]=Y;setp(D);return -1;}else setp(D);return 0;}
|
|
if((d==4&&D==1)||(d<D)){turn(1);if(!(t=mov(0))){setp(D);turn(0);turn(0);mov(0);ch[++nch][0]=X;A[X][Y]=500;ch[nch][1]=Y;setp(D);return -1;}else setp(D);return 0;}
|
|
if((d>D)||(D==4&&d==1)){turn(0);if(!(t=mov(0))){setp(D);turn(0);turn(0);mov(0);ch[++nch][0]=X;A[X][Y]=500;ch[nch][1]=Y;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));
|
|
}
|