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

2019-7-24 11:30 [复制链接] 12 902

本帖最后由 sogw 于 2019-8-10 08:54 编辑
$ L) n# L# z& p+ e4 R

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

3 X& k) X# C; _6 d- N

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

4 I$ v2 h( k0 b6 i* N; y
    + @1 Y: Y9 k0 i7 f5 j
  1. # n% Q9 G; p3 t5 b# T7 E$ d3 I9 r

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

    % Q9 F5 H; c$ r
  2. / k; Q( v) G) x
  3. - P) `3 z' N! y. t+ \- J  |

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

    / {- O) X0 V* C4 A2 k/ m' G0 l
  4. $ j$ M" [7 k* K; g
  5. 若没有使能 env 的自动更新软件包功能则需要手动更新,在 env 中使用命令 “scons --update” 更新软件包(下载)
  6. . Y8 T7 v5 |. p- D1 ?) Y
7 f! N: g4 a, `8 }/ d0 B

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

' x0 p8 g1 t3 Y" B( ]. X

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

7 i3 w# m1 n/ s5 g+ M- I9 Q

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

# m  m: b5 f# O: E" l0 O, j! P$ o

”组装“ car

3 K2 t$ X- o& _  t5 C& T% d

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

- C1 Z) p: H  U1 k
    ! F" s. a7 \  C1 i# c
  1. 1 A/ o0 b+ ~. {& W6 A$ y

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

    ; a. a) [) E' _& {5 D2 `* [! l
  2. ) q! w" \; P5 l7 D3 i7 a
  3. , U, r' H$ n: m& U( t1 H' z1 n

    初始化动力学模型

    1 U- V0 D) a  L5 Q
  4.   f; D7 `' B/ C0 x+ }6 }* c
  5. 初始化车底盘 (为底盘添加车轮、动力学模型)
  6. % l6 }- j1 B- k' n+ B3 z; V2 m
; ^% m3 T0 n+ Z* j

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

7 B- D9 ~  ]1 x- s/ U0 V) |$ C/ U( U
#include <rtthread.h>7 S0 Q( ]) G) n- g. \' O! M4 |
#include <chassis.h>; M% }9 R$ E. i2 `& E' ~( e/ K" H
#include <command.h>
4 {! K/ ^$ D6 v4 t6 f#include <ps2.h>, ?* A) ]1 I, B
#include <dual_pwm_motor.h>
' D: E0 U! o' ?9 M2 ]# v1 ~#include <ab_phase_encoder.h>4 }& @, x! i. y3 \; p
#include <inc_pid_controller.h>
+ b% t% @% L7 N! U8 P- j8 J9 W+ z' M8 C7 E, C% t8 c3 t; D
#define DBG_SECTION_NAME  "car"* g, w. t% M; f) C
#define DBG_LEVEL         DBG_LOG
1 Q6 z6 F6 T" ]4 ]7 H#include <rtdbg.h>
, b; n8 v, i8 C( l9 Z9 i3 ^+ J% s" k) A: c0 k6 A. d' p
// MOTOR/ i& [9 ~9 _% y: r( G; F. v3 b. W
#define LEFT_FORWARD_PWM            "pwm4"8 D, a( Y  f2 q( u$ R
#define LEFT_FORWARD_PWM_CHANNEL    3" R; M8 C: Q8 L7 g1 j# q( u3 ?- M# b
#define LEFT_BACKWARD_PWM           "pwm4"; ?% L9 ^- x7 ?1 C! r3 e2 \: w
#define LEFT_BACKWARD_PWM_CHANNEL   4: t% s2 C+ ~1 k
2 W" q; M) T4 `& B- p
#define RIGHT_FORWARD_PWM           "pwm2"
  l  `( Q7 M6 j) B. A3 @/ R9 t#define RIGHT_FORWARD_PWM_CHANNEL   3
" l: s; a3 T: \' o) B  A: n#define RIGHT_BACKWARD_PWM          "pwm2"8 M& r% E( |6 g6 N
#define RIGHT_BACKWARD_PWM_CHANNEL  4/ s1 w8 Z: u  k9 m  ]5 r! F* O

* Z6 R+ d' I! d0 }$ p// ENCODER* `8 r. ?6 P! Z) z/ R3 U
#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)
  y- B- y, }, @. p7 d1 g#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)
