RT-Thread Hoist_Motor PID

本节介绍的是一个举升电机，顾名思义，通过转轴控制物体升降，为双通道磁性译码器，利用电调进行操控，具体驱动类似于大学期间最大众的SG180°舵机，在一定的频率下，通过调制脉宽进行控制。

设备介绍

• 具体实控
  例如在50Hz情况下，即周期为20ms
  ①驱动信号区间： 区间一(0.5ms-1.5ms )和区间二(1.5ms-2.5ms ) （注意都是开区间且存在死区）
  ②其中区间一和区间二分别表示不同的方向运动，例区间一表示正方向，则区间二表示反方向。
  ③旋转速度呈现为“ V ” 字形，即0.5ms和2.5ms分别表示为正反方向的最快速度，1.5ms左右分别表示正反方向的最慢速度。其中小于等于0.5ms时、1.5ms时和大于等于2.5ms时电机都保持停转状态。

• 电调引线
  再看电调引线，可注意 分别有7根线，如上图，其中粗线有四根，红黑两根线为电源线，这里接24V，还有两根黄蓝接电机，控制电机不同转向；细线有三根，分别是红黑白，其中红黑为电源线，输出5V，可选择是否需要给MCU供电，黑线接地，白色线为PWM信号输入线，接收MCU发过来的信号进而控制电机转动。

• 电机引线
  如上图，电机有6根线，跟大众使用的编码器电机无差别，两根电机引线+两根编码器电源线+AB相

• PID
  由于前几节中已介绍pwm的基本使用，这里就不再介绍，下面我直接介绍我的PID设计
  ①首先在主函数中对PID进行初始化，即设置目标数、比例积分微分常数、输出限幅、积分限幅。
  ②在定时器中断中对旋转产生的脉冲进行采样，然后进行PID运算，将输出信号传入电调。
  ③在P、I、D参数调节中，我习惯先调I，将P和D置0，从小–>大调，观察电机变化，这里我使用的是一个上位机软件VOFA，串口协议，通过上传指定数据，可以很好的观察波形变化，可发现 它呈现出一个缓慢上升的波形，这时可以对I进行放大，加快上升速度，我的调节是：调至I能很好的达到目标点，且在第一次达到目标点时，可以让它超出适量值，再对比左右部分数值，发现这个点的I值达到目标点的速度更快，则这个点就是我要的I值。然后再对P进行调节，也可以选择从小–>大调，P可以很好的反馈出控制器对电机的控制速度，即加大对目标值的反应，具体调节方法同I。最后在对D进行调节（对于一般的控制，PI两个参数足以满足需求，如果最后完美，可再选择D），D表现为误差变化率的变化，可以抑制电机的超调等，对于D，你可以用某物体人为的阻挡电机转动，观察电机变化，例如你提供足够大的阻力，电机肯定会直接拉满，这时观察电机再次回到目标点的时间。

软件设计

• 1. 设备初始化

/************************** Uart ************************************/
void Uart_Init(void)
{
    char str[] = "hello RT-Thread!\r\n";
    /* step1：查找串口设备 */
    serial = rt_device_find(SAMPLE_UART_NAME);

    /* step2：修改串口配置参数 */
    config.baud_rate = BAUD_RATE_115200;        //修改波特率为 115200
    config.data_bits = DATA_BITS_8;           //数据位 8
    config.stop_bits = STOP_BITS_1;           //停止位 1
    config.bufsz     = 128;                   //修改缓冲区 buff size 为 128
    config.parity    = PARITY_NONE;           //无奇偶校验位

    /* step3：控制串口设备。通过控制接口传入命令控制字，与控制参数 */
    rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config);

    /* step4：打开串口设备。以中断接收及轮询发送模式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
}

/************************** Timer ************************************/
void Timer_Init(void)
{
    rt_hwtimer_mode_t mode;            /* 定时器模式  */
    rt_hwtimerval_t timeout_s;         /* 定时器超时值  */
    rt_uint32_t freq = 1000000;       /* 计数频率  */

    // 使用前必须先手动打开时钟
    __HAL_RCC_TIM3_CLK_ENABLE();

    /* 查找定时器设备 */
    hw_dev = rt_device_find(HWTIMER_DEV_NAME);
    if (hw_dev == RT_NULL)
    {
      rt_kprintf("hwtimer sample run failed! can't find %s device!\n", HWTIMER_DEV_NAME);
    }

    /* 以读写方式打开设备 */
    rt_err_t ret = rt_device_open(hw_dev, RT_DEVICE_OFLAG_RDWR);
    if (ret != RT_EOK)
    {
      rt_kprintf("open %s device failed!\n", HWTIMER_DEV_NAME);
    }

    /* 设置超时回调函数 */
    rt_device_set_rx_indicate(hw_dev, Timer3_Out);

    /* 设置计数频率(若未设置该项，默认为1Mhz 或 支持的最小计数频率) */
    rt_device_control(hw_dev, HWTIMER_CTRL_FREQ_SET, &freq);
    /* 设置模式为周期性定时器（若未设置，默认是HWTIMER_MODE_ONESHOT）*/
    mode = HWTIMER_MODE_PERIOD;
    ret = rt_device_control(hw_dev, HWTIMER_CTRL_MODE_SET, &mode);
    if (ret != RT_EOK)
    {
      rt_kprintf("set mode failed! ret is :%d\n", ret);
    }

    /* 设置定时器超时值为2s并启动定时器 */
    timeout_s.sec = 0;      /* 秒 */
    timeout_s.usec = 40000;     /* 微秒 */
    if (rt_device_write(hw_dev, 0, &timeout_s, sizeof(timeout_s)) != sizeof(timeout_s))
    {
        rt_kprintf("set timeout value failed\n");
    }
}

