狂暴战车使用 rt-robots 软件包 “组装” car

发布于 2019-07-24 11:30:26 浏览：2606 订阅该版

[tocm]

#  狂暴战车 使用 rt-robots 软件包 “组装” car

## 首先需要获取软件包，过程如下

1. 在工程目录打开 env 工具，使用 ”pkgs --upgrade“ 更新软件包索引（ps：此只需要进行一次）

2. env 中使用 ”menuconfig“ 命令选中 rt-botos 软件包，然后退出配置界面并保存

3. 若没有使能 env 的自动更新软件包功能则需要手动更新，在 env 中使用命令 “scons --update” 更新软件包（下载）

## 然后添加应用文件，重新生成工程

在 applications 目录下添加空白文件 car.c

之后就要重新生成工程了，当然如果使用 gcc 编译的话，没有这个需要， 此处我使用 mdk 编译下载，需要生成工程。在 env 中使用命令 “scons --target=mdk5” 生成 mdk 工程。

## ”组装“ car

在空白的 car.c 中使用 rt-robots “组装” 自己的小车。组装流程大致是：

1. 初始化车轮 (为车轮添加电机、编码器、PID)

2. 初始化动力学模型

3. 初始化车底盘 (为底盘添加车轮、动力学模型)

完成后 car.c 内容如下 （_*当前含有遥控部分*_）

