# Ball Balancer not balancing

Hi everyone,

I'm attempting to follow https://www.instructables.com/Ball-Balancer/ but instead of using a capacitive plate I'm using a webcam to track the location to learn openCV.

There's a few videos of the ball balancer here: Imgur: The magic of the Internet and you can see that it doesn't balance the ball on the platform. The different videos are with different PID values.

I'm not quite sure where the issue is, I've been trying to tune the PID values but I haven't had any success/I don't really know what I'm doing.

Here's the arduino code I'm using:

``````//***3RPS Parallel Manipulator Ball Balancer Code BY Aaed Musa**
//--------------------------------------------------------------

//libraries
#include <AccelStepper.h>
#include <InverseKinematics.h>
#include <MultiStepper.h>
#include <stdint.h>
#include <math.h>

Machine machine(2, 3.125, 1.75, 3.669291339);     //(d, e, f, g) object to define the lengths of the machine

//stepper motors
AccelStepper stepperA(1, 2, 5);  //(driver type, STEP, DIR) Driver A or X
AccelStepper stepperB(1, 3, 6);  //(driver type, STEP, DIR) Driver B or Y
AccelStepper stepperC(1, 4, 7);  //(driver type, STEP, DIR) Driver C or Z
MultiStepper steppers;           // Create instance of MultiStepper

//stepper motor variables
long pos[3];                                            // An array to store the target positions for each stepper motor
int ENA = 8;                                           //enable pin for the drivers
double angOrig = 206.662752199;                        //original angle that each leg starts at
double speed[3] = { 0, 0, 0 }, speedPrev[3], ks = 5000;  //the speed of the stepper motor and the speed amplifying constant

//ball location
int xVal = 0;
int yVal = 0;
int ball_pos[2] = {0,0};

//PID variables
//double kp = 400, ki = 0, kd = 800;
//double kp = 4E-4, ki = 0, kd = 7E-3;                                                       //PID constants
double kp = 4E-4, ki = 0, kd = 4E-3;
double error[2] = { 0, 0 }, errorPrev[2], integr[2] = { 0, 0 }, deriv[2] = { 0, 0 }, out[2];  //PID terms for X and Y directions
long timeI;                                                                           //variables to capture initial times

//Other Variables
double angToStep = 6400 / 360;  //angle to step conversion factor (steps per degree) for 16 microsteps or 3200 steps/rev
bool detected = 0;              //this value is 1 when the ball is detected and the value is 0 when the ball in not detected

void setup() {
Serial.begin(115200);
// Adding the steppers to the steppersControl instance for multi stepper control
//Enable pin
pinMode(ENA, OUTPUT);           //define enable pin as output
digitalWrite(ENA, HIGH);        //sets the drivers off initially
delay(2000);                    //small delay to allow the user to reset the platform
digitalWrite(ENA, LOW);         //sets the drivers on
moveTo(4.25, 0, 0);             //moves the platform to the home position
steppers.runSpeedToPosition();  //blocks until the platform is at the home position
}
void loop() {
PID(0, 0);  //(X setpoint, Y setpoint) -- must be looped
}

