読者です 読者をやめる 読者になる 読者になる

P制御 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 = 10;

const int L_Back = 9;

const int R_Forward = 6;

const int R_Back = 5;

const int kp = 20; //need tochange!

const int kd = 110; //need tochange!


int LMP;

int RMP;

int P = 0;

int D = 0;

int PD = 0;

int motor1 = 27;//need tochange!

int back ;//= 150;


int      i      =0;

int      j      =30;

float LP;


float diff[30]    = {};





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) < 500){

    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

    for (i=0; i<j; i++){

      diff[i] = diff[i+1];

  }

    diff[j] = LP;

    P = kp * (LP);

    D = kd * (diff[j] - diff[0]);

    PD = P + D;

    

    back = LP * LP * 5;*/

        P = kp * (LP);


  if(n==0){

  

  driveMotor(LMP,RMP);

  }else{

  LMP = motor1 + P - back;  //need to change!

  RMP = motor1 - P - 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);

  }

}