STC32G 三轮车负压电磁

前言

年后就没怎么碰车了，到3月中旬换三轮了，可算有一点成效了，做个记录。

整车效果

移步B站
18届负压电磁全元素
18届负压电磁无元素，速度纯享版
资源
工程代码

控制思路

循迹

角速度环以及速度环

差比和的结果与0做差得到的误差作为角速度环的输入。
角速度环的输出输出作为我们期望小车转向的角速度。

简单对差速小车建模，以顺时针方向的角速度为正参考方向，推导数学公式可得

$v_{Lref}=v_m+\frac{\omega L}{2}$
$v_{Rref}=v_m-\frac{\omega L}{2}$

其中$v_{Lref}$是左轮的参考速度，$v_{Rref}$是右轮的参考速度，$v_m$是小车整体的运行速度，或假想小车中心有一个轮子的运行速度，$L$是车模后轮的轮距，我测量的D车模的后轮距为0.17m，$\omega$为小车的角速度，即角速度环的输出。

得到左右两轮子的参考速度以后，经过速度环控制后得到左右电机的占空比并幅值即可。

其中角速度环用PD控制，P环节用于放大误差送入矫正环节（在这里的三轮车矫正环节即控制两个轮子的速度以矫正误差），D环节用于抑制车体的震荡；速度环用PI控制，P放大误差进行速度调节，I用于消除静差，尤其是当我们上负压以后，I尤为重要。

由于三轮车的运动轨迹完全取决于两个轮子速度之间的关系，两个轮子的速度环控制必须做到快速而精准，需花时间调节速度环的参数。

电机PID参数整定

环岛处理

1. 检测到圆环（预圆环）（某个电感大于某个阈值）
2. 到达最佳入环位置（强制入环，直到陀螺仪大于某个，开始园内循迹）
3. 圆内循迹
4. 出环。

障碍处理

1. 识别障碍**(TOF测距小于某个限制)**
2. 避开障碍（左右轮固定差速，直到陀螺仪积分大于某个角度）
3. 回到赛道（左右轮固定差速，直到陀螺仪为小于某个角度）
4. 姿态调整（让小车的姿态基本与赛道平齐，直到陀螺仪角度在某个范围内或误差在某个范围内）

关键代码部分

差比和以及当前速度计算

三电感电磁循迹小车

角速度环

void dir_loop()
{
    float temp;
    pid_steer.ek =  0 - error;
    temp = pid_steer.KP * pid_steer.ek
           + imu660ra_gyro_z / 65.6;
//           + 300 * (pid_steer.ek - pid_steer.ek_1); // 参考角速度

    pid_motor_L.SetValue = speed_rate * speed_goal  -  temp * 0.0875;
    pid_motor_R.SetValue = speed_rate * speed_goal  +  temp * 0.0875;
}

速度环

void speed_loop()
{
    float inc_L, inc_R;
    // 限制内轮速度
    if(pid_motor_L.SetValue < 0)
        pid_motor_L.SetValue = 0;
    if(pid_motor_R.SetValue < 0)
        pid_motor_R.SetValue = 0;

    // 外轮速度限制
    if(pid_motor_L.SetValue > 2 * speed_goal * speed_rate)
        pid_motor_L.SetValue = 2 * speed_goal * speed_rate;
    if(pid_motor_R.SetValue > 2 * speed_goal * speed_rate)
        pid_motor_R.SetValue = 2 * speed_goal * speed_rate;

    inc_L = PID_Control_Inc(&pid_motor_L, 0);
    inc_R = PID_Control_Inc(&pid_motor_R, 0);

    duty_L += inc_L;
    duty_R += inc_R;

    if(duty_L > Motor_UpperLimit)
        duty_L = Motor_UpperLimit;
    if(duty_L < Motor_LowerLimit)
        duty_L = Motor_LowerLimit;

    if(duty_R > Motor_UpperLimit)
        duty_R = Motor_UpperLimit;
    if(duty_R < Motor_LowerLimit)
        duty_R = Motor_LowerLimit;
}

环岛处理

void circ_handler()
{
    static uint16 count_delay_R_circ_pre;
    static uint16 count_delay;

    if(flag_L_circ_pre) // 预圆环阶段
    {
        count_delay_R_circ_pre++;
        if(count_delay_R_circ_pre > 0.5 / speed_goal * 60 * TIM1_ISR_F) //
        {
            flag_L_circ_pre = 0;
            flag_L_circ_frc = 1; // force to get into circ
        }
    }
    if(flag_L_circ_frc)
    {
        Angle_circ += imu660ra_gyro_z * 0.002 / 65.6;
        if(Angle_circ < -35)
        {
            flag_L_circ_frc = 0;
            flag_L_circ_in = 1;
            flag_L_circ_out = 0;
        }
        else
        {
            pid_motor_L.SetValue = 0 * speed_rate * speed_goal;
            pid_motor_R.SetValue = 2 * speed_rate * speed_goal;
        }

    }
    if(flag_L_circ_in)
    {
        Angle_circ += imu660ra_gyro_z * 0.002 / 65.6;
        if(Angle_circ <= -300)
            flag_L_circ_in = 0;
    }
    if(flag_L_circ_in == 0 && flag_L_circ_out == 0)
    {
        count_delay++;
        if(count_delay > 5 * TIM1_ISR_F)
        {
            flag_L_circ_out = 1; // 已出环
            count_delay = 0;
            Angle_circ = 0;
        }
    }
}

障碍处理

void block_handler()
{
    if(flag_block_detected)
    {
        pid_motor_L.SetValue = 0 * speed_rate * speed_goal;
        pid_motor_R.SetValue = 2 * speed_rate * speed_goal;
        Angle_block += imu660ra_gyro_z * 0.002 / 65.6;
    }
    if(Angle_block < -45)
    {
        flag_block_detected = 0;
        flag_block_back = 1;
    }
    if(flag_block_back)
    {
        if(Angle_block < 50)
        {
            pid_motor_R.SetValue = 0 * speed_rate * speed_goal;
            pid_motor_L.SetValue = 2 * speed_rate * speed_goal;
        }
        else
            flag_block_adjust = 1;
        if(flag_block_adjust)
        {
            pid_motor_L.SetValue = 0.3 * speed_rate * speed_goal;
            pid_motor_R.SetValue = 1.7 * speed_rate * speed_goal;
        }


        Angle_block += imu660ra_gyro_z * 0.002 / 65.6;
    }
    if(Angle_block < 10 && Angle_block > -10 && flag_block_adjust)
    {
        flag_block_detected = 0;
        flag_block_back = 0;
        flag_block_adjust = 0;
        Angle_block = 0;

    }
}