```C
#include <rtthread.h>
#include <chassis.h>
#include <command.h>
#include <ps2.h>
#include <dual_pwm_motor.h>
#include <ab_phase_encoder.h>
#include <inc_pid_controller.h>

#define DBG_SECTION_NAME  "car"
#define DBG_LEVEL         DBG_LOG
#include <rtdbg.h>

// MOTOR
#define LEFT_FORWARD_PWM            "pwm4"
#define LEFT_FORWARD_PWM_CHANNEL    3
#define LEFT_BACKWARD_PWM           "pwm4"
#define LEFT_BACKWARD_PWM_CHANNEL   4

#define RIGHT_FORWARD_PWM           "pwm2"
#define RIGHT_FORWARD_PWM_CHANNEL   3
#define RIGHT_BACKWARD_PWM          "pwm2"
#define RIGHT_BACKWARD_PWM_CHANNEL  4

// ENCODER
#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)
#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)
#define RIGHT_ENCODER_A_PHASE_PIN   36      //
#define RIGHT_ENCODER_B_PHASE_PIN   8       //
#define PULSE_PER_REVOL           2000      // Real value 2000
#define SAMPLE_TIME                 50

// CONTROLLER PID
#define PID_SAMPLE_TIME             50
#define PID_PARAM_KP                6.6
#define PID_PARAM_KI                6.5
#define PID_PARAM_KD                7.6

// WHEEL
#define WHEEL_RADIUS             0.066
#define GEAR_RATIO                   1

// CAR
chassis_t chas;

#define WHEEL_DIST_X                 0
#define WHEEL_DIST_Y              0.13

// Car Thread
#define THREAD_PRIORITY             10
#define THREAD_STACK_SIZE          512
#define THREAD_TIMESLICE             5

static rt_err_t car_forward(rt_int8_t cmd, void *param);
static rt_err_t car_backward(rt_int8_t cmd, void *param);
static rt_err_t car_turnleft(rt_int8_t cmd, void *param);
static rt_err_t car_turnright(rt_int8_t cmd, void *param);
static rt_err_t car_stop(rt_int8_t cmd, void *param);

static rt_thread_t tid_car = RT_NULL;

void car_thread(void *param)
{
    // TODO

struct velocity target_velocity;

target_velocity.linear_x = 0.00f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = 0;
    chassis_set_velocity(chas, target_velocity);

while (1)
    {
        rt_thread_mdelay(50);
        chassis_update(chas);
    }

}

void car_init(void *parameter)
{
    // 1. Initialize two wheels - left and right
    wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);
    if (c_wheels == RT_NULL)
    {
        LOG_D("Failed to malloc memory for wheels");
    }

// 1.1 Create two motors
    dual_pwm_motor_t left_motor   = dual_pwm_motor_create(LEFT_FORWARD_PWM, LEFT_FORWARD_PWM_CHANNEL, LEFT_BACKWARD_PWM, LEFT_BACKWARD_PWM_CHANNEL);
    dual_pwm_motor_t right_motor  = dual_pwm_motor_create(RIGHT_FORWARD_PWM, RIGHT_FORWARD_PWM_CHANNEL, RIGHT_BACKWARD_PWM, RIGHT_BACKWARD_PWM_CHANNEL);

// 1.2 Create two encoders
    ab_phase_encoder_t left_encoder  = ab_phase_encoder_create(LEFT_ENCODER_A_PHASE_PIN, LEFT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL);
    ab_phase_encoder_t right_encoder = ab_phase_encoder_create(RIGHT_ENCODER_A_PHASE_PIN, RIGHT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL);

// 1.3 Create two pid contollers
    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD);
    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD);

// 1.4 Add two wheels
    c_wheels[0] = wheel_create((motor_t)left_motor,  (encoder_t)left_encoder,  (controller_t)left_pid,  WHEEL_RADIUS, GEAR_RATIO);
    c_wheels[1] = wheel_create((motor_t)right_motor, (encoder_t)right_encoder, (controller_t)right_pid, WHEEL_RADIUS, GEAR_RATIO);

// 2. Iinialize Kinematics - Two Wheel Differential Drive
    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);

// 3. Initialize Chassis
    chas = chassis_create(c_wheels, c_kinematics);

// Set Sample time
    encoder_set_sample_time(chas->c_wheels[0]->w_encoder, SAMPLE_TIME);
    encoder_set_sample_time(chas->c_wheels[1]->w_encoder, SAMPLE_TIME);
    controller_set_sample_time(chas->c_wheels[0]->w_controller, PID_SAMPLE_TIME);
    controller_set_sample_time(chas->c_wheels[1]->w_controller, PID_SAMPLE_TIME);

// 4. Enable Chassis
    chassis_enable(chas);

// Register command
    command_register(COMMAND_CAR_STOP     , car_stop);
    command_register(COMMAND_CAR_FORWARD  , car_forward);
    command_register(COMMAND_CAR_BACKWARD , car_backward);
    command_register(COMMAND_CAR_TURNLEFT , car_turnleft);
    command_register(COMMAND_CAR_TURNRIGHT, car_turnright);

rt_kprintf("car command register complete<br>");

// Controller
    ps2_init(28, 29, 62, 61);

// thread
    tid_car = rt_thread_create("tcar",
                              car_thread, RT_NULL,
                              THREAD_STACK_SIZE,
                              THREAD_PRIORITY, THREAD_TIMESLICE);

if (tid_car != RT_NULL)
    {
        rt_thread_startup(tid_car);
    }
}

static rt_err_t car_forward(rt_int8_t cmd, void *param)
{
    struct velocity target_velocity;

target_velocity.linear_x = 0.05f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = 0;
    chassis_set_velocity(chas, target_velocity);

LOG_D("forward cmd");

return RT_EOK;
}

static rt_err_t car_backward(rt_int8_t cmd, void *param)
{
    struct velocity target_velocity;

target_velocity.linear_x = -0.05f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = 0;
    chassis_set_velocity(chas, target_velocity);

LOG_D("backward cmd");

return RT_EOK;
}

static rt_err_t car_turnleft(rt_int8_t cmd, void *param)
{
    struct velocity target_velocity;

target_velocity.linear_x = 0.00f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = 0.5;
    chassis_set_velocity(chas, target_velocity);

LOG_D("turnleft cmd");

return RT_EOK;
}

static rt_err_t car_turnright(rt_int8_t cmd, void *param)
{
    struct velocity target_velocity;

target_velocity.linear_x = 0.00f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = -0.5;
    chassis_set_velocity(chas, target_velocity);

LOG_D("turnright cmd");

return RT_EOK;
}

static rt_err_t car_stop(rt_int8_t cmd, void *param)
{
    struct velocity target_velocity;

target_velocity.linear_x = 0.00f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = 0;
    chassis_set_velocity(chas, target_velocity);

LOG_D("stop cmd");

return RT_EOK;
}
```

### 说明：

- PULSE_PER_REVOL 车轮转一圈输出的脉冲数
- SAMPLE_TIME 编码器采样时间，现单位为 ms
- PID_SAMPLE_TIME PID计算周期，现单位为 ms 
- WHEEL_RADIUS 车轮半径， 标准单位 m
- GEAR_RADIUS 电机减速比，无量纲
- WHEEL_DIST_X X轴方向两轮之间的间距，标准单位 m
- WHEEL_DIST_Y Y轴方向两轮之间的间距，标准单位 m

