Hello all,
I have been struggling with part of the PID code below. I am having a hard time understandung how the "out" part of this PID drives the motors to the setpoint. In the code below the" out " is mentioned once in the code. I (being new to Arduino) would have expected to see "out = drive(0,0);" to drive the motors to the setpoint. But as I said "out" is mentioned only once(except for the return). Maybe I am missing something that has been starring me in the face the whole time.
My goal is to use THIS PID to do other fun things and learn. I understand that there are other PID examples, and tutorials out there to learn from, but I am dead set to understand this one
So if I wanted to make a code that measured how hot one of my sensers(reading) is, and cool it down. And the temp I wanted it to be (target).
And if I used a fan (out) to cool it down. What part of the PID below would be used to drive the motor of the fan to the target temp?
For most of you this is an easy explaination, and I appreciate all help so I can move forword.
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
/* Variables for the PID controller */
float Kp = 58.30f;
float Kd = 0.00039f;
float Ki = 0.0024f; //0.0020 seems to work good
float reading = 0.0f;
float target = -1.79f;// Add negative to give more PWM to rear // rear is the uno side
float errorCurrent = 0.0f;
float errorLast = 0.0f;
float errorSum = 0.0f;
float errorDelta = 0.0f;
unsigned long timeLast = 0;
int correction = 0;
/* Variables for Motor control*/
const int MotorLeftPin1 = 8;
const int MotorLeftPin2 = 9;
const int MotorLeftPinEn = 10; //PWM enabled side the Mega is on
const int MotorRightPin1 = 12;
const int MotorRightPin2 = 13;
const int MotorRightPinEn = 11; //PWM enabled oposit side of Mega
const int minSpeed = 90; // defines the useful PWM range of the motors [minSpeed, 255]
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
mpu.setDMPEnabled(true);
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
}
pinMode(MotorLeftPin1, OUTPUT);
pinMode(MotorLeftPin2, OUTPUT);
pinMode(MotorLeftPinEn, OUTPUT);
pinMode(MotorRightPin1, OUTPUT);
pinMode(MotorRightPin2, OUTPUT);
pinMode(MotorRightPinEn, OUTPUT);
#define MICROSECONDS_PER_TIMER0_OVERFLOW (clockCyclesToMicroseconds(64 * 256))
to:
#define MICROSECONDS_PER_TIMER0_OVERFLOW (clockCyclesToMicroseconds(1 * 510))
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// drive(255, 255);
// delay(5000);
// drive(0,0);
// delay(5000);
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
//Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
//
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
if (millis()>=15000) {
reading = ypr[2] * 180/M_PI;
// stop motors and the PID controller if the bot is tilted more than 30 degrees
if ((int)reading >=-30 && reading <= 30) {
correction = pid();
drive(correction, correction);
}
else {
drive(0,0);
}
}
#endif
}
void drive(int leftMotorSpeed, int rightMotorSpeed) {
int left = constrain(leftMotorSpeed, -255, 255);
int right = constrain(rightMotorSpeed, -255, 255);
if (left < 0) {
if (left >= -40) {left = 0;}
else if (left >= -100) {left = -100;}
digitalWrite(MotorLeftPin1, LOW);
digitalWrite(MotorLeftPin2, HIGH);
analogWrite(MotorLeftPinEn, -left);
}
else if (left == 0) {
digitalWrite(MotorLeftPin1, LOW);
digitalWrite(MotorLeftPin2, LOW);
digitalWrite(MotorLeftPinEn, LOW);
}
else {
if (left <= 40) {left = 0;}
else if (left <= 100) {left = 100;}
digitalWrite(MotorLeftPin1, HIGH);
digitalWrite(MotorLeftPin2, LOW);
analogWrite(MotorLeftPinEn, left);
}
if (right < 0) {
if (right >= -40) {right = 0;}
else if (right >=-100) {right = -100;}
digitalWrite(MotorRightPin1, LOW);
digitalWrite(MotorRightPin2, HIGH);
analogWrite(MotorRightPinEn, -right);
}
else if (right == 0) {
digitalWrite(MotorRightPin1, LOW);
digitalWrite(MotorRightPin2, LOW);
digitalWrite(MotorRightPinEn, LOW);
}
else {
if (right <= 45) {right = 0;}
else if (right <= 90) {right = 90;};
digitalWrite(MotorRightPin1, HIGH);
digitalWrite(MotorRightPin2, LOW);
analogWrite(MotorRightPinEn, right);
}
}
int pid() {
// Calculate elapsed time since last run
unsigned long now = millis();
float timeDelta = (float)(now - timeLast);
// initialize timeDelta as float to help with the next calculations
// Compute error data
errorCurrent = target - reading;
errorSum = errorSum + (errorCurrent * timeDelta);
errorDelta = (errorCurrent - errorLast) / timeDelta;
// constrain the output to the acceptable range for motor PWM signal
float pTerm = Kp * errorCurrent;
float iTerm = Ki * errorSum;
float dTerm = Kd * errorSum;
// constrain the integral term to [-255; 255]
float limit = 255.0f;
if (iTerm > limit) {iTerm = limit;}
else if (iTerm < -limit) {iTerm = -limit;}
// Calculate PID output
float pid = pTerm + iTerm + dTerm;
// constrain the PID output to [-255; 255]
if (pid > limit) {pid = limit;}
else if (pid < -limit) {pid = -limit;}
// Store some data for the next run
errorLast = errorCurrent;
timeLast = now;
int out = (int)pid * (-1); // not worried about correct rounding here
return out;
}
