I think this is the wrong approach. The higher the CoG, the greater the toppling moment and the harder it is to correct for it. Start by making it easy for yourself with a design that is only just top-heavy and no more. Like this one:
Robot
Are you sure that your motors have enough torque to keep the robot vertical? It would be a good idea to estimate the CoG and calculate the toppling moment at, say, 10 degrees from vertical. Then your motors need to have at least that much torque at whatever speed you will run them at. Preferably a lot more. Check the datasheet.
I’m not familiar with the ADXL345 but you need to be aware that calculating the tilt angle from the three acceleration vectors like your code does will only give good results when the only acceleration is due to gravity. But a self-balancing robot is always making small accelerations, and these will mess up the readings. Sophisticated accelerometers do some clever math and give you the tilt angle directly.
Here’s my code. You’re not going to be able to use it directly but it will give you an idea. This is cascade control where the outer speed control PID loop cascades onto the inner roll control PID loop. This allows the two loops to be tuned independently of each other.
This robot uses stepper motors and the PWM pulse train goes to a stepper controller rather than a DC bridge but the principle is similar. I’m not very happy with this design - I think stepper motors are not right for this application, and one day I will rebuild it with DC motors under current control (not speed control).
// The detailed strategy is that a speed control PID controller is cascaded onto a roll control PID controller.
// THe setpoint of the speed controller is zero, and this has the effect of limiting any tendency for the robot
// to run continually in any direction.
// The roll controller (which is actually controlling pitch) carries out the balancing around zero roll.
speedControlPID.SetSampleTime(5);
speedControlPID.SetOutputLimits(-speedControlRange, speedControlRange);
speedControlPID.Start(0.0,0.0,speedControlPIDSetpoint); // input, current output and setpoint for PID controller
rollControlPID.SetSampleTime(5);
rollControlPID.SetOutputLimits(-rollControlRange, rollControlRange);
rollControlPID.Start(0.0,0.0,0.0); // input, current output and setpoint for PID controller. Overwritten by tuning pots in next tab
digitalWrite(DRVENBL, false); // drive output is low to enable
}
if(oneShotOff) {digitalWrite(DRVENBL, true);} // drive output is high to disable
// The PID controller output is mapped to the PWM period used by TCC0 when generating the PWM pulse train to drive the motors.
// The floating-point PID output and scaling variables are mutiplied by 10000 in the "map" function below,
// because the absolute values can be quite low (typical max < 20),
// and, when converted to integers, may not have sufficient granularity to drive the PWM output variable smooothly.
// Note that the rollDeadband value below is not applied to the measured roll in degrees, but rather to the
// PID controller output. The value therefore has no meaning in terms of degrees and should not be mistaken for that.
// It is an empirical number that provides a means of disabling the motor drive output around the narrow band
// where it is changing direction.
speedControlPID.Setpoint(speedControlPIDSetpoint); // Speed control PID setpoint is zero
speedControlPIDOutput = speedControlPID.Run(rollControlPIDOutput); // speed control PID PV (speed) is approximated by output of roll control PID
rollControlPID.Setpoint(-speedControlPIDOutput); // speed control PID output cascades to setpoint of roll control
rollControlPIDOutput = rollControlPID.Run(measuredRollFiltered); // PV input to PID controller is the measured roll. Setpoint is zero (i.e. vertical).
rollControlPIDOutputFiltered = rollControlPIDOutput; // default unlimited case which may get overwritten if parameter has changed by more than MAXCHANGEPERSAMPLE
// Control intent is to map PID controller output linearly to drive speed command,
// with speed increasing with increasing controller output (tilt away from vertical).
// First convert the PID controller output over the required control range, into
// the required speed in steps per second, over speed range from slowest to fastest
int i = abs(rollControlPIDOutputFiltered * 10000.0); // Remove sign, as the pulse train is the same in either direction.
int j = rollControlRange * 10000.0;
speedStepsPerSec = map(i, 0, j, speedSlowest, speedFastest);
// Then calculate corresponding PWM period in microsecond ticks.
PWMperiod = (1E6 / speedStepsPerSec); // divide into 1E6 since PWM ticks are 1Mhz and that is what TCC0 is counting.
REG_TCC0_PERB = PWMperiod; // load desired PWM period into TCC0 PER register to generate PWM pulse train on PULSE (D5)
while(TCC0->SYNCBUSY.bit.PERB); // Wait for synchronization
REG_TCC0_CCB1 = PWMperiod / 2; // Pulse train duty always half the period
while(TCC0->SYNCBUSY.bit.CCB1); // Wait for synchronization
// This section adjusts the balance point of the robot in real time. The direction of drive as
// required by the PID output loop is used to increase or decrease the balance point value.
// Effectively this is a closed loop that averages out the direction of drive to close in on the perfect balance point.
if (rollControlPIDOutputFiltered > 0.5 && sustainedOn) {
if (COGcompensation > -45) COGcompensation -= COGadder;
}
if (rollControlPIDOutputFiltered < -0.5 && sustainedOn) {
if(COGcompensation < - 25) COGcompensation += COGadder;
}
// The PID controller output is now mapped to the drive direction logic taking account of a deadband around zero.
// Note that the variables below controlling the drive are not measured roll but the PID controller output.
// This could theoretically lead to the situation where the drive is driven in what would be the wrong direction
// based on the roll at that instant. However this would probably only occur if there was a lot of integral accumulated,
// since that is the only reason why the PID output would be the opposite sign to the measured roll.
if (-rollControlPIDOutputFiltered > rollDeadband){ // negative PID output will occur when roll is positive (forward).
rollPositive = true;
rollLessThanDeadband = false;
rollNegative = false;
}
if (-rollControlPIDOutputFiltered < rollDeadband && rollControlPIDOutputFiltered > -rollDeadband){
rollPositive = false;
rollLessThanDeadband = true;
rollNegative = false;
}
if (-rollControlPIDOutputFiltered < -rollDeadband){
rollPositive = false;
rollLessThanDeadband = false;
rollNegative = true;
}
// driveForward() and DriveReverse() do not need to persist; in fact it is preferable not to issue continuous TCC0 enable instructions.
// So generate a one-shot for each.
driveFwdOneShot = rollPositive && !digitalRead (SWITCH) && !driveFwdOneShotSetup;
driveFwdOneShotSetup = !(rollPositive && !digitalRead (SWITCH));
if(driveFwdOneShot){driveForward();}
if (rollLessThanDeadband || digitalRead (SWITCH)){stopDrive();}
driveRevOneShot = rollNegative && !digitalRead (SWITCH) && !driveRevOneShotSetup;
driveRevOneShotSetup = !(rollNegative && !digitalRead (SWITCH));
if(driveRevOneShot){driveReverse();}