//moves/positions the platform with the given parameters
void moveTo(double hz, double nx, double ny) {
//if the ball has been detected
if (detected) {
//calculates stepper motor positon
for (int i = 0; i < 3; i++) {
pos[i] = round((angOrig - machine.theta(i, hz, nx, ny)) * angToStep);
}
//sets calculated speed
stepperA.setMaxSpeed(speed[A]);
stepperB.setMaxSpeed(speed[B]);
stepperC.setMaxSpeed(speed[C]);

//sets acceleration to be proportional to speed
stepperA.setAcceleration(speed[A] * 30);
stepperB.setAcceleration(speed[B] * 30);
stepperC.setAcceleration(speed[C] * 30);

//sets target positions
stepperA.moveTo(pos[A]);
stepperB.moveTo(pos[B]);
stepperC.moveTo(pos[C]);
//runs stepper to target position (increments at most 1 step per call)
for (int i = 0; i < 1; i++){
stepperA.run();
stepperB.run();
stepperC.run();
}

}
//if the ball hasn't been detected
else {
for (int i = 0; i < 3; i++) {
pos[i] = round((angOrig - machine.theta(i, hz, 0, 0)) * angToStep);
}
//sets max speed
stepperA.setMaxSpeed(3200);
stepperB.setMaxSpeed(3200);
stepperC.setMaxSpeed(3200);
//moves the stepper motors
steppers.moveTo(pos);
steppers.run();  //runs stepper to target position (increments at most 1 step per call)
}
}
//takes in an X and Y setpoint/position and moves the ball to that position
void PID(double setpointX, double setpointY) {
//TSPoint p = ts.getPoint();  //measure X and Y positions
//if the ball is detected (the x and y position will not be 0)
if (xVal != 0 && yVal!=0) {
detected = 1;
//calculates PID values
for (int i = 0; i < 2; i++) {
errorPrev[i] = error[i];                                                                     //sets previous error
error[i] = (i == 0) * (xVal - setpointX) + (i == 1) * (yVal - setpointY);  //sets error aka X or Y ball position
integr[i] += error[i] + errorPrev[i];                                                        //calculates the integral of the error (proportional but not equal to the true integral of the error)
deriv[i] = error[i] - errorPrev[i];                                                          //calcuates the derivative of the error (proportional but not equal to the true derivative of the error)
deriv[i] = isnan(deriv[i]) || isinf(deriv[i]) ? 0 : deriv[i];                                //checks if the derivative is a real number or infinite
out[i] = kp * error[i] + ki * integr[i] + kd * deriv[i];                                     //sets output
out[i] = constrain(out[i], -0.25, 0.25);                                                     //contrains output to have a magnitude of 0.25
}
//calculates stepper motor speeds
for (int i = 0; i < 3; i++) {
speedPrev[i] = speed[i];                                                                                                           //sets previous speed
speed[i] = (i == A) * stepperA.currentPosition() + (i == B) * stepperB.currentPosition() + (i == C) * stepperC.currentPosition();  //sets current position
speed[i] = abs(speed[i] - pos[i]) * ks;                                                                                            //calculates the error in the current position and target position
//speed[i] = constrain(speed[i], speedPrev[i] - 1600, speedPrev[i] + 1600);                                                            //filters speed by preventing it from being over 100 away from last speed
speed[i] = constrain(speed[i], 0, 6000);                                                                                           //constrains sped from 0 to 1000
}
//Serial.println((String) "X OUT = " + out[0] + "   Y OUT = " + out[1] + "   Speed A: " + speed[A]);  //print X and Y outputs
}
//if the ball is not detected (the x value will be 0)
else {
//double check that there is no ball
delay(5);                  //10 millis delay before another reading
yVal = Serial.readStringUntil('\r').toInt();  //measure X and Y positions again to confirm no ball
if (xVal == 0 && yVal == 0) {             //if the ball is still not detected
//Serial.println("BALL NOT DETECTED");
detected = 0;
}
}
//continues moving platform and waits until 20 millis has elapsed
timeI = millis();

while (millis() - timeI < 20) {
moveTo(4.25, -out[0], -out[1]);  //moves the platfrom
}
}
``````

Thanks

"Tuning" the PID constants is the last step in the entire process.

The first step is to verify that the image processing produces a valid, stable ball position, and second, if the ball moves off position, the calculated position error term has the correct sign to reduce the error.

Third, the commanded platform moves have to all work together appropriately to correct the position error. If any motor moves in the wrong direction, it won't work.

This is what openCV is currently returning in terms of ball position. I'm having trouble debugging in arduino because the serial monitor is being used by Python. I'm fairly sure the error term has the right sign because when the ball is still it tilts the platform in the direction of the center.

You obviously need to be able to interactively debug.

If the serial monitor is in use, debug output should go to a second serial port, which will need a USB-UART serial port adapter.

Any terminal program can communicate with the Arduino via a serial port, and I tend to use TeraTerm or PuTTY on a PC.

Alternatively, have the Python program communicate via the second serial port.

By the way, these comments suggest that you are making large, random changes to the PID constants. If so, that is not the way to proceed. An internet search for "PID tuning" will turn up tutorials on systematic approaches, which start by examining the proportional error term, in order to estimate a reasonable ballpark value for Kp, with Ki and Kd set to zero.

``````//double kp = 400, ki = 0, kd = 800;
//double kp = 4E-4, ki = 0, kd = 7E-3;                                                       //PID constants
double kp = 4E-4, ki = 0, kd = 4E-3;
``````