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

发布于 2019-07-24 11:30:26
    本帖最后由 sogw 于 2019-8-10 08:54 编辑


[md][md]# 狂暴战车 使用 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
#include
#include
#include
#include
#include
#include

#define DBG_SECTION_NAME "car"
#define DBG_LEVEL DBG_LOG
#include

// 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\n");

// 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 软件包文档, 注意小车坐标系等[/md][/md]

查看更多

关注者
0
被浏览
1.4k
12 个回答
moss
moss 2019-07-24
那么强大吗?自带两轮差速驱动模型啊
shadowliang
shadowliang 2019-07-25
厉害厉害,正疑问着rt-robots 软件包应该怎样用呢。感谢楼主分享。
sogw
sogw 2019-07-25
moss 发表于 2019-7-24 11:45
那么强大吗?自带两轮差速驱动模型啊


对的, 还有四轮等的动力学模型
AdamShen
AdamShen 2019-07-30
命令是pkgs --upgrade吧?
sogw
sogw 2019-07-30
AdamShen 发表于 2019-7-30 14:58
命令是pkgs --upgrade吧?


是的, 感谢提醒, 我改下
sogw
sogw 2019-08-01
rt-robot 软件包有更新,接口变动。更新“组装”过程
ysj
ysj 2019-09-13
求助,为啥我电机模型一直失败啊
sogw
sogw 2019-10-09
依最新的rt-robot更新“组装”过程

[md]```c
#include
#include
#include
#include
#include
#include
#include

#define DBG_SECTION_NAME "car"
#define DBG_LEVEL DBG_LOG
#include

// 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);
}

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, 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);

// 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);

// 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);

// 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);
}
}
```[/md]
Carry
Carry 2019-10-24
sogw 发表于 2019-10-9 20:38
依最新的rt-robot更新“组装”过程

[md]```c


请问这kinematics_get_velocity函数是不是预留的?demo上没用到过
sogw
sogw 2019-10-29
Carry 发表于 2019-10-24 20:56
请问这kinematics_get_velocity函数是不是预留的?demo上没用到过


是不是预留 《==》 有没有被调用到? 用SourceInsight 或其它工具看看就知道了
Carry
Carry 2019-10-30
sogw 发表于 2019-10-29 22:09
是不是预留 《==》 有没有被调用到? 用SourceInsight 或其它工具看看就知道了 ...


看过了,没有。找你确认一下。是不是预留来做里程计的?

撰写答案

请登录后再发布答案,点击登录

发布
问题

分享
好友