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

2019-7-24 11:30 [复制链接] 9 622

本帖最后由 sogw 于 2019-8-10 08:54 编辑
) `2 V& Q. _  x5 a/ c4 e4 b

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

1 e* H* K$ {* o6 h$ Y

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

" r, Q# c2 }& A! G$ k! Y
    ' U! p% Q' b& i# `  G9 @
  1. 0 d% W" T5 D* D1 L  v, l) H6 X; _

    在工程目录打开 env 工具,使用 ”pkgs --upgrade“ 更新软件包索引(ps:此只需要进行一次)

    : r# k2 O: K* A9 {. g
  2. 7 Q. W: C, m9 i4 _
  3. , r+ y+ A- e2 M

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

    0 [( J& f$ w0 A  b1 ~# d! u
  4. / l0 B/ [/ Z3 v; O
  5. 若没有使能 env 的自动更新软件包功能则需要手动更新,在 env 中使用命令 “scons --update” 更新软件包(下载)
  6. + m3 X1 q" \5 |2 v; x0 J. \7 D
! S$ s" ]! B" g) |+ J

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

6 }, n$ L) k4 }5 x/ U

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

4 |- t. Y/ s; p- C; q

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

- O8 \1 e, ~" Y! y: U8 I# V' r

”组装“ car

