Chinese balance group reference design

English document after automatic translation:

Linked file: The balance group reference designe V2_Chinese.zh-CN.en.pdf

Original document:

Linked file: The balance group reference designe V2_Chinese.pdf

Firmware:

Linked file: MagneticCar.rar

The code have all the comments in EN and was easy for me to find the balance control code!!!

On their Timer1 interrupt (Events.c) they call in sequence:

AngleCalculate();
AngleControl();
MotorOutput(); // Output motor control voltage;
---
void AngleCalculate(void)
{
float fDeltaValue;

g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;
g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;

g_fCarAngle = g_fGyroscopeAngleIntegral;
fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
}

void AngleControl(void)
{
float fValue;

fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P +
(CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D;

if(fValue > ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX;
else if(fValue < ANGLE_CONTROL_OUT_MIN) fValue = ANGLE_CONTROL_OUT_MIN;

g_fAngleControlOut = fValue;
}
void MotorOutput(void)
{
float fLeft, fRight;
fLeft = g_fAngleControlOut - g_fSpeedControlOut - g_fDirectionControlOut;
fRight = g_fAngleControlOut - g_fSpeedControlOut + g_fDirectionControlOut;

if(fLeft > MOTOR_OUT_MAX) fLeft = MOTOR_OUT_MAX;
if(fLeft < MOTOR_OUT_MIN) fLeft = MOTOR_OUT_MIN;
if(fRight > MOTOR_OUT_MAX) fRight = MOTOR_OUT_MAX;
if(fRight < MOTOR_OUT_MIN) fRight = MOTOR_OUT_MIN;

g_fLeftMotorOut = fLeft;
g_fRightMotorOut = fRight;
MotorSpeedOut();
}
-------------------
This is the main equation for balance:
fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P + (CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D;