/************************** PWM ************************************/
void PWM_Init(void)
{
    /* 查找设备 */
    pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
    if (pwm_dev == RT_NULL)
    {
       rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
    }
    /* 使能设备 */
    rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL1);

    /* 设置PWM周期和脉冲宽度 */
    rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL1, period, pulse);
}

/************************** Encoder ************************************/
void Encoder_Init(void)
{
    /* 查找脉冲编码器设备 */
    pulse_encoder_dev = rt_device_find(PULSE_ENCODER_DEV_NAME);
    if (pulse_encoder_dev == RT_NULL)
    {
        rt_kprintf("pulse encoder sample run failed! can't find %s device!\n", PULSE_ENCODER_DEV_NAME);
    }

    /* 以只读方式打开设备 */
    rt_err_t ret = rt_device_open(pulse_encoder_dev, RT_DEVICE_OFLAG_RDONLY);
    if (ret != RT_EOK)
    {
        rt_kprintf("open %s device failed!\n", PULSE_ENCODER_DEV_NAME);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

• 2. 以下为位置式PID计算，可供参考

PID_VAR_TYPE Position_PID_Cal(PID * s_PID,PID_VAR_TYPE now_point)
{
    s_PID->LastResult = s_PID->Result;                 // 简单赋值运算
    //误差计算
    s_PID->Error = s_PID->SetPoint - now_point;
    s_PID->SumError += s_PID->Error;                            //积分误差累加
    //积分限幅
    PID_VAR_TYPE IOutValue = s_PID->SumError * s_PID->Integral;
    if(IOutValue > s_PID->IntegralMax)IOutValue = s_PID->IntegralMax;
    else if(IOutValue < s_PID->IntegralMin)IOutValue = s_PID->IntegralMin;
    //PID计算
    s_PID->Result =  s_PID->Proportion  *  s_PID->Error                          // 比例项
                   + IOutValue                                                     // 积分项
                   + s_PID->Derivative  * (s_PID->Error - s_PID->LastError);     // 微分项

    s_PID->PrevError = s_PID->LastError;                               // 简单赋值运算
    s_PID->LastError = s_PID->Error;                       // 简单赋值运算

    //输出限幅
    if(s_PID->Result > s_PID->OutMax)s_PID->Result = s_PID->OutMax;
    else if(s_PID->Result < s_PID->OutMin)s_PID->Result = s_PID->OutMin;

    return s_PID->Result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

• 3. VOFA
  正如上面所介绍的一个上位机软件，可观察PID波形变化，协助开发，具体协议如下所示：

void SendDatatoVoFA(rt_uint8_t byte[],float v_real)
{
    rt_uint8_t t_test=0;//四位发送
    rt_uint8_t send_date[4]={0};//发送数据

    Float_to_Byte(v_real,byte);  //类型转换
    for(t_test=0;t_test<4;t_test++)
    {
        rt_device_write(serial, 0, &byte[t_test], 1);
    }

    send_date[0]=0X00;send_date[1]=0X00;
    send_date[2]=0X80;send_date[3]=0X7f;
    for(t_test=0;t_test<4;t_test++)
    {
        rt_device_write(serial, 0, &send_date[t_test], 1);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

• 4. 在定时器中断不断进行PID计算，并输出PWM信号

static rt_err_t Timer3_Out(rt_device_t dev, rt_size_t size)
{    
    rt_device_read(pulse_encoder_dev, 0, &count, 1);  /* 读取脉冲编码器计数值 */
    rt_int32_t out=Position_PID_Cal(&pid1,count);
    rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL1, period, out);
    rt_kprintf("%d - %d\n",count,out);

    SendDatatoVoFA(byte,count);   //发送到vofa上位机，查看波形
    rt_device_control(pulse_encoder_dev, PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);/* 清空脉冲编码器计数值 */

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12