_*Command register 和 ps2_init 与 "组装" car 无关，无需关注*_

## 注意事项

- 关注 rt-robots 软件包文档， 注意小车坐标系等

13 个回答

moss 2019-07-24

这家伙很懒，什么也没写！

shadowliang 2019-07-25

Hello,world!!!

sogw 2019-07-25

这家伙很懒，什么也没写！

AdamShen 2019-07-30

这家伙很懒，什么也没写！

sogw 2019-07-30

这家伙很懒，什么也没写！

sogw 2019-08-01

这家伙很懒，什么也没写！

ysj 2019-09-13

这家伙很懒，什么也没写！

sogw 2019-10-09

这家伙很懒，什么也没写！

依最新的rt-robot更新“组装”过程

```c
#include <rtthread.h>
#include <chassis.h>
#include <command.h>
#include <ps2.h>
#include <dual_pwm_motor.h>
#include <ab_phase_encoder.h>
#include <inc_pid_controller.h>

#define DBG_SECTION_NAME  "car"
#define DBG_LEVEL         DBG_LOG
#include <rtdbg.h>

// MOTOR
#define LEFT_FORWARD_PWM            "pwm4"
#define LEFT_FORWARD_PWM_CHANNEL    3
#define LEFT_BACKWARD_PWM           "pwm4"
#define LEFT_BACKWARD_PWM_CHANNEL   4

#define RIGHT_FORWARD_PWM           "pwm2"
#define RIGHT_FORWARD_PWM_CHANNEL   3
#define RIGHT_BACKWARD_PWM          "pwm2"
#define RIGHT_BACKWARD_PWM_CHANNEL  4

// ENCODER
#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)
#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)
#define RIGHT_ENCODER_A_PHASE_PIN   38      // GET_PIN(C, 6)
#define RIGHT_ENCODER_B_PHASE_PIN   39      // GET_PIN(C, 7)
#define PULSE_PER_REVOL           2000      // Real value 2000
#define ENCODER_SAMPLE_TIME         50

// PID CONTROLLER
#define PID_SAMPLE_TIME             50
#define PID_PARAM_KP                6.6
#define PID_PARAM_KI                6.5
#define PID_PARAM_KD                7.6

// WHEEL
#define WHEEL_RADIUS             0.066
#define GEAR_RATIO                   1

// CAR
chassis_t chas;

#define WHEEL_DIST_X                 0
#define WHEEL_DIST_Y              0.13

// PS2
#define PS2_CS_PIN                  28
#define PS2_CLK_PIN                 29
#define PS2_DO_PIN                   4
#define PS2_DI_PIN                  36

// Car Thread
#define THREAD_PRIORITY             10
#define THREAD_STACK_SIZE          512
#define THREAD_TIMESLICE             5

static rt_thread_t tid_car = RT_NULL;

void car_thread(void *param)
{
    // TODO

struct velocity target_velocity;

target_velocity.linear_x = 0.00f;
    target_velocity.linear_y = 0;
    target_velocity.angular_z = 0;
    chassis_set_velocity(chas, target_velocity);

// Open-loop control
    // controller_disable(chas->c_wheels[0]->w_controller);
    // controller_disable(chas->c_wheels[1]->w_controller);

while (1)
    {
        rt_thread_mdelay(ENCODER_SAMPLE_TIME);
        chassis_update(chas);
    }

//    chassis_destroy(chas);
}

// 1.2 Create two encoders
    ab_phase_encoder_t left_encoder  = ab_phase_encoder_create(LEFT_ENCODER_A_PHASE_PIN, LEFT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL, ENCODER_SAMPLE_TIME);
    ab_phase_encoder_t right_encoder = ab_phase_encoder_create(RIGHT_ENCODER_A_PHASE_PIN, RIGHT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL, ENCODER_SAMPLE_TIME);

// 1.3 Create two pid contollers
    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);
    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);

// 2. Iinialize Kinematics - Two Wheel Differential Drive
    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);

// 3. Initialize Chassis
    chas = chassis_create(c_wheels, c_kinematics);

// 4. Enable Chassis
    chassis_enable(chas);

// Remote-control
    command_init(chas);

ps2_init(PS2_CS_PIN, PS2_CLK_PIN, PS2_DO_PIN, PS2_DI_PIN);

if (tid_car != RT_NULL)
    {
        rt_thread_startup(tid_car);
    }
}
```