Scribd
Upload
26 views3 pages

PID

This document contains code for controlling the position of a motor using PID control. It defines pins and variables for an encoder, motor driver, and PID parameters. Functions are used to calculate the PID output, drive the motor, and check the encoder position.

Uploaded by

chanh.lechanh
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
26 views3 pages

PID

This document contains code for controlling the position of a motor using PID control. It defines pins and variables for an encoder, motor driver, and PID parameters. Functions are used to calculate the PID output, drive the motor, and check the encoder position.

Uploaded by

chanh.lechanh
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/ 3

//CONTROL MOTOR POSITION

//PINS
const int channelA = 2;
const int channelB = 4;
int pul = 0;
const int enA = 10;
int enA_value = 0; //(0-255)
const int in1 = 8;
const int in2 = 9;
int direct = 0; // direct = 0: CCW, = 1: CW
float targetP = 20;
int i =0;
volatile float motorPosition = 0;
float previousPosition = -1;
//PID parameters
float kp = 0.5;
float ki = 3;
float kd = 1;
float controlsignal = 0; //u
float previousTime = 0;
float previousError = 0;
float errorIntegral = 0;
float currentTime = 0;
float deltaTime = 0;
float errorValue = 0;
float edot = 0;
void setup()
{
Serial.begin(9600);
pinMode(channelA,INPUT);
pinMode(channelB,INPUT);
attachInterrupt(digitalPinToInterrupt(channelA), checkEncoder, RISING);
}
void loop()
{
while (i < 6)
{
//forward
calculatePID();
driveMotor();
printValues();
//return
calculatePID ();
back ();
}
}
void calculatePID()
{
currentTime = micros();
deltaTime = (currentTime - previousTime)/1000000.0
previousTime = currentTime;
errorValue = motorPosition - targetP;
edot = (errorValue - previousError) / deltaTime; //Khau D
errorIntegral = errorIntegral + (errorValue * deltaTime); //Khau I
controlSignal = (kp * errorValue) + (kd * edot) + (ki * errorIntegral);
previousError = errorValue;
if (motorPosition == 30) {
i++;
delay (1000);
motorPosition = 0;
}
}
void printValues()
{
Serial.println(i);
}
void back()
{
if (controlSignal < 0) //negative value: CCW
{
direct = 1;
}
else if (controlSignal > 0) //positive: CW
{
direct = -1;
}
else //0: STOP
{
direct = 0;
}
enA_value = (int)fabs(controlSignal);
if (enA_value > 255) //fabs() = floating point absolute value
{
enA_value = 255;
}

if (enA_value < 30 && errorValue != 0)


{
enA_value = 30;
}

if (direct == -1) //-1 == CCW


{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
else if (direct == 1) // == 1, CW
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
else // == 0, stop/break
{
digitalWrite(in1, LOW);
digitalWrite(in1, LOW);
enA_value = 0;
}
analogWrite(enA, enA_value);
}
void driveMotor()
{
{
if (controlSignal < 0) //negative value: CCW
{
direct = -1;
}
else if (controlSignal > 0) //positive: CW
{
direct = 1;
}
else //0: STOP
{
direct = 0;
}
enA_value = (int)fabs(controlSignal);
if (enA_value > 255) //fabs() = floating point absolute value
{
enA_value = 255;
}

if (enA_value < 30 && errorValue != 0)


{
enA_value = 30;
}

if (direct == -1) //-1 == CCW


{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
else if (direct == 1) // == 1, CW
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
else // == 0, stop/break
{
digitalWrite(in1, LOW);
digitalWrite(in1, LOW);
enA_value = 0;
}
analogWrite(enA, enA_value);
Serial.print(errorValue);
Serial.print(" ");
}
void checkEncoder()
{
pul = digitalRead(channelB);

if (pul == 1) //CW direction


{
motorPosition++;
}
else //else, it is zero... -> CCW direction
{
motorPosition--;
}
}

You might also like