Scribd
Upload
38 views2 pages

Sketch May30a

This document contains code for controlling a motor using PID control based on encoder feedback. It defines pin connections for the motor, encoder, and PID constants. It initializes timers and interrupts for reading the encoder and controlling the motor. The PID controller computes the motor output based on the setpoint, encoder position feedback, and PID terms. Serial communication is used to set the setpoint.

Uploaded by

mgx265pqzc
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
38 views2 pages

Sketch May30a

This document contains code for controlling a motor using PID control based on encoder feedback. It defines pin connections for the motor, encoder, and PID constants. It initializes timers and interrupts for reading the encoder and controlling the motor. The PID controller computes the motor output based on the setpoint, encoder position feedback, and PID terms. Serial communication is used to set the setpoint.

Uploaded by

mgx265pqzc
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 2

#include <MsTimer2.

h>
//motor parameters
int motor_ENA=6;
int motor_IN1=8;
int motor_IN2=7;
//encoder para
unsigned int timer_int=20;
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3
long pulse_number=0;
int rpm;
//PID constants
double kp=20;//20//6
double ki=1;
double kd=10;//0.5//20
double error;
double lastError=0;
double error_lala=0;

double input,output,setPoint;
double cumError,rateError;
int SetValue=0;
String inString="";
int computePID(double inp);

void setup() {
pinMode(motor_ENA,OUTPUT);//使能和 PWM 调速端口
pinMode(motor_IN1,OUTPUT);//方向
pinMode(motor_IN2,OUTPUT);//方向
MsTimer2::set(timer_int, control);
MsTimer2::start();
pinMode(ENCODER_A_PIN,INPUT);//方向
pinMode(ENCODER_B_PIN,INPUT);//方向
attachInterrupt(0, read_quadrature, FALLING); //EN_A 脚下降沿触发中断
Serial.begin(9600); //初始化串口
}

void loop() {
//接受串口发来的字符串，转化成对应的数值
if (Serial.available() > 0)
{
int inChar = Serial.read();
if (isDigit(inChar))
{
inString += (char)inChar;
}
if (inChar == ' '|| inChar == '\n')
{
SetValue = inString.toInt();
inString = "";
}
}

}
void read_quadrature()
{
if(digitalRead(ENCODER_A_PIN) == LOW)
{
if(digitalRead(ENCODER_B_PIN) == LOW)
{
pulse_number ++;
}
if(digitalRead(ENCODER_B_PIN) == HIGH)
{
pulse_number --;
}
}
}

void control()
{
output = computePID(pulse_number);
if(output>0)
{
if(output>255)output=255;
digitalWrite(motor_IN1, HIGH);
digitalWrite(motor_IN2, LOW);
}
else
{
if(output<-255)output=-255;
output=-output;
digitalWrite(motor_IN1, LOW);
digitalWrite(motor_IN2, HIGH);
}
analogWrite(motor_ENA, output);
Serial.println(pulse_number);
pulse_number = 0;
}

int computePID(double inp)


{
error = SetValue - inp;
cumError += error;
rateError = (error - lastError);
double out = kp*error+ki*cumError+kd*rateError;
lastError = error;
return out;
}

You might also like