PD制御 uekun
const int a = A4;
const int b = A3;
const int c = A2;
const int d = A1;
const int e = A0;
const int L_Forward = 9;
const int L_Back = 10;
const int R_Forward = 6;
const int R_Back = 5;
const int kp = 70; //need tochange!
const int kd = 110; //need tochange!
int LMP;
int RMP;
int P = 0;
int D = 0;
int PD = 0;
int motor1 = 250;
int back ;
int i;
int j = 2;
float LP;
float diff[2] = {};
void setup(){
pinMode(L_Forward,OUTPUT);//motorPin
pinMode(L_Back,OUTPUT);//motorPin
pinMode(R_Forward,OUTPUT);//motorPin
pinMode(R_Back,OUTPUT);//motorPin
pinMode(a,INPUT);//sensorPin
pinMode(b,INPUT);//sensorPin
pinMode(c,INPUT);//sensorPin
pinMode(d,INPUT);//sensorPin
pinMode(e,INPUT);//sensorPin
delay(2000);
}
void loop(){
int n = 0;
double ls = 0;
if(analogRead(a) < 100){
ls -= 3;
n += 1;
}
if(analogRead(b) < 100){
ls -= 1;
n += 1;
}
if(analogRead(c) < 100){
ls += 0;
n += 1;
}
if(analogRead(d) < 100){
ls += 1;
n += 1;
}
if(analogRead(e) < 100){
ls += 3;
n += 1;
}
LP = (ls/n);
//D
diff[0] = diff[1];
diff[1] = LP;
P = kp * (LP);
D = kd * (diff[1] - diff[0]);
PD = P + D;
back = LP * LP * 5;
if(n==0){
driveMotor(LMP,RMP);
}else{
LMP = motor1 + PD - back; //need to change!
RMP = motor1 - PD - back; //need to change!
driveMotor(LMP,RMP);
}
}
//don't touch! this area.
void driveMotor(int left,int right){
if(left>255){
left = 255;
}else if(left<-255){
left = -255;
}
if(right>255){
right = 255;
}else if(right < -255){
right = -255;
}
if(left>0){
analogWrite(L_Forward,left);
analogWrite(L_Back,0);
}else if(left<0){
left = abs(left);
analogWrite(L_Forward,0);
analogWrite(L_Back,left);
}else{
digitalWrite(L_Forward,HIGH);
digitalWrite(L_Back,HIGH);
}
if(right>0){
analogWrite(R_Forward,right);
analogWrite(R_Back,0);
}else if(right<0){
right = abs(right);
analogWrite(R_Forward,0);
analogWrite(R_Back,right);
}else{
digitalWrite(R_Forward,HIGH);
digitalWrite(R_Back,HIGH);
}
}