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