92 lines
2.1 KiB
C++
92 lines
2.1 KiB
C++
int in[7];
|
|
int ma[7]={0,0,0,0,0,0,0};
|
|
int mi[7]={500,500,500,500,500,500,500};
|
|
int md[]={10,11,9,6,8,7};
|
|
int cv;int k,pcv;
|
|
int perr;
|
|
long cerr;int pwma,pwmb,pwmm=255;
|
|
//float kp=0.28,ki=0.000008,kd=0.01;
|
|
float kp=0.19,ki=0.00008,kd=0.05;
|
|
void setup(){
|
|
Serial.begin(9600);
|
|
pinMode(13,INPUT);
|
|
pinMode(12,INPUT);
|
|
for(int i=0;i<5;i++)pinMode(md[i],OUTPUT);
|
|
digitalWrite(md[0],HIGH);
|
|
digitalWrite(md[1],LOW);
|
|
digitalWrite(md[4],HIGH);
|
|
digitalWrite(md[5],LOW);
|
|
speedControl(255,255);
|
|
for(int i=0;i<500;i++)
|
|
takeInput();
|
|
digitalWrite(md[0],LOW);
|
|
digitalWrite(md[1],HIGH);
|
|
digitalWrite(md[4],LOW);
|
|
digitalWrite(md[5],HIGH);
|
|
for(int i=0;i<1000;i++)
|
|
takeInput();
|
|
digitalWrite(md[0],HIGH);
|
|
digitalWrite(md[1],LOW);
|
|
digitalWrite(md[4],HIGH);
|
|
digitalWrite(md[5],LOW);
|
|
while(in[3]==0)
|
|
takeInput();
|
|
digitalWrite(md[0],HIGH);
|
|
digitalWrite(md[1],LOW);
|
|
digitalWrite(md[4],LOW);
|
|
digitalWrite(md[5],HIGH);
|
|
}
|
|
|
|
void takeInput(){
|
|
//in[0]=digitalRead(12);
|
|
//in[7]=digitalRead(13);
|
|
in[1]=analogRead(A1);
|
|
in[2]=analogRead(A2);
|
|
in[3]=analogRead(A3);
|
|
in[4]=analogRead(A4);
|
|
in[5]=analogRead(A5);
|
|
for(int i=1;i<=5;i++){
|
|
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[3]=(in[3]>150)?1:0;
|
|
}
|
|
void setValues(){
|
|
if(in[3]==1)k=(-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 =k*in[3]
|
|
+(1-in[3])*(((in[1]+in[2])>(in[4]+in[5]))?(k-(+(255-in[1])*3+(255-in[2]))):((255-in[4])+(255-in[5])*3+k));
|
|
else cv=1800*(pcv/abs(pcv));
|
|
pcv=cv;
|
|
Serial.println(cv);
|
|
}
|
|
void loop (){
|
|
/*for(int i=0;i<7;i++)
|
|
{
|
|
Serial.print(in[i]);Serial.print(" ");
|
|
}*/
|
|
|
|
takeInput();
|
|
setValues();
|
|
int err = 0 - cv;
|
|
cerr+=err;
|
|
float pwm = err*kp + cerr*ki + (err-perr)*kd;
|
|
perr=err;
|
|
//Serial.println(pwm);
|
|
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;
|
|
speedControl(pwma,pwmb);
|
|
}
|
|
void speedControl(int a, int b){
|
|
|
|
analogWrite(md[2], map(a,0,255,0,255));//246
|
|
analogWrite(md[3], map(b,0,255,0,250));}
|