Bot balancing without gyros … for a few seconds at least

No gyros, no accelerometers – only a sonar ranger and lots of code.

The motors used are standard servos modified for continuous rotation, sonar sensor is a HC-SR04 as detailed in a previous post

Code ? Well here’s a simplified portion of the PID loop:

int Drive = 0; // PWM value sent to Servos
int Error[5]; // array of 5 Error elements
int P = 0; // proportional term
int I = 0; // integral term
int D = 0; // derivative term

#define Current 0
#define Sum 1
#define Last 2
#define SecondToLast 3
#define Delta 4

int PID() {
Error[Current] = SetPoint – Ptime;
P = Error[Current] * Kp;
Error[Sum] = Error[Current] + Error[Last] + Error[SecondToLast];
I = Error[Sum] * Ki;
Error[Delta] = Error[Current] – Error[Last];
D = Error[Delta] * Kd;
Drive = P + I + D;
Error[SecondToLast] = Error[Last];
Error[Last] = Error[Current];

return Drive;
}