& H( c* b  l8 U! B

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

: R: p: D) g3 \/ s6 v
    7 f- |2 V6 A4 E+ J8 P% U. X" Y0 R
  1. " S$ }1 X6 b6 |+ t) h

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

    ! a( n  W6 r0 ?* T
  2. 3 w, I' ~' E" Q2 Y
  3. . {- U0 a' O1 A7 t9 o( K

    初始化动力学模型

    & {& u7 }. W0 C
  4. , m; l( l! D3 [2 b, Y+ t+ V
  5. 初始化车底盘 (为底盘添加车轮、动力学模型)
  6. " H6 l( k% T$ T/ c4 H
7 L/ J7 l8 v0 {1 T* m1 b- y

完成后 car.c 内容如下 (当前含有遥控部分

! [2 t; T* }7 Q' J
#include <rtthread.h>2 E  O+ W" n& y  `( s" i- S
#include <chassis.h>
. U) e: o# P  K. }$ _#include <command.h>! o4 t1 r' d0 a4 G, p. M
#include <ps2.h>
5 B$ [+ u7 F& T6 S, s7 _/ m) {6 p#include <dual_pwm_motor.h>4 }) A- {% S4 N) i6 ?7 d
#include <ab_phase_encoder.h>
' _4 X$ j% k+ \, N% ]1 E#include <inc_pid_controller.h># V5 H; h6 t9 v( E0 m
+ s! S# G, ~/ |- E3 ]
#define DBG_SECTION_NAME  "car"
9 w" L& L/ G/ Z. A#define DBG_LEVEL         DBG_LOG% P! K6 M: k/ [$ \3 s
#include <rtdbg.h>
5 V! I+ V% p1 z6 x: ]8 @& J) S# L* H- O* O
// MOTOR
. I: l- W' t( {# \7 q#define LEFT_FORWARD_PWM            "pwm4"
7 B# r9 ?5 a# Q7 L8 f' {/ K: q#define LEFT_FORWARD_PWM_CHANNEL    3: h5 U9 `' c/ s9 [1 h2 C7 v
#define LEFT_BACKWARD_PWM           "pwm4"
8 f$ c+ ?# U; q5 x#define LEFT_BACKWARD_PWM_CHANNEL   44 a: Y; ~: c. }
; X  i6 G! ^* k! M( I" ?) J- s
#define RIGHT_FORWARD_PWM           "pwm2"1 \- Q9 B+ w/ ]3 s0 x
#define RIGHT_FORWARD_PWM_CHANNEL   3$ U, @# Z3 J: B1 q" a# R) h
#define RIGHT_BACKWARD_PWM          "pwm2"$ p9 t2 y8 y' b; V# ?! e  _7 V# Y
#define RIGHT_BACKWARD_PWM_CHANNEL  4/ X5 _, s: l# G1 C9 c) `
+ N( @* T- R4 e. N' a3 J
// ENCODER& k$ p4 ?2 |1 [* \: e$ i
#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)
% V4 U- k6 d$ h: R, k- _#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)
! t) G/ C2 W; {1 ~; G. y+ u#define RIGHT_ENCODER_A_PHASE_PIN   36      //
, L9 j" j" _8 z0 y* c# R#define RIGHT_ENCODER_B_PHASE_PIN   8       //0 @# r3 ^! t, z" ]9 w9 D/ B+ w
#define PULSE_PER_REVOL           2000      // Real value 2000# v4 R* b. u3 q! l, h" B
#define SAMPLE_TIME                 50
. ^, U+ D9 C4 J) V( `1 Y! b2 H
$ X* x; C# B7 s6 C// CONTROLLER PID
# v3 ]2 E  k4 e# v7 ~# O8 u) i#define PID_SAMPLE_TIME             50
* h4 U% b3 C2 {1 W4 B& P: r#define PID_PARAM_KP                6.6) c0 p0 t. [  s( Q4 L. _7 h- [; [6 Y
#define PID_PARAM_KI                6.5
: U0 }: c1 G! j* H2 i( v/ ~#define PID_PARAM_KD                7.6
' a  J+ m* B- S# ]5 C' x8 f+ I% W6 O" |
// WHEEL
4 O* v9 ^1 n8 e4 x* c7 g, l#define WHEEL_RADIUS             0.066
4 `& N9 C. g- [; c" T#define GEAR_RATIO                   14 z+ M0 d* P0 A) e
9 B8 |, z  x% M: z: N) I( O
// CAR
) E- `' ]- f* g; echassis_t chas;
! P6 \& `& d( s* Y0 l# E# w$ `5 h1 e
#define WHEEL_DIST_X                 0
" o, p8 G+ o. X4 M: w6 j" d#define WHEEL_DIST_Y              0.13+ z* a5 ~2 O# D4 k( b2 M6 n

5 U9 \6 a5 A* W1 S: Z# M// Car Thread( V* ]% O, ?/ d# @7 L
#define THREAD_PRIORITY             106 j3 P9 b! i9 b+ }5 o1 ^; r
#define THREAD_STACK_SIZE          5120 w# D8 @+ W. o2 A* e# F1 Y
#define THREAD_TIMESLICE             5
% N4 V. |5 ?  U5 g9 w& ~+ L7 Q. v) c8 F
static rt_err_t car_forward(rt_int8_t cmd, void *param);& V3 |0 z/ ?" L' J# {8 O3 ~
static rt_err_t car_backward(rt_int8_t cmd, void *param);
+ H( z2 ]0 {  r; b1 u+ q- Pstatic rt_err_t car_turnleft(rt_int8_t cmd, void *param);
9 G8 S7 Y( r" c; ~7 ystatic rt_err_t car_turnright(rt_int8_t cmd, void *param);/ G& n+ ~' Y' u9 x8 z
static rt_err_t car_stop(rt_int8_t cmd, void *param);
$ M7 y& _8 d" I* J2 V" o5 g7 v3 F: }3 r
static rt_thread_t tid_car = RT_NULL;
' ]  ^% K5 y0 ?* v' W. y, x8 u9 V  m; Z0 f6 c' B
void car_thread(void *param)
1 a; Y& e6 k0 ~( }{
* D- E, a0 T2 |) r    // TODO
1 `% e+ D6 m7 \  q9 e  A, B
, ^. J$ y, Z$ |& e: A4 F    struct velocity target_velocity;% N2 R  M9 n; Z0 {; w2 T7 s% U: W# s
* K% J5 N$ A3 k8 m* s9 e1 D
    target_velocity.linear_x = 0.00f;% g4 s, k& x' I; R: r' _! ]% Q
    target_velocity.linear_y = 0;4 ^3 R1 J3 |% d3 z- J
    target_velocity.angular_z = 0;0 g+ l+ L2 h' o3 a, g
    chassis_set_velocity(chas, target_velocity);
0 K1 i" I. ?% q1 e- S: G$ {( w% F; Y) e6 C( K( y+ j, `
    while (1)( y; x, S+ Z: P0 ]: B6 z
    {% z, @2 V9 u( y7 X! i/ O( W
        rt_thread_mdelay(50);8 [3 w, a* i# v: I% b
        chassis_update(chas);' Y! A$ E' Y5 f3 j4 N9 g& t
    }! Z6 h: e! s$ {8 Z1 i

# T2 ]1 }- M( r9 ?# s5 m# o: |2 ]}; V7 H+ N! b6 x  ~6 S

3 q0 W3 Q  v% {8 jvoid car_init(void *parameter)
* a: ?1 `$ t0 a# r: t+ r{
; @$ \) k" v% C1 l5 T& F    // 1. Initialize two wheels - left and right) s- d3 n1 q0 Y5 z3 h, |
    wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);; Z$ N- a0 L0 b  g6 J/ n8 P
    if (c_wheels == RT_NULL)
! w8 V5 B# B  f7 {3 A    {
) y/ X6 d% \9 q0 P9 W        LOG_D("Failed to malloc memory for wheels");
- Z2 V$ E, p. y  v* _* ~    }
$ Z  `# p& g8 L  E; ~" s' U" N5 f( O5 z2 O$ i4 |3 c; r* Z
    // 1.1 Create two motors  _5 O; }1 e+ L+ R1 D% ]5 f
    dual_pwm_motor_t left_motor   = dual_pwm_motor_create(LEFT_FORWARD_PWM, LEFT_FORWARD_PWM_CHANNEL, LEFT_BACKWARD_PWM, LEFT_BACKWARD_PWM_CHANNEL);0 r& }9 s7 v2 \" @6 O5 C0 q. X
    dual_pwm_motor_t right_motor  = dual_pwm_motor_create(RIGHT_FORWARD_PWM, RIGHT_FORWARD_PWM_CHANNEL, RIGHT_BACKWARD_PWM, RIGHT_BACKWARD_PWM_CHANNEL);
4 t; t/ B& y  D* D$ l7 N0 t
3 N7 i! ~2 i$ P4 T5 ?5 N: z6 @    // 1.2 Create two encoders# e9 o& E! ~- t& J6 @
    ab_phase_encoder_t left_encoder  = ab_phase_encoder_create(LEFT_ENCODER_A_PHASE_PIN, LEFT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL);
8 R% T( v/ J# l; \9 s3 o+ l# d    ab_phase_encoder_t right_encoder = ab_phase_encoder_create(RIGHT_ENCODER_A_PHASE_PIN, RIGHT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL);; C" E4 X" V5 u& m
3 J0 b+ }# i& \- E% I# ?1 g# N
    // 1.3 Create two pid contollers
  ?+ g0 W4 r. K& M' w    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD);. k8 S6 l& ]+ ?/ V% h$ b
    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD);  P+ p( u" i) f7 J3 Z9 X" J2 n0 h

) Z8 ^0 x0 b) W* ~4 V$ v    // 1.4 Add two wheels* S& p! F) b' d" _0 N$ r! h; [
    c_wheels[0] = wheel_create((motor_t)left_motor,  (encoder_t)left_encoder,  (controller_t)left_pid,  WHEEL_RADIUS, GEAR_RATIO);8 n/ h2 E9 ^% G
    c_wheels[1] = wheel_create((motor_t)right_motor, (encoder_t)right_encoder, (controller_t)right_pid, WHEEL_RADIUS, GEAR_RATIO);
* {( F! Y9 W1 b2 N4 n" ]
- i/ ^: L7 v2 u    // 2. Iinialize Kinematics - Two Wheel Differential Drive
: P0 h; I2 Q$ [4 S, S( u( K1 f    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);8 Z+ h7 ]1 r& b6 W  P
( P. {' _) r6 U$ l
    // 3. Initialize Chassis
2 ]/ K1 {0 C9 z, G8 U7 l    chas = chassis_create(c_wheels, c_kinematics);
: d& }/ f. x. g+ W0 a4 @- B5 e% J. _! p  q1 J
    // Set Sample time/ }! e3 u- H# [9 D( s$ a" _0 k4 q
    encoder_set_sample_time(chas->c_wheels[0]->w_encoder, SAMPLE_TIME);
+ C" g- i2 h8 p9 ]5 B! i- H) T    encoder_set_sample_time(chas->c_wheels[1]->w_encoder, SAMPLE_TIME);
* k; S" m; B. M; p! j* W    controller_set_sample_time(chas->c_wheels[0]->w_controller, PID_SAMPLE_TIME);) ?  n2 Q7 V% v4 |4 V
    controller_set_sample_time(chas->c_wheels[1]->w_controller, PID_SAMPLE_TIME);
+ w$ c7 q' q0 [
' g* N4 [: r1 p9 b8 i$ N  Y8 C4 y    // 4. Enable Chassis' |. z8 K3 S& k. H( u, r
    chassis_enable(chas);% _) Y% b* O! R* E

* J+ Z7 R4 y/ g* G) U" ^3 }; n% b    // Register command
0 F& J( W6 ?% G' K( j0 ?9 ]% z5 Y4 J    command_register(COMMAND_CAR_STOP     , car_stop);* ^6 h$ d1 R3 g" S
    command_register(COMMAND_CAR_FORWARD  , car_forward);
; g5 f' o" \# b- ~    command_register(COMMAND_CAR_BACKWARD , car_backward);
5 d5 }' K! v! I' R- F/ v    command_register(COMMAND_CAR_TURNLEFT , car_turnleft);
$ F5 E2 ^- R" D2 Z' _# i    command_register(COMMAND_CAR_TURNRIGHT, car_turnright);
0 T7 H( u# k0 y- n# a2 R3 i1 q# n9 o
    rt_kprintf("car command register complete\n");
8 ]: f+ ?" _+ s! @: e
& ?9 ^- y9 q" p: P" E3 Y+ a9 ~    // Controller
# d4 Z5 w( A/ ]. k0 I0 a2 u  a    ps2_init(28, 29, 62, 61);% E7 h0 N5 S9 E4 M3 t7 c
) f: `' h! ?0 r$ b0 [
    // thread
% f6 n- [' `6 x; E* n3 e% T4 w' j9 x    tid_car = rt_thread_create("tcar",
) d1 E$ F0 w% O/ t8 Q& a                              car_thread, RT_NULL,2 Y, M, T# f3 H7 v% y
                              THREAD_STACK_SIZE,1 p, i% T8 Q2 y$ i; ~+ b2 h3 L
                              THREAD_PRIORITY, THREAD_TIMESLICE);0 e0 ?' V6 F  K' H  e
& h5 G. [/ J2 L+ n
    if (tid_car != RT_NULL)
/ A) C" m  m6 k    {$ x3 T( _- d' N! m4 A
        rt_thread_startup(tid_car);8 e) T! l4 D( g- y  B  G
    }
& ~' {8 y. N. |1 X* E}
9 X$ {8 N) B' c8 s$ M+ F4 y# U5 {" R+ ^% x  v+ V: C' h( U
static rt_err_t car_forward(rt_int8_t cmd, void *param)3 D9 h5 m/ B, A4 `2 I2 E2 r3 K5 m3 Y7 x# l" n
{7 P! \' S2 p* L, O! Y. n' P
    struct velocity target_velocity;
7 u0 }" f) Z7 r/ _
: O, K; i+ M9 N; y2 C    target_velocity.linear_x = 0.05f;5 Z5 T$ X& }. J3 J5 ~: ^% a
    target_velocity.linear_y = 0;9 O4 p3 ^1 {( y8 h7 n. p; \' k
    target_velocity.angular_z = 0;
7 F8 `& `5 h# n0 ]* Z3 l$ _/ a    chassis_set_velocity(chas, target_velocity);
1 M$ C5 T. W- M- q/ K7 Q+ X. z
    LOG_D("forward cmd");
% D5 x' R& O. r. C. \  V% x& a+ L' v- ^& w
    return RT_EOK;
3 U9 E) P% p6 s& ]8 e+ a$ Q+ `}
7 J: L7 o, f5 S2 m4 T; {4 l& l
% l4 B4 Q6 C% @" ?static rt_err_t car_backward(rt_int8_t cmd, void *param)
+ U; T8 i2 ?0 Y/ W* ^{
  C! n$ i1 o2 v    struct velocity target_velocity;% f+ k$ @: S% N' ]( F
3 T2 M' }, q0 [* W
    target_velocity.linear_x = -0.05f;
6 w" u. `2 k) J! r    target_velocity.linear_y = 0;0 V! @1 G+ [/ Z- U  d
    target_velocity.angular_z = 0;# N9 s) T$ C8 H5 l5 v$ {7 `
    chassis_set_velocity(chas, target_velocity);
! a* O1 i1 k* v' a% y9 n
# ]0 a' {5 k% j7 Z3 [    LOG_D("backward cmd");  Q4 O' G0 e6 \; }6 [$ G" L
, O/ a( s: }* e$ z
    return RT_EOK;7 f+ G! M4 h, s. V2 Y. _1 S
}6 b9 J" C# ?! E5 |5 t; l& h% c
. G% n  ]0 D9 A9 \2 o2 ?/ i- o
static rt_err_t car_turnleft(rt_int8_t cmd, void *param), j+ A8 ~  @" o) a! g( L  @5 e% X
{, E: v- ~( ~$ V$ n
    struct velocity target_velocity;3 N1 F3 ~# I+ Q6 R, R! I1 i9 k/ h
7 W$ z1 T; M) k
    target_velocity.linear_x = 0.00f;7 `* |) A% s+ e- w4 b; D. O% a) _/ t
    target_velocity.linear_y = 0;
! U7 y4 }' y3 B! a+ _    target_velocity.angular_z = 0.5;
0 b$ u4 H" s* j  s& r    chassis_set_velocity(chas, target_velocity);; t8 K' X; \% o) u6 {* z

& Y" l6 d4 S2 m    LOG_D("turnleft cmd");$ ~% h+ h/ q& h3 C0 p

. v/ @: {7 Q+ Q7 `/ }: y1 F& M    return RT_EOK;
9 c" C$ T2 B; e}' t- B1 M  v, ]; A/ I
( D" ]2 p9 w# \7 |
static rt_err_t car_turnright(rt_int8_t cmd, void *param)
5 M, d; e2 X8 \, j# C{
5 y  C* K- M- I' Q% K  \    struct velocity target_velocity;
: R" k/ L2 ~* ?2 \" M# c
& ^& H  s: X- a8 E' z  y    target_velocity.linear_x = 0.00f;0 \- e. D6 O: Y( `) O4 o; Q
    target_velocity.linear_y = 0;$ O8 s9 x% Z  o; A+ X
    target_velocity.angular_z = -0.5;
2 Y/ ?1 T( F: A4 \5 ~    chassis_set_velocity(chas, target_velocity);
( x# ?( t3 e7 j: J9 a" r6 Y, L+ |! [$ s3 T, K: o$ I
    LOG_D("turnright cmd");3 g% N0 n1 Z' s5 w2 S) I. ]

9 g3 G# V$ q7 S& x; Z5 c    return RT_EOK;
' X* [7 a: j8 r* s2 Y) |}8 X- Z3 ~) F+ Y1 y/ t
; D9 a5 k6 M' F' @3 X
static rt_err_t car_stop(rt_int8_t cmd, void *param)
7 A2 l; w" K& `0 l* _" N8 h+ R$ f{
" s& v/ T8 {0 r  K9 Q    struct velocity target_velocity;
, C3 v/ Z) Q; y% Y- s
- u1 X5 Y4 Z6 r. V: ^. U( Y2 w    target_velocity.linear_x = 0.00f;! W# _3 F5 t3 N7 B& P9 s
    target_velocity.linear_y = 0;8 _6 ?( k0 s5 u( S- l% h
    target_velocity.angular_z = 0;9 W: U' Q( j/ `6 D9 v/ d( s; X: h# j: H+ b4 o
    chassis_set_velocity(chas, target_velocity);. p9 o$ A% ~/ L: `& q9 \
; P4 A& z, M* A1 U2 u& J: I; X
    LOG_D("stop cmd");
/ ?% P0 B+ n8 I* {. P; }# ~7 m  ^0 k2 c7 l- w
    return RT_EOK;
2 E7 ?% b6 X+ n: ], Z}
9 b) ~" k+ y) X, a$ r; A

说明:

3 t) i1 A% V1 A6 d0 T; W$ h
    5 ]* R8 w5 v5 E# ]9 k' x  W" F* }
  • PULSE_PER_REVOL 车轮转一圈输出的脉冲数
  • - ^9 I9 h1 k7 r0 P5 v3 q
  • SAMPLE_TIME 编码器采样时间,现单位为 ms
  • / I0 m0 Z9 f/ b* b4 p2 Q/ h! [
  • PID_SAMPLE_TIME PID计算周期,现单位为 ms
  • 4 U6 k7 _0 y1 j2 d6 t
  • WHEEL_RADIUS 车轮半径, 标准单位 m
  • 1 e5 u/ n6 I- k0 {+ Z
  • GEAR_RADIUS 电机减速比,无量纲
  • 2 Y4 Q: Q& y# `
  • WHEEL_DIST_X X轴方向两轮之间的间距,标准单位 m
  • ' X6 M. |$ g$ w5 n/ F  J- v
  • WHEEL_DIST_Y Y轴方向两轮之间的间距,标准单位 m
  • 0 Z2 `' r. I3 D- h) @: J# D. r
; G+ X+ j6 Z& g$ _5 r2 \

_Command register 和 ps2_init 与 "组装" car 无关,无需关注_

% S! s9 D- N# ^! ~! `$ q4 Z2 u

注意事项

. `" {: F2 l# ]! Y2 a" ~
    6 K4 r* \0 X+ L/ f  @4 D8 [) V/ m$ F
  • 关注 rt-robots 软件包文档, 注意小车坐标系等
  •   [3 C/ m0 J3 F' n
7 a" o5 ~, U# v+ D, j( q) c" ~[/md]
使用道具 举报 显示全部楼层 回复
最新评论 | 正序浏览
显示全部楼层 |楼层直达:
发表于 2019-7-24 11:45:04 | 显示全部楼层
那么强大吗?自带两轮差速驱动模型啊
使用道具 举报 回复
发表于 2019-7-25 09:21:07 | 显示全部楼层
厉害厉害,正疑问着rt-robots 软件包应该怎样用呢。感谢楼主分享。
使用道具 举报 回复
发表于 2019-7-25 12:57:21 | 显示全部楼层
moss 发表于 2019-7-24 11:452 e6 F) n0 x0 S# b8 v
那么强大吗?自带两轮差速驱动模型啊
3 P6 p! [3 O( x( K% V
对的, 还有四轮等的动力学模型
使用道具 举报 回复
发表于 2019-7-30 14:58:41 | 显示全部楼层
命令是pkgs --upgrade吧?
使用道具 举报 回复
发表于 2019-7-30 18:37:39 | 显示全部楼层
AdamShen 发表于 2019-7-30 14:58( W) c2 _3 M' A) J0 K/ H8 F
命令是pkgs --upgrade吧?

. ?! k& b* P  j# C: x: n是的, 感谢提醒, 我改下
使用道具 举报 回复
发表于 2019-8-1 13:56:08 | 显示全部楼层
rt-robot 软件包有更新,接口变动。更新“组装”过程
使用道具 举报 回复
发表于 2019-9-13 10:23:03 来自手机 | 显示全部楼层
求助,为啥我电机模型一直失败啊
使用道具 举报 回复
发表于 2019-10-9 20:38:58 | 显示全部楼层
依最新的rt-robot更新“组装”过程; `/ ~6 D8 a: y! d
, S/ _( P& j0 M2 x8 }1 h2 Y& M
#include <rtthread.h>9 _5 V' j7 m6 @* x" |" H+ P6 B" E
#include <chassis.h>: u0 e/ G9 g% ]5 @% n9 X4 u: Z
#include <command.h>9 s) v3 v7 k1 b) ^7 c
#include <ps2.h>
" T6 B7 z" K- d#include <dual_pwm_motor.h>  `: d! o/ t) K3 c# L+ \( o
#include <ab_phase_encoder.h>/ @7 S& z/ n* f' B% I$ K
#include <inc_pid_controller.h>
9 y( o9 q; T. t1 d
0 h- h$ r* W3 A7 P#define DBG_SECTION_NAME  "car"* ]. p* w: G# ^! f3 |9 Q5 q# e
#define DBG_LEVEL         DBG_LOG
- L1 ^- c$ K$ p/ T; J: `& _4 e#include <rtdbg.h>* E! g( a/ h  h$ L8 E* ]" F$ |

3 x& q5 z/ q, Q5 |/ ]+ u1 b, g- `+ j// MOTOR9 }2 l" Q/ {# I2 }
#define LEFT_FORWARD_PWM            "pwm4"
, W$ I* e1 C/ c8 v. o#define LEFT_FORWARD_PWM_CHANNEL    3( o2 I' `$ M  m- [
#define LEFT_BACKWARD_PWM           "pwm4"
  W# @4 f% D, }! N#define LEFT_BACKWARD_PWM_CHANNEL   4
& X' ]8 |, R# I3 D6 O( v* U4 r# f. U# D
#define RIGHT_FORWARD_PWM           "pwm2"
0 e$ Y9 X! `0 V; i7 t: g* B#define RIGHT_FORWARD_PWM_CHANNEL   3' T  J+ C* w! W# c% l/ \3 p
#define RIGHT_BACKWARD_PWM          "pwm2"
! Q! x- p2 v6 w" L  ^#define RIGHT_BACKWARD_PWM_CHANNEL  4
5 p& b1 l' R4 f) ~2 I: Q3 y6 t/ U" z0 y$ [/ @$ C. W6 Z$ ?3 @8 F2 `
// ENCODER
) q% p2 h0 l; U6 x; Q8 r  r#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)! a- K0 j8 w" p2 F
#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)
  q. V5 ]. z; K3 F9 d$ q% D6 N/ N# H% U#define RIGHT_ENCODER_A_PHASE_PIN   38      // GET_PIN(C, 6)
" s' T# T6 y5 P1 I# {' S#define RIGHT_ENCODER_B_PHASE_PIN   39      // GET_PIN(C, 7). b" _( Z+ B( {8 G. ]# g/ a5 f
#define PULSE_PER_REVOL           2000      // Real value 2000: a2 g6 |, E' q, \3 M
#define ENCODER_SAMPLE_TIME         50
: `1 V& c. w# ~; ^( Q( W: v1 [- v3 P8 B* s6 U
// PID CONTROLLER7 P' i; O& m) B
#define PID_SAMPLE_TIME             505 y( r! F. Z: U
#define PID_PARAM_KP                6.66 Q1 y- y/ T$ }* a+ s7 W( s
#define PID_PARAM_KI                6.5) X) w$ X/ l3 z0 T: B; B( c
#define PID_PARAM_KD                7.60 J5 L9 ^: h' G

% v3 E6 T6 C3 U! L  t5 e3 I// WHEEL
2 s( G  Q% W; R2 x8 F! F#define WHEEL_RADIUS             0.0661 x% [1 q( }  H, i" L0 P
#define GEAR_RATIO                   1
# l, w/ I% l' |5 E) i5 p5 {) N, _4 j
// CAR- ]; J, y! O7 Y
chassis_t chas;
3 q5 A4 Y$ M  B$ s* K) P4 W$ o+ A) y$ C# v4 @/ V
#define WHEEL_DIST_X                 0
$ f  Y6 R* j  c, U  A" `. `#define WHEEL_DIST_Y              0.13! M% s! r, N8 B% f, r

0 r3 W; Q2 A. ^0 _" \8 }// PS2" w3 N! H+ w' r: a# W. K6 ]
#define PS2_CS_PIN                  28
! N# h! k  b, b#define PS2_CLK_PIN                 297 W1 e) ]0 v0 P- q/ {+ E
#define PS2_DO_PIN                   44 I" Y2 y3 h  ^, v+ j3 e
#define PS2_DI_PIN                  36
! x  |# J, j" B/ C+ Q
( w" W  ^: p& H) o// Car Thread
. P% C# [( X+ O1 C! _2 E#define THREAD_PRIORITY             10" d- O1 k5 n/ B7 u" ^4 A/ r
#define THREAD_STACK_SIZE          512
8 H' F& ?7 _) f+ q#define THREAD_TIMESLICE             5
" o8 m1 G+ A" Q2 d. D: h$ D' C$ ?8 ]# `
static rt_thread_t tid_car = RT_NULL;, u5 o) H) g/ j. r( X! ~/ w

1 o' l8 Q9 [4 M  ~0 A# r7 Svoid car_thread(void *param)
9 N; `1 g5 A0 H& T# }{6 D5 W# a! A; ]7 N& _+ ?
    // TODO/ F0 `( t4 A6 m" P- ~

# B/ V4 g- I! Z* P/ S8 M    struct velocity target_velocity;' B7 u, |2 y) n* Y

+ c- \& J3 x% i: e' s    target_velocity.linear_x = 0.00f;6 }+ h( Q8 V0 z+ K: L
    target_velocity.linear_y = 0;
. m( m$ Y2 S/ d) S& {    target_velocity.angular_z = 0;  t4 ]3 {+ k" m5 ]- F
    chassis_set_velocity(chas, target_velocity);
7 f# Z: `# _% K& n7 T5 w, T
1 D- H. Z, ~9 h. }    // Open-loop control3 W, o" d, z  `1 E3 _9 |
    // controller_disable(chas->c_wheels[0]->w_controller);+ X3 b" g# k- C/ ]6 L% g3 u) y
    // controller_disable(chas->c_wheels[1]->w_controller);
# r5 y' \8 e; {0 q
; j# J9 i; `. Z6 s3 U" A; x8 A) y- t    while (1)
7 ^( K$ h( {9 \2 I, a# O8 y2 q    {# _: |/ C: [. T
        rt_thread_mdelay(ENCODER_SAMPLE_TIME);
& Q- B6 R3 a2 s2 ?( {8 s4 z        chassis_update(chas);
. G" n/ ~6 }2 n$ q    }
8 F3 O# k  {4 q5 [' s/ e) a) F9 S. l4 x( `) L1 `
//    chassis_destroy(chas);
. Q, @3 O& V( |6 {$ B; u4 R+ Q}
# p7 r6 d' G3 V; Z% M
2 x9 N% R0 R) N% F- u; cvoid car_init(void *parameter)
  F/ W( D9 C! c# f9 P  B{
. T! z; H, ]- C3 C0 s' U    // 1. Initialize two wheels - left and right: b2 c: X+ e/ |* C
    wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);' S1 ^/ I/ |  [9 o
    if (c_wheels == RT_NULL)
# m; m2 z! q3 m/ c8 p. e7 @/ a    {6 H: o, t+ I2 |$ ?
        LOG_D("Failed to malloc memory for wheels");
. D' J0 V9 Z& _2 l* Z, z    }
0 }# M; x' Z5 w# s8 w) m' O  ?  ?. U# {; E# ^
    // 1.1 Create two motors
# V& e( S( D, U& }$ Q2 D8 I    dual_pwm_motor_t left_motor   = dual_pwm_motor_create(LEFT_FORWARD_PWM, LEFT_FORWARD_PWM_CHANNEL, LEFT_BACKWARD_PWM, LEFT_BACKWARD_PWM_CHANNEL);) @! N3 i7 q1 g% U: l- s5 v
    dual_pwm_motor_t right_motor  = dual_pwm_motor_create(RIGHT_FORWARD_PWM, RIGHT_FORWARD_PWM_CHANNEL, RIGHT_BACKWARD_PWM, RIGHT_BACKWARD_PWM_CHANNEL);/ i9 c) H3 }& s3 p
: J- H' A1 }8 D% M  }6 d8 l
    // 1.2 Create two encoders' u" C7 t/ c8 g
    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);! F+ j+ J0 D" p7 }
    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);
+ P2 u/ m6 f& A' r8 |) O& D/ W4 O9 t. {* x6 A/ l" x5 C2 d( k
    // 1.3 Create two pid contollers; B* k2 l* {4 W( a
    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);
! w5 m0 a  f% _" z. U; `9 Q* `. B2 o    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);
; W$ ^$ h( E! ?, A1 w; H8 C* n) a6 m# m
    // 1.4 Add two wheels# X" P1 r5 B+ S& n
    c_wheels[0] = wheel_create((motor_t)left_motor,  (encoder_t)left_encoder,  (controller_t)left_pid,  WHEEL_RADIUS, GEAR_RATIO);
& S2 H6 L! B) c    c_wheels[1] = wheel_create((motor_t)right_motor, (encoder_t)right_encoder, (controller_t)right_pid, WHEEL_RADIUS, GEAR_RATIO);
, K1 O/ M1 q0 |4 A/ [- m" _% a
& \9 D, H' q# M4 G2 S! E    // 2. Iinialize Kinematics - Two Wheel Differential Drive( }6 S" ^4 h, B$ Z0 @+ L
    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);
& m# \- g: Y$ M' l& i
2 m; N" o# n1 E$ C    // 3. Initialize Chassis2 u* i& M9 h$ n+ O$ u1 P$ C) c
    chas = chassis_create(c_wheels, c_kinematics);8 B! L- t0 j- g3 A/ {7 Y* v- \

2 R/ t! R9 ^( h+ N  Z8 ?# G0 R    // 4. Enable Chassis
" T  @3 r0 X4 \5 f% Z( v9 U* H% h    chassis_enable(chas);
; A1 \) O  S) a% l' }  q
$ Z: @' J' q$ p- M    // Remote-control
# B; w( ^/ S2 Q% E    command_init(chas);
; w$ [& \) T' ?5 c$ |& n6 o. g
$ b* Z) K0 p# W# W, x( S' A' D    ps2_init(PS2_CS_PIN, PS2_CLK_PIN, PS2_DO_PIN, PS2_DI_PIN);/ R6 a9 W% J$ f2 K3 x
3 X9 ~5 w- U" O0 e5 `  ~
    // thread
* a  F' y) R  a, C    tid_car = rt_thread_create("tcar",9 z. {. z' q6 Z1 F  ?/ i
                              car_thread, RT_NULL,
8 Z1 E/ h6 A) x- [                              THREAD_STACK_SIZE,
4 Y) e! V  \% k: o& i: v  Z                              THREAD_PRIORITY, THREAD_TIMESLICE);
1 U9 g1 l  z& Y" ?/ t9 Y  |
, Y9 V+ f7 t9 i4 t7 ?    if (tid_car != RT_NULL)
, Y, n4 q% |  i- y! ?5 P% G( E    {
! V) {; i, C8 E# p- N  }( ]        rt_thread_startup(tid_car);9 ~# b" E5 W) t
    }
" S/ a/ _- p5 h+ ]9 _}
: E: g& Y- G2 Y, ~# C
使用道具 举报 回复
发表于 2019-10-9 22:56:05 | 显示全部楼层
学习了!
使用道具 举报 回复
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|RT-Thread开发者社区 ( 沪ICP备13014002号-1

有害信息举报电话:021-31165890 手机:18930558079

© 2006-2019 上海睿赛德电子科技有限公司

Powered by RT-Thread

快速回复 返回顶部 返回列表