用PID控制小电机
作者:互联网
#include<TimerOne.h>
int NC = 4;
int ENA = 6;
int ENC_A = 2;
int ENC_B = 3;
int IN1 = 10;
int IN2 = 11;
int output_pwm;
int count = 0;
double rpm = 0,input_rspeed,output_rspeed=0;
int reduction_ratio = 20;//减速比
double err,preerr,prepreerr;
double goal_rspeed = 200;
double pid,Kp=0.6,Ki=0.1,Kd=2;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(NC,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(ENA,OUTPUT);
digitalWrite(NC,HIGH);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
Timer1.initialize(50000);
Timer1.attachInterrupt(timerIsr);
pinMode(ENC_A,INPUT);
pinMode(ENC_B,INPUT);
attachInterrupt(0,Code1,CHANGE);
attachInterrupt(1,Code2,CHANGE);
}
void timerIsr()
{
rpm = (double)count*20*60/260/4;
count = 0;
input_rspeed = rpm;
output_rspeed += PID(input_rspeed,goal_rspeed);
output_pwm = 0.3 * output_rspeed;
}
double PID(double in, double goal)
{
prepreerr = preerr;
preerr = err;
err = goal - in;
pid += Kp*(err-preerr)+Ki*err+Kd*((err-preerr)-
(preerr-prepreerr));
return pid;
}
void Code1()
{
if(digitalRead(ENC_A) == LOW)
{
if(digitalRead(ENC_B) == LOW)
{
count -= 1;
}
if(digitalRead(ENC_B) == HIGH)
{
count += 1;
}
}
if(digitalRead(ENC_A) == HIGH)
{
if(digitalRead(ENC_B) == HIGH)
{
count -= 1;
}
if(digitalRead(ENC_B) == LOW)
{
count += 1;
}
}
}
void Code2()
{
if(digitalRead(ENC_B) == LOW)
{
if(digitalRead(ENC_A) == HIGH)
{
count -= 1;
}
if(digitalRead(ENC_A) == LOW)
{
count += 1;
}
}
if(digitalRead(ENC_B) == HIGH)
{
if(digitalRead(ENC_A) == LOW)
{
count -= 1;
}
if(digitalRead(ENC_A) == HIGH)
{
count += 1;
}
}
}
void loop() {
// put your main code here, to run repeatedly:
Serial.print("goal:");
Serial.print(goal_rspeed);
Serial.print(" rpm:");
Serial.println(rpm);
analogWrite(ENA,output_pwm);
}
标签:count,控制,ENC,rspeed,电机,int,PID,HIGH,digitalRead 来源: https://blog.csdn.net/plzcallmefoolish/article/details/120108638