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

機体作った後、上手く動かなくて、修正後に上田リーダからもらったプログラム

上田君


const int a = A5;

const int b = A4;

const int c = A3;

const int d = A2;

const int e = A1;


const int L_Forward = 9;

const int L_Back = 10;

const int R_Forward = 6;

const int R_Back = 5;

const int kp = 20; //need tochange!

const int kd = 0; //need tochange!


int LMP;

int RMP;

int P = 0;

int D = 0;

int PD = 0;

int motor1 = 40;

int back ;

int       i;

int       j = 10;

float LP;


float diff[10]     = {};





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

 Serial.begin(9600);

}




void loop(){


 int n = 0;

 double ls = 0;

 if(analogRead(a) < 400){  

   ls -= 2;

   n += 1;

 }

 if(analogRead(b) < 400){

   ls -= 1;

   n += 1;

 }

 if(analogRead(c) < 400){

   ls += 0;

   n += 1;

 }

 if(analogRead(d) < 400){

   ls += 1;

   n += 1;

 }

 if(analogRead(e) < 400){

   ls += 2;

   n += 1;

 }


 LP = (ls/n);

 //D

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

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

   }//⭐️


   diff[j] = LP;

   P = kp * (LP);

   D = kd * (diff[i] - diff[0]);//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);


 }

 Serial.println(LP);

}





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

 }

}