float g_middleAngle = 1.0;//平衡角度
float g_kpBalance = -85800.0;
float g_kdBalance = -400;


static int ControlBalance(float angle, short gyro)
{
    int outpwm = 0;
    float angleBias = 0.0;
    float gyroBias = 0.0;
    float tempAngle = 0.0;
    float tempGyro = 0.0;


    tempAngle = 0 - angle;//極性控制
    tempGyro = 0 - gyro;


    angleBias = g_middleAngle - tempAngle;
    gyroBias = 0 - tempGyro;
    outpwm = (g_kpBalance / 100 * angleBias + g_kdBalance
     * gyroBias / 100);


    return outpwm;
}

float g_kpSpeed = 95800.0;
float g_kiSpeed = 200;


static int ControlSpeed(long int left, long int right)
{
    int outpwm = 0;
    int speedBias = 0;
    int speedBiasLowpass = 0;
    static int speed_i = 0;
    float a = 0.68;
    static int speedBiasLast = 0;


    speedBias = 0 - left - right;
    speedBiasLowpass = (1 - a) * speedBias
        + a * speedBiasLast;
    speedBiasLast = speedBiasLowpass;


    speed_i += speedBiasLowpass;
    speed_i = limit_data(speed_i, SPEED_H, SPEED_L);


    outpwm = (g_kpSpeed * speedBiasLowpass / 100 +
        gkiSpeed * speed_i / 100);


    return outpwm;
}

static int ControlTurn(short gyro)
{
  int outpwm;


    outpwm = g_kpTurn * gyro;
  return outpwm;
}