8 b- k  B# X- e! }#define RIGHT_ENCODER_A_PHASE_PIN   36      //
" y7 `5 c+ B8 X  J#define RIGHT_ENCODER_B_PHASE_PIN   8       //
6 a! W& b3 m' l5 _, A0 q#define PULSE_PER_REVOL           2000      // Real value 2000) Y" n& h* u' c
#define SAMPLE_TIME                 50
5 H. i/ M+ W& b6 K# E7 V) ]2 e( W- R! k7 U) U! m/ F0 B( `
// CONTROLLER PID
% E0 V# u# \1 ]. p$ Z) Y. d#define PID_SAMPLE_TIME             50
% {+ L, Z- _8 @( w#define PID_PARAM_KP                6.6
+ }# h7 |0 \# r; H( E( }1 Q$ Q#define PID_PARAM_KI                6.5$ c  s3 G! F' y$ n3 ~
#define PID_PARAM_KD                7.67 H; M3 z% @- [' x9 h" U/ x

3 \, c0 n( W6 O+ P# a9 g9 p- e8 ~// WHEEL* s" v, s% r+ f# S) G- M' u
#define WHEEL_RADIUS             0.0666 \1 j4 O' }$ p  [$ O
#define GEAR_RATIO                   1/ n) O- z4 ^2 V2 F& @. F& b" z1 ^
4 c1 ~) [; @1 |9 v9 T
// CAR
$ j+ p! |- z: }4 Nchassis_t chas;3 D, R4 T1 Z4 b4 X1 y
8 [# G; ]# t8 I3 ~; k. ]+ z/ ?) S6 }
#define WHEEL_DIST_X                 0
' I( N4 k: t1 \#define WHEEL_DIST_Y              0.13, d4 G7 u; R% q* i5 j
( q" q8 }  \# o  l
// Car Thread4 p, P$ \9 T" Z
#define THREAD_PRIORITY             10/ P3 E8 H' V' g
#define THREAD_STACK_SIZE          512
6 c: Y1 ~. G: ?5 V#define THREAD_TIMESLICE             54 w7 j! q( P2 [5 n1 f+ n

- [6 N3 c% o' E1 bstatic rt_err_t car_forward(rt_int8_t cmd, void *param);
5 z7 A- m+ R7 t8 C5 Z/ E2 Nstatic rt_err_t car_backward(rt_int8_t cmd, void *param);5 G  B! c- ^+ i. |4 g5 g
static rt_err_t car_turnleft(rt_int8_t cmd, void *param);% p8 }5 L, q* q0 O9 C( Z
static rt_err_t car_turnright(rt_int8_t cmd, void *param);
( f6 U" {6 H6 M9 N6 z! }3 _! V( [static rt_err_t car_stop(rt_int8_t cmd, void *param);; C! P3 C1 E0 i5 u% R+ M9 f4 N5 g

: A/ I# @; e4 C( j' R' N7 M  zstatic rt_thread_t tid_car = RT_NULL;
! a: A: H) q2 J3 ?0 k
7 D) p" ^2 a1 \, G% D- u" i" Pvoid car_thread(void *param)% A" D3 [/ S/ V& d+ r4 s
{
, Y. s4 G& n+ [7 w    // TODO
; M# ^; |. i9 e1 F8 |; O; o
( {$ k, J! h; D5 H    struct velocity target_velocity;& a, P/ F) }- ~6 M
. ^5 C* H9 t$ u! c0 |. J) E
    target_velocity.linear_x = 0.00f;
6 {+ n- b2 ^) ^    target_velocity.linear_y = 0;1 T2 }, r, }6 J
    target_velocity.angular_z = 0;
7 O' y2 [7 M- ~, C    chassis_set_velocity(chas, target_velocity);' P- L/ `( C; z4 B* o# g2 N8 B( d

( e% s1 ~; x# F5 z: F& D4 v    while (1)4 e9 e5 t7 s4 r; }
    {( C9 P5 n7 P( \$ a) t
        rt_thread_mdelay(50);
( q4 k5 W" r1 ?7 \  G( V1 o        chassis_update(chas);. i9 l' }0 n3 B2 c, k2 E
    }
4 P! B5 _3 V+ n% {6 ^. z) R/ ~* a  Z7 ]( U* {  E; E' n  A
}9 `( h) e5 M3 e3 {+ c* ]  H' z
+ J1 j0 R4 c1 t8 ^$ \
void car_init(void *parameter)
5 S4 Q9 j, b. I1 ]. c6 G( O{
0 q8 s5 J% Y( ?- F    // 1. Initialize two wheels - left and right
9 [$ f% L/ {& z5 l9 A8 O    wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);/ a/ A, O1 c9 J% c' |2 ^
    if (c_wheels == RT_NULL)% Y9 ~1 ?! [9 k# o$ r3 z
    {! S5 w4 \' o& i; L
        LOG_D("Failed to malloc memory for wheels");
# c2 N$ ^& a( [- L  g* d7 V    }
0 G3 j7 g  w' l7 x% [( s! O8 m8 \8 R* ]1 |
    // 1.1 Create two motors, y% `6 R$ ?/ N) D' Z
    dual_pwm_motor_t left_motor   = dual_pwm_motor_create(LEFT_FORWARD_PWM, LEFT_FORWARD_PWM_CHANNEL, LEFT_BACKWARD_PWM, LEFT_BACKWARD_PWM_CHANNEL);. ?- M. Q* u& [$ v' U6 ]
    dual_pwm_motor_t right_motor  = dual_pwm_motor_create(RIGHT_FORWARD_PWM, RIGHT_FORWARD_PWM_CHANNEL, RIGHT_BACKWARD_PWM, RIGHT_BACKWARD_PWM_CHANNEL);/ d$ y0 L) ]8 S8 T" W
+ o5 G1 A9 L) d
    // 1.2 Create two encoders5 K8 ^+ u$ P& f' b0 z
    ab_phase_encoder_t left_encoder  = ab_phase_encoder_create(LEFT_ENCODER_A_PHASE_PIN, LEFT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL);/ e1 _. S. V# a, {
    ab_phase_encoder_t right_encoder = ab_phase_encoder_create(RIGHT_ENCODER_A_PHASE_PIN, RIGHT_ENCODER_B_PHASE_PIN, PULSE_PER_REVOL);
: j- u1 B8 P& T) H) J* L+ m( b; d  \% F$ V; Z$ [+ j
    // 1.3 Create two pid contollers2 d. |/ Z! l; r/ h" d
    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD);
+ o# s  M, L6 x9 w4 l2 P. {) c    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD);0 t% v$ J# u! D$ t# H7 Y& n* M: A3 M

. e" v' X8 A8 U2 l4 N    // 1.4 Add two wheels/ O1 r( N4 O; q9 W( m/ u& A* H; f
    c_wheels[0] = wheel_create((motor_t)left_motor,  (encoder_t)left_encoder,  (controller_t)left_pid,  WHEEL_RADIUS, GEAR_RATIO);
7 q- d/ P  y) B2 e: O, Q5 x    c_wheels[1] = wheel_create((motor_t)right_motor, (encoder_t)right_encoder, (controller_t)right_pid, WHEEL_RADIUS, GEAR_RATIO);4 k# N" V" J9 g9 `: _0 Q8 x

6 z; n! P8 N! L4 X& h    // 2. Iinialize Kinematics - Two Wheel Differential Drive; B- B4 |7 N; C( V. P; v7 q
    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);
# J' `  I/ h0 U, T. b: K0 z$ r" J
' [: n3 H) }% N$ e8 v0 E$ H, u    // 3. Initialize Chassis+ B3 W# \2 ]9 X4 l
    chas = chassis_create(c_wheels, c_kinematics);
2 ?6 d& E+ e% y7 Q, T4 A( \4 h. q# B9 e- z2 e4 ^; n
    // Set Sample time4 |& d* ?& a/ D5 s/ s: y
    encoder_set_sample_time(chas->c_wheels[0]->w_encoder, SAMPLE_TIME);
) G9 x5 x$ o; b( U    encoder_set_sample_time(chas->c_wheels[1]->w_encoder, SAMPLE_TIME);
# n2 d! z% G( o5 _5 T    controller_set_sample_time(chas->c_wheels[0]->w_controller, PID_SAMPLE_TIME);9 G# q6 g" H, }4 ]% B" Z4 _
    controller_set_sample_time(chas->c_wheels[1]->w_controller, PID_SAMPLE_TIME);
( o$ ]8 w! P, t8 ]6 Z
$ h+ G% P, q3 S, P; F    // 4. Enable Chassis/ {8 Y: W) c! z8 O! P
    chassis_enable(chas);8 e4 W/ r% W, c( V% i; \6 [
+ g. A8 c* k' R  Y9 `% s0 I
    // Register command
5 Z' d* e! `* A) O    command_register(COMMAND_CAR_STOP     , car_stop);
1 [- \7 L* j0 ~2 ^    command_register(COMMAND_CAR_FORWARD  , car_forward);' Y) q* D+ l( A( o( i9 C0 ~
    command_register(COMMAND_CAR_BACKWARD , car_backward);
* N- `1 D# w4 x4 K' z. L4 }    command_register(COMMAND_CAR_TURNLEFT , car_turnleft);
3 Y( K$ `& v0 l. m2 a5 J: h$ v    command_register(COMMAND_CAR_TURNRIGHT, car_turnright);
% F1 Z+ H& h7 R- {3 ]" F
$ b4 \& m8 V4 f7 C3 d" i" p: _* G6 ~& Q    rt_kprintf("car command register complete\n");
8 {$ f9 p8 P; }( p; j  K( J, H! ?0 n# ?# `9 v
    // Controller9 i0 e  B! r( z  l0 [( A
    ps2_init(28, 29, 62, 61);' k. d! Z, y8 B4 a& f" H9 L$ ]) G* V. v
% W7 i& E; {6 F5 q" ?+ W: ]7 D* f
    // thread( v: M7 t- _" c) t
    tid_car = rt_thread_create("tcar",# Z- U) R' k* x( L
                              car_thread, RT_NULL,
& `4 J! r' A6 y5 Y3 m, q; }/ P4 s                              THREAD_STACK_SIZE,
0 d8 ]& C7 p6 T) S& }                              THREAD_PRIORITY, THREAD_TIMESLICE);
0 T* O* r% w7 H) h* f3 ^! z, b9 C3 B- p: i2 G% n+ I1 V
    if (tid_car != RT_NULL)- F3 P* [* c3 @5 G# ]
    {
6 i# |" |+ N% q        rt_thread_startup(tid_car);9 `: A7 N# F& R$ ~1 G
    }2 ]$ B( U1 L5 T, g1 H7 z: Q* U! J0 {
}! O8 o5 m6 Q4 V( j' N8 c
: B3 x7 e' A+ h
static rt_err_t car_forward(rt_int8_t cmd, void *param)
% k$ z6 g4 A- c" X- v1 V$ T{
. t/ d9 T; |! S+ x    struct velocity target_velocity;
3 M/ Z% k# R! f9 o) x, ]2 M. _* I6 j
    target_velocity.linear_x = 0.05f;
( d! U% ~9 _9 h% b# o* c; Q6 P    target_velocity.linear_y = 0;& J! i" \( z% A
    target_velocity.angular_z = 0;6 q4 D' @. d2 P2 u4 _1 z
    chassis_set_velocity(chas, target_velocity);/ f! Q. s, K+ a3 A: R. ~( p

! x. q# h- N" W    LOG_D("forward cmd");* S. H, @9 \% C0 t9 N6 Q3 R
0 a; V7 t8 n% L, H7 U8 y
    return RT_EOK;' W2 X% P( D0 H+ X
}
& m' S& ^8 @, o6 R1 Y, B4 q" W* k; ^# I0 f, ^7 p, {6 f' u
static rt_err_t car_backward(rt_int8_t cmd, void *param)
0 C1 k$ d3 I' r; M0 q$ _{* p9 e1 {% B3 X
    struct velocity target_velocity;2 i" _/ P( g% A! w
6 N/ _% \. U! r! @8 ^8 e
    target_velocity.linear_x = -0.05f;& c/ X: S$ ^! C# J1 S' y& Z; Z
    target_velocity.linear_y = 0;
4 m- K& n8 t4 v% u* \    target_velocity.angular_z = 0;
1 G# i) K  i6 Y/ `    chassis_set_velocity(chas, target_velocity);5 P7 U# e. t! b* C
1 @7 d  x% U4 S- O' D2 E
    LOG_D("backward cmd");
+ I( J# t! D6 E3 W/ m9 i# Q4 q4 U4 |
    return RT_EOK;
( H4 \/ k2 A! H! F4 Q- l( ]}
  y' l$ V/ b, w) z; a
% l+ G2 h4 [8 _7 p& Ystatic rt_err_t car_turnleft(rt_int8_t cmd, void *param)* ?3 t7 E& k. c
{; |( E: j; I* C' _% N
    struct velocity target_velocity;1 r* h( Z' [# e$ S, t) E

2 X2 T/ s- W6 Q. B* l    target_velocity.linear_x = 0.00f;
, z; X. P$ O: g, }8 c# [: l9 ?5 a$ C4 ]    target_velocity.linear_y = 0;: C# ?) j5 A8 t7 w* v
    target_velocity.angular_z = 0.5;/ A- x6 w9 A& T% v3 C2 W, O
    chassis_set_velocity(chas, target_velocity);- ?- b% k0 t3 ~$ U+ t6 r8 ^9 M

. T. V3 C: s0 |1 \    LOG_D("turnleft cmd");
, X+ N8 ~8 u7 x! x/ w$ ^6 M+ n, \# r' `, I0 W3 V
    return RT_EOK;
; [) p& k/ z: K! ?}
, M5 y1 T1 j" c( R; l7 l: v$ `. _
6 j$ }! I( p& M" v; s/ n+ ?7 rstatic rt_err_t car_turnright(rt_int8_t cmd, void *param)
. O. ]2 v: T4 K# D' |7 I{
8 ?6 v6 D7 F: ]% f3 x% U! g    struct velocity target_velocity;4 d+ s+ Q, e' q5 C; t  U: \! h, j
" ]' l2 R. X8 Y2 i9 I
    target_velocity.linear_x = 0.00f;# ]$ z. O- ?* ^0 |  I: h# x5 p: ]2 F
    target_velocity.linear_y = 0;
, X5 f1 w3 H: M3 u% x8 Q" P% c    target_velocity.angular_z = -0.5;
) r, T6 u9 ]+ f" U+ |7 C- [" c    chassis_set_velocity(chas, target_velocity);
6 ?0 S4 Q- }9 t& v# b( K9 b4 L
# y. _1 [7 B! W. F5 w* q" x' [* \    LOG_D("turnright cmd");3 z) ]$ j! [; j" E: Y

% S" \5 N6 [( H9 _* w    return RT_EOK;$ s2 d; d0 d; U9 e, }) ^- A
}
  P; M5 I5 n2 i! m) t
& g4 W1 W* g/ T) kstatic rt_err_t car_stop(rt_int8_t cmd, void *param)
, ~& d8 g+ _" Z' _1 U{7 V6 q5 M. c5 N0 I5 B7 B
    struct velocity target_velocity;
9 r& [; o! o- b& G& Y' S' @! }; {8 H/ `) @/ B2 @0 U6 G  V
    target_velocity.linear_x = 0.00f;
8 J3 x( L9 B) T# B6 z* @% F    target_velocity.linear_y = 0;
& ?2 ^6 H( a+ ^7 x1 L    target_velocity.angular_z = 0;
' H; `' e- g9 X' h    chassis_set_velocity(chas, target_velocity);- G, J0 K. G4 P" m8 ?

( F$ q/ \7 H/ E! a7 H8 Y    LOG_D("stop cmd");
) T! x0 ~7 g4 j( U  r  T( f8 R3 I( B& C. R) Q- |
    return RT_EOK;: J, q. `# W' e; [* h
}
4 ^$ o/ \5 }, T" ?! F" |

说明:

3 Q, S' L# u9 d8 C9 n: w
    9 W7 P) f3 w! _1 @% M6 e
  • PULSE_PER_REVOL 车轮转一圈输出的脉冲数
  • 0 v1 H& H% q( d8 _& j! c, `
  • SAMPLE_TIME 编码器采样时间,现单位为 ms
  • 4 Y/ s! H2 W7 L1 i: A1 C* H1 P) U
  • PID_SAMPLE_TIME PID计算周期,现单位为 ms
  • 5 C1 x+ A$ I3 k* Q9 Y4 i: K7 i
  • WHEEL_RADIUS 车轮半径, 标准单位 m
  • ; W# [; p4 \: q/ s
  • GEAR_RADIUS 电机减速比,无量纲
  • ' r+ s5 i5 x% a6 u/ s8 C7 Q$ w3 s
  • WHEEL_DIST_X X轴方向两轮之间的间距,标准单位 m
  • , ~) ?6 H& U) f$ A
  • WHEEL_DIST_Y Y轴方向两轮之间的间距,标准单位 m
  • + o1 B" I! u+ Z* S/ p, U9 d: y
$ h9 \* ~1 g* Y

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

, d( M; a6 y( Z0 \. k

注意事项

0 ?+ ^1 [4 f- a8 w4 Q
    5 _/ g# T# h) b: G4 M
  • 关注 rt-robots 软件包文档, 注意小车坐标系等
  • % p/ ^' s: m" Q6 J9 |8 Q9 X
* D/ m- ]; G5 W; V) [" B[/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:45
/ C8 k- j. S! Z$ b那么强大吗?自带两轮差速驱动模型啊

# T, O" l: g1 R, x" I' |对的, 还有四轮等的动力学模型
使用道具 举报 回复
发表于 2019-7-30 14:58:41 | 显示全部楼层
命令是pkgs --upgrade吧?
使用道具 举报 回复
发表于 2019-7-30 18:37:39 | 显示全部楼层
AdamShen 发表于 2019-7-30 14:58) H. K, [" f6 Z; ~* g6 `
命令是pkgs --upgrade吧?
1 M9 V+ ?. }" N$ p0 b+ c
是的, 感谢提醒, 我改下
使用道具 举报 回复
发表于 2019-8-1 13:56:08 | 显示全部楼层
rt-robot 软件包有更新,接口变动。更新“组装”过程
使用道具 举报 回复
发表于 2019-9-13 10:23:03 来自手机 | 显示全部楼层
求助,为啥我电机模型一直失败啊
使用道具 举报 回复
发表于 2019-10-9 20:38:58 | 显示全部楼层
依最新的rt-robot更新“组装”过程! Z7 _# X) q! f7 J: o$ w8 s( V
: p, U* d' e0 {
#include <rtthread.h>
; G( L3 Y7 r# a#include <chassis.h>
# B. p$ L3 n- I- f3 o3 t$ E#include <command.h>
  b& L* Q4 q( R5 q#include <ps2.h>- d' W  k- W" d4 B
#include <dual_pwm_motor.h>3 _+ ^0 w' C! I' [
#include <ab_phase_encoder.h>5 x" w. B: E$ Q
#include <inc_pid_controller.h>
- ~9 {5 b4 S2 N, l; [0 ~' z- ~( s: C. [( ]1 h
#define DBG_SECTION_NAME  "car"- j$ _3 y1 {4 L5 |
#define DBG_LEVEL         DBG_LOG% D/ ?: o& `0 j' u- N  Y+ O/ W
#include <rtdbg.h>
" @9 w+ u  L$ E
( O5 t+ v' Z- Q// MOTOR: A. ]2 b# ?+ t! i
#define LEFT_FORWARD_PWM            "pwm4"
$ c. L8 \: V+ k, ?#define LEFT_FORWARD_PWM_CHANNEL    3
) T1 ^; a7 g2 |9 f8 H#define LEFT_BACKWARD_PWM           "pwm4"
- ^. ]! v0 x; m  k2 X8 H#define LEFT_BACKWARD_PWM_CHANNEL   4
' t/ y; o& i/ x3 c9 i  c- d9 E. F1 _) D+ N* p6 r
#define RIGHT_FORWARD_PWM           "pwm2"2 i: |6 q* u' ^) i" ^+ J3 b
#define RIGHT_FORWARD_PWM_CHANNEL   3
' d( Z! }2 ~0 s: m#define RIGHT_BACKWARD_PWM          "pwm2"9 |1 p- e/ L6 X1 ]+ K$ B
#define RIGHT_BACKWARD_PWM_CHANNEL  44 K5 L4 [6 b/ O1 R9 U' H
' ~  g) V, ^: d2 o
// ENCODER  \) M! s; c) S4 U( R  A2 d
#define LEFT_ENCODER_A_PHASE_PIN    31      // GET_PIN(B, 15)! z; Z% B/ c' N" x8 N/ H8 h7 V% @2 K
#define LEFT_ENCODER_B_PHASE_PIN    34      // GET_PIN(C, 2)* P) ^1 t+ B" u$ T' z* `# D8 b" S* K
#define RIGHT_ENCODER_A_PHASE_PIN   38      // GET_PIN(C, 6); o' B4 @0 V9 ~) Q' @0 o
#define RIGHT_ENCODER_B_PHASE_PIN   39      // GET_PIN(C, 7)
; f9 T3 z1 L4 p: W7 d#define PULSE_PER_REVOL           2000      // Real value 2000
2 ~' G- e  L. y" [$ l#define ENCODER_SAMPLE_TIME         50$ n; W" E4 V8 ^% l$ ~( C

% z" R8 V* g! M- J$ _2 e// PID CONTROLLER8 n/ e0 ?' F# d# k3 I
#define PID_SAMPLE_TIME             50% |) e0 e2 L( D: I% N- E, ^0 j
#define PID_PARAM_KP                6.6
* m" U' j* l9 |* w+ g% ]#define PID_PARAM_KI                6.5% b- _9 j$ c) g$ ?- o
#define PID_PARAM_KD                7.62 [4 n' r$ O  L* f0 |% a- l/ Z

6 ~1 S9 i, s. @( ^// WHEEL3 N% ?2 e* {/ c; A
#define WHEEL_RADIUS             0.066+ e. W) s7 T1 i+ c0 n+ H0 b
#define GEAR_RATIO                   1
/ |7 {- E* R4 U. I1 x# A
+ k# [3 t& ]; z7 a( }// CAR  @- N6 g8 P% G0 {
chassis_t chas;  o; C4 ]) X! M* L0 E
. [2 I" ~/ q+ ]4 _+ e% c
#define WHEEL_DIST_X                 04 i( Z4 A( C" n7 S! n; y, L' ]. n
#define WHEEL_DIST_Y              0.13
3 j2 @# j' T! l% t1 J0 F% @3 K7 I7 `, c) E0 b( Q
// PS2
. ?* B7 N1 }* m. M8 ], M# r7 A#define PS2_CS_PIN                  28
+ {- _- c" `9 c* R#define PS2_CLK_PIN                 29
3 n9 H9 M! q& |2 k" V#define PS2_DO_PIN                   4! z! I! i& Z' O6 S( ]
#define PS2_DI_PIN                  36
1 e' B7 o% i, f; y4 {0 F% L; t
" e$ ]. Z1 F' T% O! n' z2 a: z( h& l// Car Thread* Z4 W% [( t' P% R. E5 l6 @/ j) ~0 ?
#define THREAD_PRIORITY             10
& R; ~9 h) G+ O% W( F#define THREAD_STACK_SIZE          512
- q* X! u3 j& K! Y( ]. t! _#define THREAD_TIMESLICE             5
5 j; T+ }0 W# V/ S! A9 |* t) a
% l; z" b0 I' j# ^9 ^" tstatic rt_thread_t tid_car = RT_NULL;
7 p  A4 r2 Y1 p# Z( ?: D% @- X  V" B7 {, G1 n
void car_thread(void *param)! X  C0 W: l1 u5 i# C
{
" Q  |  |7 s) n% f    // TODO
% e: A3 k/ d, B1 j0 x4 z* A
) W& z8 z( H2 s; L& X& A9 T    struct velocity target_velocity;
! q0 C( L7 Z/ @) H
) s3 a( C) [6 ~) h7 u    target_velocity.linear_x = 0.00f;! w: Z( s1 \: b: |" @0 k! ^; |/ n
    target_velocity.linear_y = 0;
) v% ?; I; d; N* q1 c    target_velocity.angular_z = 0;1 i( A4 U& J% A+ M0 w
    chassis_set_velocity(chas, target_velocity);
& P% A5 j; \1 K# m7 ]) _+ [* }! Y) E, U& \" b) u7 B
    // Open-loop control
" `% u3 B$ f# E% h    // controller_disable(chas->c_wheels[0]->w_controller);1 w; p2 D1 X9 n, |, x
    // controller_disable(chas->c_wheels[1]->w_controller);
: Y( j  k7 h  v% l3 |
! G4 ]) j, u/ u    while (1)
& \! f2 {# C+ B* K  }    {
& G* H+ b+ ?( u+ f3 k' d        rt_thread_mdelay(ENCODER_SAMPLE_TIME);0 L* ^4 q7 o0 e& E$ F2 X! q
        chassis_update(chas);+ O( n& l  h7 n( @) b) C
    }& a5 P  \9 n2 J, D  y9 [

( _$ b% I; Z. W//    chassis_destroy(chas);
$ C$ A7 ?: B7 }" h( Z}% N9 g# R3 F$ {; T
9 [! A0 u. r. R( c# c
void car_init(void *parameter)
& ]9 ^) J2 |' ]{9 B: w, V) K& Q$ d
    // 1. Initialize two wheels - left and right; R% q7 i8 d4 @2 D+ N: i% ~. O
    wheel_t* c_wheels = (wheel_t*) rt_malloc(sizeof(wheel_t) * 2);3 ~7 J$ l. Q# K
    if (c_wheels == RT_NULL)! T. k% ^4 E$ Q6 C' ?8 F& n
    {
2 v2 F0 a- X& @        LOG_D("Failed to malloc memory for wheels");- O# c' c5 V# K. J
    }, W" j% \4 k. _1 K' _4 H$ t+ Z& `
; N8 J( C' G+ Q% Z: p2 _4 u
    // 1.1 Create two motors- r4 F/ t: Y) j+ a3 d2 A$ K0 z
    dual_pwm_motor_t left_motor   = dual_pwm_motor_create(LEFT_FORWARD_PWM, LEFT_FORWARD_PWM_CHANNEL, LEFT_BACKWARD_PWM, LEFT_BACKWARD_PWM_CHANNEL);- v' M; I- [$ ]# n6 c( W
    dual_pwm_motor_t right_motor  = dual_pwm_motor_create(RIGHT_FORWARD_PWM, RIGHT_FORWARD_PWM_CHANNEL, RIGHT_BACKWARD_PWM, RIGHT_BACKWARD_PWM_CHANNEL);
( m7 L9 V' T/ o, x! v9 g% r: Z& s, M9 Y: y5 U! j
    // 1.2 Create two encoders
* T5 a: F( g: j    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);( }' P& d3 E; a3 e; a7 s4 {
    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);
; z/ _6 \! D! Y; `3 W8 |/ d: q7 p9 H: Q# s6 O5 i% p( L
    // 1.3 Create two pid contollers7 V! e9 B7 t. o0 |9 }& c" A+ N
    inc_pid_controller_t left_pid  = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);; V) ?6 J4 ]4 {9 s) c: A  z  V4 e
    inc_pid_controller_t right_pid = inc_pid_controller_create(PID_PARAM_KP, PID_PARAM_KI, PID_PARAM_KD, PID_SAMPLE_TIME);+ `) K# U; J- s, u" ]  n9 U
: C3 h! H* Q" Q" b: ]6 ~1 `
    // 1.4 Add two wheels
/ f) [  ^4 X. @; y/ W; |- \    c_wheels[0] = wheel_create((motor_t)left_motor,  (encoder_t)left_encoder,  (controller_t)left_pid,  WHEEL_RADIUS, GEAR_RATIO);0 _, s' e- {( ~3 l, X
    c_wheels[1] = wheel_create((motor_t)right_motor, (encoder_t)right_encoder, (controller_t)right_pid, WHEEL_RADIUS, GEAR_RATIO);
- u2 C0 M/ ]( k8 x* p- Y/ x9 Q* Z; E* q3 |' k
    // 2. Iinialize Kinematics - Two Wheel Differential Drive, |; e( x2 N5 G- W
    kinematics_t c_kinematics = kinematics_create(TWO_WD, WHEEL_DIST_X, WHEEL_DIST_Y, WHEEL_RADIUS);
$ m/ u, W! l4 M; Y9 e/ r- N5 I4 f0 N+ W' b( ~
    // 3. Initialize Chassis
+ x! Y9 T) P! o4 [  p0 A' E2 v    chas = chassis_create(c_wheels, c_kinematics);4 c( b9 s; D$ t* c. ?

6 ~; I* f. C3 f3 [! E- x" H& ?    // 4. Enable Chassis
; e! X1 c) v$ W# }+ Q    chassis_enable(chas);
: D- G: O! o2 K' T+ n
( z* P7 p3 P: C0 {  u6 V# l3 j    // Remote-control! C% }, U; b4 N+ [) x* d! E) o
    command_init(chas);% ?. O+ ~9 q, N$ B  ^* R
" c# Q# r: Y) F. {1 @
    ps2_init(PS2_CS_PIN, PS2_CLK_PIN, PS2_DO_PIN, PS2_DI_PIN);0 P' F/ S; w! l& y& a( Q8 w, Q
$ ?. s% g- |( f  G# C8 l3 N3 y
    // thread6 O6 n$ `" z0 l- f
    tid_car = rt_thread_create("tcar",) N( D+ j4 z; r
                              car_thread, RT_NULL,0 Y, i# [. G# S& B! T$ b  r
                              THREAD_STACK_SIZE,
# t# O4 \- V" l+ r- I9 L5 n6 q                              THREAD_PRIORITY, THREAD_TIMESLICE);' o) |' D% R  i+ x& r  m
$ c$ ]) c+ p- ]0 o2 ]: \
    if (tid_car != RT_NULL)  r4 c# U8 M! G7 K
    {
1 [5 r$ R8 E4 ~1 `' v- K$ ^        rt_thread_startup(tid_car);
& U6 P- X& Z, _6 l2 N6 }9 d    }
& |* G- ?" Q+ i: j; E9 N0 l2 [3 S6 E}
' W! b/ H: h$ L8 [/ a$ M) m  Z% j0 X
使用道具 举报 回复
发表于 2019-10-9 22:56:05 | 显示全部楼层
学习了!
使用道具 举报 回复
发表于 2019-10-24 20:56:37 | 显示全部楼层
sogw 发表于 2019-10-9 20:38
0 b5 M; O2 q( U; S+ {依最新的rt-robot更新“组装”过程- d0 G' z0 q1 F; f$ Y( s" J' J
/ |# F/ U+ u/ E) \$ q# i
[md]```c

6 v) k8 b8 }, k1 R% N请问这kinematics_get_velocity函数是不是预留的?demo上没用到过
使用道具 举报 回复
发表于 2019-10-29 22:09:11 | 显示全部楼层
Carry 发表于 2019-10-24 20:56! Y. J1 u- i- \. a" |. ^# [- I3 `8 Y
请问这kinematics_get_velocity函数是不是预留的?demo上没用到过
! S# y0 C. g. R$ r' |. K' w* Z
是不是预留 《==》 有没有被调用到?  用SourceInsight 或其它工具看看就知道了
使用道具 举报 回复
发表于 2019-10-30 16:35:43 | 显示全部楼层
sogw 发表于 2019-10-29 22:09& b& S0 I; `5 A8 |# U, v% n* v" J
是不是预留 《==》 有没有被调用到?  用SourceInsight 或其它工具看看就知道了 ...
) R$ u' p2 c4 c# P3 O% h* C; R
看过了,没有。找你确认一下。是不是预留来做里程计的?
使用道具 举报 回复
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

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

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

Powered by RT-Thread

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