Hello everyone, I'm trying to make a 2 axis balancing stick with the Arduino uno,adafruit motor shield , mpu6050 gyroscope and 2x 12v
300 rpm motors. The project works on the principle of angular momentum to prevent the stick from falling. I will paste a link at the end. The problem I'm having is controlling the motor speed so that they get enough power and can reverse the direction when the speed goes into the negetive.
#include <SoftwareSerial.h>
#include <Wire.h>
#include <AFMotor.h>
#include <MPU6050_light.h>
SoftwareSerial BTSerial(A0, A1); // RX, TX pins for SoftwareSerial
AF_DCMotor xMotor(1);
AF_DCMotor yMotor(4);
const float Kp = 1.0; // Proportional gain
const float Ki = 0.1; // Integral gain
const float Kd = 0.01; // Derivative gain
const float dt = 0.02; // Time interval (adjust as needed)
float setpointX = 0.0; // Initial vertical position along X-axis
float setpointY = 0.0; // Initial vertical position along Y-axis
float currentAngleX = 0.0;
float currentAngleY = 0.0;
float prevErrorX = 0.0;
float prevErrorY = 0.0;
float integralX = 0.0;
float integralY = 0.0;
MPU6050 mpu(Wire);
void setup() {
Serial.begin(9600);
Wire.begin();
BTSerial.begin(9600);
// Initialize MPU6050
byte status = mpu.begin(3, 0);
Serial.print(F("MPU6050 status: "));
Serial.println(status);
while (status != 0) {} // stop everything if could not connect to MPU6050
/*while (!mpu.begin(3, 0)) {
BTSerial.println("Failed to initialize MPU6050!");
Serial.println("Failed to initialize MPU6050!");
delay(1000);*/
Serial.println(F("Calculating offsets, do not move MPU6050"));
mpu.calcAccOffsets();
mpu.calcGyroOffsets();
delay(1000);
}
void loop() {
mpu.update();
// Measure current angles (replace this with your actual sensor reading)
float gyroReadingX = mpu.getGyroX(); // Assuming X-axis for angular velocity
float gyroReadingY = mpu.getGyroY(); // Assuming Y-axis for angular velocity
currentAngleX = 0.98 * (currentAngleX + gyroReadingX * dt) + 0.02 * setpointX; // Update current angle along X-axis
currentAngleY = 0.98 * (currentAngleY + gyroReadingY * dt) + 0.02 * setpointY; // Update current angle along Y-axis
// PID control for X-axis
float errorX = setpointX - currentAngleX;
integralX += errorX * dt;
float derivativeX = (errorX - prevErrorX) / dt;
float outputX = Kp * errorX + Ki * integralX + Kd * derivativeX * 100;
// Motor control for X-axis
int xMotorSpeed = 100 - outputX; // Adjust direction based on your motor setup
xMotor.setSpeed(xMotorSpeed);
// PID control for Y-axis
float errorY = setpointY - currentAngleY;
integralY += errorY * dt;
float derivativeY = (errorY - prevErrorY) / dt;
float outputY = Kp * errorY + Ki * integralY + Kd * derivativeY * 100;
// Motor control for Y-axis
int yMotorSpeed = 200 + outputY; // Adjust direction based on your motor setup
yMotor.setSpeed(yMotorSpeed);
// Run motors
xMotor.run(FORWARD);
yMotor.run(FORWARD);
// Update variables for the next iteration
prevErrorX = errorX;
prevErrorY = errorY;
// Print data to BT
BTSerial.print("Setpoint X: ");
BTSerial.print(setpointX);
BTSerial.print("\tCurrent Angle X: ");
BTSerial.print(currentAngleX);
BTSerial.print("\tOutput X: ");
BTSerial.print(outputX);
BTSerial.print("\tX Motor Speed: ");
BTSerial.print(xMotorSpeed);
BTSerial.print("\tSetpoint Y: ");
BTSerial.print(setpointY);
BTSerial.print("\tCurrent Angle Y: ");
BTSerial.print(currentAngleY);
BTSerial.print("\tOutput Y: ");
BTSerial.print(outputY);
BTSerial.print("\tY Motor Speed: ");
BTSerial.println(yMotorSpeed);
// send data to serial monitor
Serial.print("Setpoint X: ");
Serial.print(setpointX);
Serial.print("\tCurrent Angle X: ");
Serial.print(currentAngleX);
Serial.print("\tOutput X: ");
Serial.print(outputX);
Serial.print("\tX Motor Speed: ");
Serial.print(xMotorSpeed);
Serial.print("\tSetpoint Y: ");
Serial.print(setpointY);
Serial.print("\tCurrent Angle Y: ");
Serial.print(currentAngleY);
Serial.print("\tOutput Y: ");
Serial.print(outputY);
Serial.print("\tY Motor Speed: ");
Serial.println(yMotorSpeed);
delay(20);
}
ignore the bluetooth module, its just for sending sensor data
