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);
 }
}