自平衡机器人——NXTWAY

糖伯虎

前段时间很多人在问我这个机器人的程序怎么写，说老实话，我其实也是只知道个原理，真写的话，不一定写得出
这几天，腾出点空来了，给大家分享一下这个机器人。

搭建的图纸以前我发过了，地址如下
http://www.cmnxt.cn/viewthread.php?tid=32&extra=page%3D1

现在给出它的程序，用的NBC写的。我写了一些注释，方便朋友看，不影响使用

PS：需要注意的是，这个机器人对光线的要求很高，最好在较黑暗的地方测试，不然效果不如作者视频的好哦

1. 
   
2. //------------------------------------------------
   
3. // NXTway - Philo - www.philohome.com - 6/5/2006
   
4. //------------------------------------------------
   
5. dseg segment
   
6. // Sensor values
   
7. NVal word
   
8. offset word
   
9. err sdword
   
10. errold sdword
    
11. errdiff sdword
    
12. errint sdword
    
13. // Motor values
    
14. theUF byte
    
15. thePower sbyte
    
16. theOM byte OUT_MODE_MOTORON+OUT_MODE_BRAKE
    
17. theRM byte OUT_REGMODE_IDLE
    
18. theRS byte OUT_RUNSTATE_RUNNING
    
19. thePorts byte[] OUT_B, OUT_C  // motors B and C
    
20. // pid coeffs
    
21. kp sdword 30
    
22. kd sdword 35
    
23. ki sdword 5
    
24. scale sdword 45
    
25. // pid value
    
26. pid sdword
    
27. //temp var
    
28. temp sdword
    
29. // timer vars
    
30. thenTick dword
    
31. nowTick dword
    
32. dseg ends
    
33. thread main
    
34. setin IN_TYPE_LIGHT_ACTIVE, IN_2, Type
    
35.   // initialize motors
    
36.   set thePower, 0
    
37. set theUF, UF_UPDATE_SPEED+UF_UPDATE_MODE
    
38. setout thePorts, OutputMode, theOM, RegMode, theRM, RunState, theRS, UpdateFlags, theUF, Power, thePower
    
39. set theUF, UF_UPDATE_SPEED
    
40. 
    
41.   // wait a bit to let sensor stabilize
    
42. gettick nowTick
    
43. add thenTick, nowTick, 100 // 等 100 ms
    
44. Waiting:
    
45. gettick nowTick
    
46. brcmp LT, Waiting, nowTick, thenTick //如果nowTick小于thenTick时，跳到Waiting
    
47.   // reads center value. NXTway must be balanced.
    
48.   getin NVal, IN_2, NormalizedValue // More precise than PercentValue
    
49.   mov offset, NVal
    
50. Forever:
    
51. getin NVal, IN_2, NormalizedValue // read sensor values
    
52.   sub err, NVal, offset // Substract center value
    
53.   brtst GT, ErrPos, err // 如果err大于0，跳到ErrPos
    
54.   mul err, err, 16      // (less variation if far from surface)
    
55.   div err, err, 10
    
56. ErrPos:
    
57.   sub errdiff, err, errold  // 计算微分错误
    
58.   mov errold, err
    
59.   
    
60.   add errint, errint, err   // 计算积分错误
    
61.   mul errint, errint, 2     // (with fast damping)
    
62.   div errint, errint, 3
    
63.   
    
64.   mul pid, kd, errdiff      // 微分部分
    
65.   mul temp, kp, err         // 比例部分
    
66.   add pid, pid, temp        // Add Diff+Prop
    
67.   mul temp, ki, errint      // 积分部分
    
68.   add pid, pid, temp        // Add int
    
69.   div pid, pid, scale       // Scale PID value
    
70.   
    
71.   // saturate over 100 and under -100
    
72.   brcmp LT, under100, pid, 100   //pid小于100时，跳到under100
    
73.   mov pid, 100
    
74. under100:
    
75.   brcmp GT, overMin100, pid, -100  //pid大于-100时，跳到overMin100
    
76.   mov pid, -100
    
77. overMin100:
    
78.   mov thePower, pid          // Update motor power
    
79. setout thePorts, UpdateFlags, theUF, Power, thePower
    
80.   jmp Forever
    
81.   exit
    
82. endt
    

复制代码

笨虫虫

请问这个代码是怎么写入NXT的呀！

lczhd

程序怎么用？

糖伯虎

程序是用NBC写的，需要用到软件 Bricx Command Center
这个软件可以到编程区里的nxc区下载

hyzfyz

有兴趣

yiren

强啊！不知道能不能用nxt-g做（图标的那种）？

糖伯虎

NXTG写什么的程序，估计就平衡不了了

asdf

感谢楼主分享。这个机器人是什么原理啊？用一个光传感器就可以单脚走了？真奇怪。

YOUJIE987

能否用rxe程序发给我?(nxt编译后的程序)我不会用bricxcc

papa_bear

光传感器 在这里起的是什么作用

糖伯虎

回复 8# asdf


    光感靠近地面的程度不同，会有不同的光值，通过计算光值的偏差然后以PID算法反馈速度给马达，就可以达到平衡的效果

糖伯虎

回复 10# papa_bear


    光感起获取偏差的作用

糖伯虎

回复 9# YOUJIE987


    看到有点晚，使用很简单，你可以去NXC区下载个教程就行，不行再问我。

YOUJIE987

谢谢!

qd-lego

程序很复杂，看不懂