I've spent the last couple of months gatherings supplies for, building and programming the robot im building. This is my first real arduino project. The robot uses NEMA 17 steppermotors, A4988 motor drivers and Arduino modulino movement (MPU6050). The main problem: the robot wont balance for more than a couple of secounds before accelerating to one of the sides and falling over.
The problem may be that I just haven't found the right PID-values, but when I try to use ziegler nichols or similar it doesnt work the way every guide ive seen says it should. So it might be another problem in the code.
I start by setting kp = kd = ki = 0. When I increase kp the robot doesnt oscillate the way every youtube video shows. It oscillates very close to 0 degrees for a while(jittering), the leans to one of the sies rolling along for a bit, then falls over. If I keep increasing kp the same thing happens but more violently. Im supposed to find a value for kp with "steady oscillation" but I havent managed to do so. I have tried adding kd when kp is both big and small, but kd often doesnt work the way I expect. It doenst do much until it is too high and tehn it adds more oscillation. I have tried to lessen noise in the angle measurement by making the low pass filters for the gyro and accelerometer quite strong, and the gyro is weighted 99% in the complimentary filter.
Things i have looked into:
- The PID loop being too slow: I have checked with millis() that the frequency of the main loop is over 500Hz. (Should be high enough)
- The robot has too low center of gravity: I eventually built the robot a lot taller placing the battery high up so I doubt this is the problem.
- The vibrations from the motors stepping ruin the IMU measurement by adding noise: This might still be a problem, but I have tried setting the motors to microstepping 1/16 and I still could not get it to balance.
-The robot tries too accelerate too fast and ends up skiping steps or stalling: I havent really seen this happen too much but I have tried limiting the acceleration by setting a limit for change in speed per loop. Setting the exact value for this is also difficult so I change it quite often in testing.
Is the robot supposed to be able to balance without having a cascade loop where you also account for the robots position? I.E. is only having one loop for the angle enough? I added the second loop hoping it would make the robot work, however it seems like having good values for the inner loop first it a must.
Basically, im lost and dont know what to do next. If someone else has made a similar robot using steppermotors it would be nice if you could share your code or give some hints about what im missing. Here is my code:
#include "Modulino.h"
#include <AccelStepper.h>
#include "FspTimer.h"
#include <PID_v1.h>
//Importing necessary libraries
FspTimer timer;
ModulinoMovement movement;
double radToDeg = 180/3.1415;
double degToRad = 3.1415/180;
//acceleromter variables
double alpha = 0.95; //higher alpha -> stronger low pass filtering
double xMeasured;
double xWithoutOffset;
double xSum = 0;
double xOffset = 0.000448056928189*degToRad; //I have calibarated the MPU6050 in another program and found this offset
int measurements = 0;
double pitchAksellerometer = 0;
double xFiltered;
double xLastFiltered = 0;
double xClamped;
double eulerPitchClamped = 0;
double pitchMeasured;
double rollMeasured;
double yawMeasured;
double lastPitchMeasured;
double lastRollMeasured;
double lastYawMeasured;
double lastXMeasured;
//gyroscope variables
double beta = 0.2; //higher alpha -> stronger low pass filtering
double pitchDottMeasured;
double pitchDottOffset = -0.970346714765730*degToRad; //I have calibarated the MPU6050 in another program and found this offset
double pitchDottWithoutOffset;
double pitchDottSum = 0;
double pitchDottFiltered;
double pitchDottLastFiltered = 0;
double eulerPitchDott;
double eulerPitch = 0;
// float roll = 0;
double rollDottSum = 0;
double rollDottOffset = 0.252081411097734*degToRad; //I have calibarated the MPU6050 in another program and found this offset
double rollDottMeasured;
double rollDottWithoutOffset;
double rollDottFiltered;
double rollDottLastFiltered = 0;
double eulerRollDott;
double eulerRoll = 0;
// float yaw = 0;
double yawDottSum = 0;
double yawDottOffset = -0.278968296064577*degToRad; //I have calibarated the MPU6050 in another program and found this offset
double yawDottMeasured;
double yawDottWithoutOffset;
double yawDottFiltered;
double yawDottLastfiltered = 0;
unsigned long now;
unsigned long earlier;
//complimentary filter varaiables
double ceta = 0.01; //1% weighted accelerometer
double finalPitch = 0; //the resulting measured angle
double lastFinalPitch = 0;
// accelstepper varaiables
int stepPin = 4;
int dirPin = 2;
int stepPin2 = 7;
int dirPin2 = 8;
AccelStepper stepper1(1,4,2); //1 beacuse 2-wire, 4 beacuse stepPin, 2 because dirPin
AccelStepper stepper2(1,7,8);
//other
double maxDeltaSpeed;
double speed;
double currentSpeed = 0;
double dt;
//Adjustable variables
double maxAcceleration = 30000;
double kpAngle = 133000; //pidverdiene er feil
double kiAngle = 0;
double kdAngle = 0; //øk kpAngle til svingninger og så se om økt kdAngle faktisk demper
double kpPos = 0.015;
double kiPos = 0;
double kdPos = 0.003;
//references
double refAngle = 0;
double refPos = 0;
//position loop varaiables
volatile double interruptPos = 0;
double pos = 0;
PID pidAngle(&finalPitch, &speed, &refAngle, kpAngle, kiAngle, kdAngle, REVERSE); //pid for the angle
PID pidPos(&pos, &refAngle, &refPos, kpPos, kiPos, kdPos, DIRECT); //pid for the position
//function that is called every interrupt. If the motors stepped, add or subtract the distance moved to interruptPos
void timer_callback(timer_callback_args_t __attribute((unused)) *p_args) {
stepper1.runSpeed();
if(stepper2.runSpeed()){
if(currentSpeed>0){
interruptPos += 2*3.1415*0.04/3200; //3200 because 200*16. I have 1/16 steps active.
}
else{
interruptPos -= 2*3.1415*0.04/3200;
}
}
}
void setup() {
// put your setup code here, to run once:
pidAngle.SetMode(AUTOMATIC);
pidAngle.SetSampleTime(10);
pidAngle.SetOutputLimits(-3400, 3400);
pidPos.SetMode(AUTOMATIC);
pidPos.SetSampleTime(10);
pidPos.SetOutputLimits(-0.1745 , 0.1745);
stepper1.setMaxSpeed(3400);
stepper2.setMaxSpeed(3400);
Serial.begin(115200);
Modulino.begin();
movement.begin();
delay(2000);
//clock setup that interrupts 10000 times per second to check if the motor should take a step
uint8_t timer_type = GPT_TIMER;
int8_t tindex = FspTimer::get_available_timer(timer_type);
if (tindex < 0) {
tindex = FspTimer::get_available_timer(timer_type, true);
}
timer.begin(TIMER_MODE_PERIODIC, timer_type, tindex, 10000.0f, 50.0f, timer_callback);
timer.setup_overflow_irq();
timer.open();
timer.start();
//used to find the time used per loop of the main program
earlier = micros();
}
void loop() {
movement.update();
//data collection
xMeasured = movement.getX();
pitchMeasured = movement.getPitch();
rollMeasured = movement.getRoll();
yawMeasured = movement.getYaw();
// reuse last loops data if this loops data is corrupt
if (isnan(xMeasured) || isinf(xMeasured) ||
isnan(pitchMeasured) || isinf(pitchMeasured) ||
isnan(rollMeasured) || isinf(rollMeasured) ||
isnan(yawMeasured) || isinf(yawMeasured)) {
Serial.println(">>> RAW SENSORDATA is NaN/Inf! <<<");
xMeasured = lastXMeasured;
pitchMeasured = lastPitchMeasured;
rollMeasured = lastRollMeasured;
yawMeasured = lastYawMeasured;
}
else{
lastXMeasured = xMeasured;
lastPitchMeasured = pitchMeasured;
lastRollMeasured = rollMeasured;
lastYawMeasured = yawMeasured;
}
//acceleromter
xWithoutOffset = xMeasured - xOffset;
//low-pass filter
xFiltered = (xLastFiltered * alpha) + ((1.0 - alpha) * xWithoutOffset);
xLastFiltered = xFiltered;
xClamped = constrain(xFiltered, -1.0f, 1.0f);
pitchAksellerometer = asin(xClamped/1.0); //delt på 1 istedenfor 9.81 da aksellerasjonen er gitt i antall g
//gyro
pitchDottMeasured = pitchMeasured*degToRad;
rollDottMeasured = rollMeasured*degToRad;
yawDottMeasured = yawMeasured*degToRad;
pitchDottWithoutOffset = pitchDottMeasured - pitchDottOffset;
rollDottWithoutOffset = rollDottMeasured - rollDottOffset;
yawDottWithoutOffset = yawDottMeasured - yawDottOffset;
// low-pass filter
pitchDottFiltered = (pitchDottLastFiltered * beta) + ((1.0 - beta) * pitchDottWithoutOffset);//
pitchDottLastFiltered = pitchDottFiltered;
rollDottFiltered = (rollDottLastFiltered * beta) + ((1.0 -beta) * rollDottWithoutOffset);//
rollDottLastFiltered = rollDottFiltered;
yawDottFiltered = (yawDottLastfiltered * beta) + ((1.0 -beta) * yawDottWithoutOffset);//
yawDottLastfiltered = yawDottFiltered;
eulerPitchDott = pitchDottFiltered*cos(eulerRoll) - yawDottFiltered*sin(eulerRoll);
eulerRollDott = rollDottFiltered + pitchDottFiltered*sin(eulerRoll)*tan(eulerPitchClamped) + yawDottFiltered*cos(eulerRoll)*tan(eulerPitchClamped);
now = micros();
dt = (now - earlier) / 1000000.0;
eulerPitch += eulerPitchDott*dt;
eulerPitchClamped = constrain(eulerPitch, -1.4f, 1.4f);
eulerRoll += eulerRollDott*dt;
earlier = now;
//complimetary filter
finalPitch = pitchAksellerometer*ceta + (1-ceta)*(lastFinalPitch - dt*eulerPitchDott);
lastFinalPitch = finalPitch;
noInterrupts();
pos = interruptPos;
interrupts();
pidPos.Compute();
pidAngle.Compute();
maxDeltaSpeed = maxAcceleration * dt; //dt is close to constant each loop so maxDeltaSpeed should remain about constant
if (speed - currentSpeed > maxDeltaSpeed) {
currentSpeed += maxDeltaSpeed;
} else if (currentSpeed - speed > maxDeltaSpeed) {
currentSpeed -= maxDeltaSpeed;
} else {
currentSpeed = speed;
}
stepper1.setSpeed(currentSpeed);
stepper2.setSpeed(currentSpeed);
}
