平衡车平衡控制算法

平衡车平衡控制算法.txt

/*

******************************************************************************* * @函数名称: f_ContrlBanlance_Ctrl

* @摘 要: 平衡车平衡控制算法

* @输 入: 无

* @输 出: 无

******************************************************************************* */

static void f_ContrlBanlance_Ctrl(void)

{

float kp,kd;

int_32 speed;

uint_8 direction;

int_16 encoder_left,encoder_right;

// 平衡比例系数

kp = 50.0;

// 平衡微分系数

kd = 15.0;

// 平衡控制算法参数

speed = banlance_angle*kp+banlance_gyro[0]*kd;

// 速度控制算法(可选)

encoder_left = -f_HardwareEncoder_Read(2);

encoder_right = f_HardwareEncoder_Read(3);

banlance_motion = f_ContrlBanlance_Motion(encoder_left, encoder_right);

speed = speed+banlance_motion;

// 增加转速差,实现左右转动控制(可选)

speed_left = speed+10-speed_diff;

speed_right = speed+10+speed_diff;

// 根据得到的转速驱动马达

f_ContrlBanlance_SetSpeed();

}

第 1 页