機体作った後、上手く動かなくて、修正後に上田リーダからもらったプログラム
上田君
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);
}
}
