Hi. I am building a PID function to keep my vehicle in a proper angle position using the GY-91 and DC motors with encoders. I stumbled across an issue with the code where it wouldn't pick up the sensory inputs from my Gyro that I needed to perform proper correction. I can say this because when I try displaying the correction values for the gyro and then moving my robot in different directions, my motors don't change in speed. May I get assistance, please?
#include <Wire.h> //Include wire library
#include <MPU6050_light.h> //Include library for MPU communication
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
const byte MOTOR_A = 2; // Motor 2 Interrupt Pin - INT 1 - Right Motor
const byte MOTOR_B = 3; // Motor 1 Interrupt Pin - INT 0 - Left Motor
float stepcount = 480.00;
float wheeldiameter = 63.67;
volatile int counter_A = 0;
volatile int counter_B = 0;
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
// Motor A connections
#define enA 13
#define in1 12
#define in2 11
// Motor B connections
#define enB 8
#define in3 10
#define in4 9
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
MPU6050 mpu(Wire1);
void setup() {
Serial.begin(9600);
Wire.begin();
Wire1.begin();
mpu.begin();
Serial.print(F("MPU6050 status: "));
Serial.println(F("Calculating offsets, do not move MPU6050"));
delay(1000);
mpu.calcGyroOffsets();
attachInterrupt(digitalPinToInterrupt (MOTOR_A), ISR_countA, RISING);
attachInterrupt(digitalPinToInterrupt (MOTOR_B), ISR_countB, RISING);
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
Serial.println(F("SSD1306 allocation failed"));
for (;;);
}
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
// Turn off motors - Initial state
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
delay(2000);
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println(F("Hello, OLED!"));
display.display();
delay(2000); // Pause for 2 seconds
PID_revised(CMtoSteps(100), 50);
}
void loop() {
}
/*
void PID_controller(Angle, KProportional, KIntegration, KDerivative) {
// Compute Error
int Target = ; // Change this to Mod Angle when adding Mod Angle Algorithm. Add a Gyro Reset Accerleration Algorithm in order to apply accurate Dead Reckoning
// Time Computation
long currentTime = micros();
float deltaTime = (currentTime - previousTime) / 1.0e6;
// Compute Proportional, Integral, and Derivative
int error = Angle - Target;
float errorDerivative = error - errorPrevious / deltaTime;
errorIntegral = errorIntegral + error * deltaTime;
float PID = (KProportional * error) + (KIntegration * errorIntegral) * (KDerivative * errorDerivative);
// Cycle the Values
previousTime = currentTime;
errorPrevious = error;
return PID;
}
*/
void PID_revised(int steps, int power) {
mpu.update();
counter_A = 0;
counter_B = 0;
float Kp = 3.00;
float Ki = 3.00;
float Kd = 3.00;
float lastError = 0.0;
float integral = 0.0;
while (steps > counter_A && steps > counter_B) {
if (steps > counter_A && steps > counter_B) {
mpu.update();
int error = mpu.getAngleZ();
if (error = 0) {
float integral = 0;
} else {
float integral = integral + error;
}
float derivative = error - lastError;
float correction = ((Kp * error) + (Ki * integral) + (Kd * derivative)) * -1;
float powerLeft = correction + power;
float powerRight = power - correction;
analogWrite(enA, powerLeft);
analogWrite(enB, powerRight);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
lastError = error;
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print(powerRight);
display.print(", ");
display.println(powerLeft);
display.display();
delay(10);
} else {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
}
}
int CMtoSteps(float cm) {
int result; // Final calculation result
float circumference = (wheeldiameter * 3.14) / 10; // Calculate wheel circumference in cm
float cm_step = circumference / stepcount; // CM per Step
float f_result = cm / cm_step; // Calculate result as a float
result = (int) f_result; // Convert to an integer (note this is NOT rounded)
return result; // End and return result
}
void ISR_countA()
{
counter_A++; // increment Motor A counter value
}
// Motor B pulse count ISR
void ISR_countB()
{
counter_B++; // increment Motor B counter value
}