T-frog Project

> ドライバ設計者の、東北大の渡辺です。
>
>> 12 RADIUS[0] 0.150000
>> 12 RADIUS[1] -0.150000
>> のパラメータを++や-+、--に変更しても正しく動作しません。++、--の場合は下記よりも角速度が速くなり、パラメータ通りその場回転の動作になります。
>
> RADIUSの値は、キネマティクス・逆キネマティクスの計算のみに用いるので、
> モータ制御には影響を与えません。
> 現状、エンコーダのA,B相を逆に接続している場合に、
> ソフトウェアで逆転する設定は用意していません。
> もし、逆の場合は配線を修正して下さい。
>
>
>> 1回転1000パルスの2相エンコーダ、3相ホール素子、原点信号（index）のセンサを持ち、4極のブラシレスモータとなります。5V電源を入れた状態で、GNDとパルス間の波形はオシロで出ている事は確認してあります。
>
> 多極のモータなので、マニュアルにも記載していますが、
> 2磁極（電気角と機械角が一致）するモータにギアがついているように換算してパラメータを与える必要があります。
> このあたりの計算は、ドライバのページからリンクがある、パラメータ生成ツールで計算できます。
> モータのスペックに4極と書いてあるのは、おそらく4磁極なので、
> パラメータ生成ツールの極数欄には2を入力することになります。
>
> // Atsushi WATANABE
> // Assistant Professor
> // Field Robotics Laboratory, Tohoku University
> // 6-6-10, Aramaki-Aoba, Aoba-ku, Sendai, 980-8579, Japan
> // Phone: [Telephone number removed] / E-mail:
>
>
> 2016年8月3日 0:44 河原貴軌 <>:
>> お世話になっております。
>> 金沢工業大学OBの河原と申します。
>>
>>
>> 趣味用途で貴社のモータドライバTF-2MD3-R6Aを使用してブラシレスモータを回そうとしているのですが正常な動作を確認できず、ご教授願いたく連絡致しました。
>>
>> 現状、モータの接続と動作確認のため、ギアやタイヤなどを付けずにrun_testの動作を机上で見ているのですが、回転後に速度が変動せず制御されている動作が確認できません。
>> また、-oで確認できるオドメトリの値も回転方向が補正されず、正しく動いているように見えません。
>> パラメータなのか、何か問題があればご指摘お願い致します。
>>
>>
>> 環境としては
>> ・ypspurをインストールしたUbuntu14.04から、sampleのrun_testで動作確認を行っています。
>>
>> そのrun_testの動作中、本来は2つのモータが逆回転し、1m進んだ所で一度停止、同じ方向に回転して旋回動作、180度（th 1.57）になると再度停止し、1m戻ってくるような動作になると思いますが、
>> 両モータとも反転動作も無く、等速で回転し続ける状態から変化しません。
>> メールの最後に動作中のコンソールを添付しましたが、直進の制御になるはずが角度補正が制御されず回転を続けるような動作に見えます。
>>
>> エンコーダの回転方向が怪しいとは思いましたが、
>> 12 RADIUS[0] 0.150000
>> 12 RADIUS[1] -0.150000
>> のパラメータを++や-+、--に変更しても正しく動作しません。++、--の場合は下記よりも角速度が速くなり、パラメータ通りその場回転の動作になります。
>> たまたまx座標が1mを超えた事でspin指令に入る事がありますが、停止せず逆回転もなくそのまま周り続けます。
>>
>> ・モータはACサーボモータ（3相ブラシレス）の「42JSF630AS-1000」という中国製の物を使用しています。
>> ( http://www.jmc-driver.com/h-pd-138.html )
>> 1回転1000パルスの2相エンコーダ、3相ホール素子、原点信号（index）のセンサを持ち、4極のブラシレスモータとなります。5V電源を入れた状態で、GNDとパルス間の波形はオシロで出ている事は確認してあります。
>> 定格は24Vですが、今は駆動電圧12Vで動作を行っています。
>> エンコーダの位相遅れなどが明記されていなかったため、配線は手探りで安定して回転できる接続を探しました。
>>
>> MASSも1や25kgとしてみたりしましたが特に変化ありません。
>> ファームウエアについては、既存の物とv0.1.3の両方を試しています。
>> LEDなども異常アラームは発生していません。
>>
>>
>>
>> お手数ですが、宜しくお願い致します。
>>
>>
>> ・参考までに動作中のコンソール表示を下記に添付致します。
>> ~/ypspur/yp-robot-params/robot-params$ ypspur-coordinator -p test.param -d /dev/ttyACM1 --verbose -o
>> ++++++++++++++++++++++++++++++++++++++++++++++++++
>> YamabicoProject-Spur
>> Ver. 1.14.0
>> ++++++++++++++++++++++++++++++++++++++++++++++++++
>> Device Information
>> Port : /dev/ttyACM1
>> Vender : T-frog Prject
>> Product : T-frog Motor Driver rev.5
>> Firmware: 98d3cea
>> Protcol : YPP:06:00
>> Serialno: 01300000
>> ++++++++++++++++++++++++++++++++++++++++++++++++++
>> Driver depending parameters
>> Name : unknown
>> PWM resolution: 1200
>> Motor number : 2
>> ++++++++++++++++++++++++++++++++++++++++++++++++++
>> Parameter file: test.param
>> 0 VERSION 4.000000
>> 5 VOLT 12.000000
>> 6 CYCLE 0.001000
>> 16 CONTROL_CYCLE 0.020000
>> 2 TORQUE_FINENESS 0.000005
>> 12 RADIUS[0] 0.150000
>> 12 RADIUS[1] -0.150000
>> 15 TREAD 0.200000
>> 37 MASS 0.500000
>> 40 TIRE_M_INERTIA 0.000000
>> 38 MOMENT_INERTIA 0.000000
>> 31 TORQUE_VISCOS 0.000050
>> 30 TORQUE_NEWTON 0.020000
>> 29 TORQUE_MAX 0.200000
>> 36 TORQUE_LIMIT 1.000000
>> 11 MOTOR_PHASE 3.000000
>> 4 COUNT_REV 1000.000000
>> 7 GEAR 4.000000
>> 10 MOTOR_VC 500.000000
>> 9 MOTOR_TC 0.053000
>> 8 MOTOR_R 1.100000
>> 39 MOTOR_M_INERTIA 0.000000
>> 27 GAIN_KP 10.000000
>> 28 GAIN_KI 3.000000
>> 34 INTEGRAL_MAX 0.500000
>> 17 MAX_VEL 2.000000
>> 18 MAX_W 3.000000
>> 19 MAX_ACC_V 2.000000
>> 20 MAX_ACC_W 12.000000
>> 21 MAX_CENTRI_ACC 2.450000
>> 22 L_C1 0.010000
>> 23 L_K1 800.000000
>> 24 L_K2 300.000000
>> 25 L_K3 200.000000
>> 26 L_DIST 0.600000
>> A 0.000703
>> B 0.000703
>> C -0.000703
>> D -0.000703
>> E 0.000000
>> F 0.000000
>> ++++++++++++++++++++++++++++++++++++++++++++++++++
>>
>> Warn: Baudrate setting is not supported on this device.
>> Applying parameters.
>> YP-Spur coordinator started.
>> Trajectory control loop started.
>> Command analyzer started.
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> Command: vel 1.000000
>> Mode: servo 1
>> Command: dvel 1.000000
>> Command: w 0.157080
>> Command: dw 1.570796
>> Command: set pos 0.000000 0.000000 0.000000
>> Command: stop_line 0.997279 -0.142067 -0.142550
>> Mode: servo 3
>> Command: over line ( dist = -0.995000 )
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007422 0.000000 -0.142550
>> 0.007189 0.000033 -0.144906
>> 0.007189 0.000033 -0.144906
>> 0.007072 0.000050 -0.143728
>> 0.006722 0.000101 -0.142550
>> 0.006373 0.000151 -0.143728
>> 0.005556 0.000269 -0.147262
>> 0.004857 0.000373 -0.149618
>> 0.004857 0.000373 -0.151975
>> 0.004275 0.000462 -0.153153
>> 0.003926 0.000516 -0.154331
>> 0.003576 0.000571 -0.153153
>> 0.002179 0.000786 -0.155509
>> -0.000149 0.001151 -0.164934
>> -0.005029 0.001964 -0.174358
>> -0.020460 0.004682 -0.187317
>> Command: over line ( dist = -1.007604 )
>> -0.039559 0.008302 -0.193208
>> -0.059330 0.012170 -0.189674
>> -0.079345 0.016012 -0.193208
>> -0.098307 0.019722 -0.207345
>> -0.116752 0.023603 -0.216770
>> -0.135506 0.027733 -0.217948
>> -0.155060 0.032063 -0.222660
>> -0.173789 0.036304 -0.235619
>> -0.192461 0.040787 -0.246222
>> -0.211085 0.045467 -0.249757
>> -0.230377 0.050388 -0.253291
>> -0.248397 0.055053 -0.269784
>> -0.266224 0.059982 -0.282743
>> -0.284099 0.065175 -0.294524
>> -0.303490 0.071058 -0.299237
>> -0.321727 0.076684 -0.311018
>> -0.338999 0.082236 -0.325155
>> -0.355856 0.087919 -0.340470
>> -0.373735 0.094252 -0.353429
>> -0.392082 0.101021 -0.362854
>> Command: over line ( dist = -1.404804 )
>> -0.409044 0.107461 -0.379347
>> -0.426006 0.114223 -0.392306
>> -0.443206 0.121340 -0.406444
>> -0.460953 0.128978 -0.413512
>> -0.477782 0.136363 -0.425293
>> -0.494201 0.143800 -0.445321
>> -0.510894 0.151767 -0.462992
>> -0.527864 0.160238 -0.473595
>> -0.545058 0.169050 -0.475951
>> -0.561080 0.177309 -0.488910
>> -0.577409 0.185996 -0.508938
>> -0.593458 0.194951 -0.527788
>> -0.609844 0.204503 -0.533678
>> -0.625971 0.214032 -0.544281
>> -0.641795 0.223609 -0.559596
>> -0.657370 0.233365 -0.576090
>> -0.672980 0.243505 -0.585514
>> -0.689180 0.254248 -0.593761
>> -0.704413 0.264530 -0.614967
>> -0.719232 0.274997 -0.633816
>> Command: over line ( dist = -1.753351 )
>> -0.733566 0.285532 -0.646775
>> -0.748984 0.297175 -0.653844
>> -0.764414 0.308999 -0.664447
>> -0.778790 0.320259 -0.684474
>> -0.792754 0.331655 -0.702146
>> -0.806696 0.343449 -0.717461
>> -0.821342 0.356229 -0.725708
>> -0.835090 0.368426 -0.742201
>> -0.848547 0.380769 -0.757517
>> -0.861986 0.393478 -0.775188
>> -0.875786 0.406999 -0.789325
>> -0.889145 0.420464 -0.799928
>> -0.901376 0.433055 -0.815243
>> -0.913736 0.446175 -0.835271
>> -0.926304 0.460065 -0.855299
>> -0.939133 0.474825 -0.862367
>> -0.951167 0.488871 -0.872970
>> -0.962749 0.502682 -0.892998
>> -0.974052 0.516723 -0.913025
>> -0.985288 0.531267 -0.927163
>> Command: over line ( dist = -2.053117 )
>> -0.996741 0.546533 -0.934231
>> -1.007666 0.561312 -0.953081
>> -1.018447 0.576486 -0.969574
>> -1.028709 0.591447 -0.983711
>> -1.039085 0.607043 -0.989602
>> -1.049499 0.622896 -1.002561
>> -1.059262 0.638187 -1.023767
>> -1.068699 0.653683 -1.042616
>> -1.077902 0.669455 -1.053219
>> -1.087694 0.686654 -1.057931
>> -1.096712 0.702668 -1.072068
>> -1.105334 0.718497 -1.089740
>> -1.113620 0.734372 -1.106233
>> -1.122223 0.751540 -1.119192
>> -1.130603 0.768818 -1.129795
>> -1.138297 0.785118 -1.142754
>> -1.145828 0.801624 -1.154535
>> -1.153402 0.818756 -1.169851
>> -1.161081 0.836870 -1.178097
>> -1.168114 0.853850 -1.192234
>> Command: over line ( dist = -2.279917 )
>> -1.174689 0.870379 -1.209906
>> -1.181220 0.887684 -1.227577
>> -1.187642 0.905656 -1.234646
>> -1.194015 0.923895 -1.239358
>> -1.199919 0.941051 -1.255852
>> -1.205685 0.958749 -1.277057
>> -1.211006 0.976340 -1.293551
>> -1.216198 0.994583 -1.297085
>> -1.221197 1.012391 -1.310044
>> -1.226026 1.030489 -1.327716
>> -1.230478 1.048442 -1.345387
>> -1.234718 1.066929 -1.353634
>> -1.238830 1.085566 -1.363059
>> -1.242596 1.103434 -1.383086
>> -1.245937 1.121027 -1.401936
>> -1.249026 1.139144 -1.416073
>> -1.252040 1.158466 -1.423141
>> -1.254882 1.177577 -1.437279
>> -1.257298 1.195558 -1.456128
>> -1.259319 1.213114 -1.472622
>> Command: over line ( dist = -2.421237 )
>> -1.261121 1.231403 -1.486759
>> -1.262772 1.251008 -1.497362
>> -1.264121 1.269337 -1.516211
>> -1.265110 1.287453 -1.530348
>> -1.265854 1.305816 -1.546842
>> -1.266316 1.325131 -1.556266
>> -1.266587 1.343743 -1.568047
>> -1.266636 1.361532 -1.588075
>> -1.266320 1.379790 -1.608103
>> -1.265630 1.398273 -1.625774
>> -1.264562 1.417683 -1.629308
>> -1.263501 1.435794 -1.641089
>> -1.262227 1.453892 -1.659939
>> -1.260601 1.472080 -1.682323
>> -1.258543 1.490461 -1.695282
>> -1.256202 1.509165 -1.702351
>> -1.253807 1.527268 -1.720022
>> -1.251075 1.545442 -1.736515
>> -1.248062 1.563452 -1.749474
>> -1.244691 1.582118 -1.755365
>> Command: over line ( dist = -2.459181 )
>> -1.241210 1.600763 -1.770680
>> -1.237632 1.618429 -1.793064
>> -1.233684 1.635895 -1.809557
>> -1.229338 1.653752 -1.816626
>> -1.224550 1.672835 -1.822516
>> -1.219913 1.690862 -1.841366
>> -1.215064 1.708345 -1.857859
>> -1.209994 1.725519 -1.874353
>> -1.204254 1.743844 -1.884956
>> -1.198393 1.761883 -1.895558
>> -1.192604 1.779078 -1.909696
>> -1.186494 1.796411 -1.923833
>> -1.179895 1.814319 -1.940326
>> -1.172832 1.832555 -1.947395
>> -1.166203 1.849317 -1.962710
>> -1.159408 1.865757 -1.978025
>> -1.152036 1.882849 -1.996875
>> -1.143953 1.900657 -2.006300
>> -1.135802 1.918174 -2.013368
>> -1.128083 1.934463 -2.028683
>> Command: over line ( dist = -2.393812 )
>> -1.120011 1.950842 -2.048711
>> -1.111504 1.967266 -2.064026
>> -1.102412 1.984180 -2.069917
>> -1.093502 2.000523 -2.084054
>> -1.084247 2.016944 -2.102904
>> -1.074803 2.032984 -2.119397
>> -1.065035 2.048967 -2.125287
>> -1.055048 2.065093 -2.133534
>> -1.045369 2.080438 -2.157096
>> -1.035200 2.095747 -2.178302
>> -1.024710 2.110837 -2.190083
>> -1.013358 2.126761 -2.194795
>> -1.002413 2.141963 -2.210110
>> -0.991659 2.156428 -2.227782
>> -0.980723 2.170608 -2.244275
>> -0.969113 2.185157 -2.258412
>> -0.956700 2.200270 -2.270193
>> -0.944869 2.214333 -2.286687
>> -0.933117 2.227844 -2.298468
>> -0.920971 2.241480 -2.313783
>> Command: over line ( dist = -2.232418 )
>> -0.907821 2.255796 -2.324386
>> -0.895004 2.269456 -2.339701
>> -0.882798 2.282071 -2.357373
>> -0.870038 2.294801 -2.377400
>> -0.856684 2.307600 -2.390359
>> -0.842478 2.320867 -2.393894
>> -0.829175 2.333204 -2.408031
>> -0.815611 2.345429 -2.428058
>> -0.801894 2.357304 -2.449264
>> -0.787656 2.369111 -2.459867
>> -0.773111 2.380914 -2.465758
>> -0.758865 2.392337 -2.483429
>> -0.744232 2.403650 -2.501100
>> -0.729402 2.414704 -2.516416
>> -0.713927 2.425873 -2.521128
>> -0.698782 2.436695 -2.537621
>> -0.684043 2.446865 -2.556471
>> -0.669017 2.456820 -2.574142
>> -0.653221 2.466888 -2.582389
>> -0.636443 2.477388 -2.589458
>> Command: over line ( dist = -1.984291 )
>> -0.620696 2.487089 -2.609485
>> -0.605061 2.496294 -2.625979
>> -0.589585 2.505066 -2.638938
>> -0.572654 2.514373 -2.648363
>> -0.555740 2.523465 -2.663678
>> -0.539421 2.531918 -2.680171
>> -0.523070 2.540048 -2.693130
>> -0.505872 2.548323 -2.704911
>> -0.488257 2.556544 -2.713158
>> -0.471862 2.564033 -2.730829
>> -0.455228 2.571277 -2.747323
>> -0.437825 2.578518 -2.766172
>> -0.419411 2.585775 -2.773241
>> -0.401826 2.592562 -2.780309
>> -0.385075 2.598892 -2.796803
>> -0.367667 2.605144 -2.816831
>> -0.349803 2.611158 -2.833324
>> -0.331393 2.617020 -2.840392
>> -0.313955 2.622438 -2.853352
>> -0.296109 2.627729 -2.869845
>> Command: over line ( dist = -1.668767 )
>> -0.278405 2.632662 -2.883982
>> -0.259949 2.637524 -2.891051
>> -0.241802 2.642169 -2.904010
>> -0.224055 2.646466 -2.926394
>> -0.206216 2.650366 -2.944065
>> -0.188426 2.653926 -2.953490
>> -0.169330 2.657561 -2.957024
>> -0.150916 2.660999 -2.974696
>> -0.132793 2.664052 -2.993545
>> -0.115199 2.666676 -3.011217
>> -0.096743 2.669096 -3.020641
>> -0.077797 2.671399 -3.030066
>> -0.059767 2.673418 -3.048916
>> -0.041585 2.675108 -3.061875
>> -0.023265 2.676571 -3.078368
>> -0.003748 2.677807 -3.087793
>> 0.014486 2.678789 -3.103108
>> 0.031909 2.679460 -3.119602
>> 0.049930 2.679856 -3.137273
>> 0.069015 2.679938 -3.151410
>> Command: over line ( dist = -1.314764 )
>> 0.088570 2.679746 -3.158479
>> 0.106593 2.679442 -3.173794
>> 0.124491 2.678865 -3.192644
>> 0.142492 2.677946 -3.212671
>> 0.161294 2.676607 -3.222096
>> 0.179848 2.675110 -3.231521
>> 0.198269 2.673449 -3.251548
>> 0.216770 2.671406 -3.270398
>> 0.235230 2.669016 -3.282179
>> 0.254010 2.666358 -3.285713
>> 0.272082 2.663735 -3.303385
>> 0.290104 2.660794 -3.325769
>> 0.308055 2.657449 -3.343440
>> 0.326638 2.653647 -3.351687
>> 0.345764 2.649568 -3.358755
>> 0.363481 2.645659 -3.377605
>> 0.381120 2.641417 -3.394098
>> 0.398688 2.636884 -3.410592
>> 0.417654 2.631656 -3.421194
>> 0.435772 2.626454 -3.437688
>> Command: over line ( dist = -0.944129 )
>> 0.453238 2.621126 -3.450647
>> 0.470521 2.615607 -3.462428
>> 0.488856 2.609514 -3.474209
>> 0.507118 2.603206 -3.485990
>> 0.524306 2.597041 -3.506017
>> 0.541037 2.590658 -3.522511
>> 0.558426 2.583694 -3.537826
>> 0.576685 2.576056 -3.540182
>> 0.593839 2.568831 -3.551963
>> 0.610476 2.561593 -3.568457
>> 0.627205 2.553984 -3.589662
>> 0.644300 2.545767 -3.602621
>> 0.661287 2.537329 -3.606156
>> 0.677402 2.529253 -3.621471
>> 0.694018 2.520605 -3.636786
>> 0.710499 2.511704 -3.654458
>> 0.727334 2.502224 -3.659170
>> 0.743408 2.493072 -3.672129
>> 0.758853 2.484011 -3.690979
>> 0.774326 2.474538 -3.709828
>> Command: over line ( dist = -0.587426 )
>> 0.790015 2.464521 -3.719253
>> 0.806398 2.453842 -3.723965
>> 0.821944 2.443604 -3.742815
>> 0.836808 2.433408 -3.760486
>> 0.851298 2.423088 -3.775802
>> 0.866387 2.411989 -3.784048
>> 0.881667 2.400554 -3.798186
>> 0.896224 2.389335 -3.817035
>> 0.910383 2.377992 -3.831172
>> 0.924653 2.366224 -3.844131
>> 0.939489 2.353664 -3.852378
>> 0.953238 2.341827 -3.868871
>> 0.966439 2.330078 -3.887721
>> 0.979848 2.317683 -3.907749
>> 0.993770 2.304286 -3.919530
>> 1.007533 2.290727 -3.926598
>> 1.020117 2.278153 -3.941913
>> 1.032753 2.265134 -3.960763
>> 1.045302 2.251707 -3.981969
>> 1.058193 2.237315 -3.991393
>> 1.070403 2.223422 -4.004353
>> Command: over line ( dist = -0.258677 )
>> 1.082355 2.209461 -4.020846
>> 1.094151 2.195214 -4.038517
>> 1.105913 2.180485 -4.050298
>> 1.117573 2.165525 -4.056189
>> 1.128858 2.150870 -4.078573
>> 1.139742 2.136062 -4.099778
>> 1.150174 2.121218 -4.116272
>> 1.160889 2.105425 -4.118628
>> 1.171633 2.089509 -4.129231
>> 1.181818 2.074069 -4.149258
>> 1.191566 2.058628 -4.169286
>> 1.201003 2.042996 -4.182245
>> 1.210773 2.026327 -4.189314
>> 1.220072 2.010202 -4.205807
>> 1.228989 1.994132 -4.219944
>> 1.237622 1.978041 -4.232903
>> 1.246698 1.960585 -4.243506
>> 1.255269 1.943665 -4.256465
>> 1.263153 1.927587 -4.272959
>> 1.270772 1.911381 -4.287096
>> Command: over line ( dist = -0.016010 )
>> 1.278549 1.894211 -4.301233
>> 1.286553 1.875852 -4.305945
>> 1.293819 1.858971 -4.320083
>> 1.300755 1.842207 -4.334220
>> 1.307454 1.825346 -4.353069
>> 1.314165 1.807480 -4.362494
>> 1.320627 1.789772 -4.369563
>> 1.326804 1.772463 -4.386056
>> 1.332809 1.754720 -4.403728
>> 1.338500 1.736874 -4.416687
>> 1.344027 1.718730 -4.420221
>> 1.349253 1.701356 -4.434358
>> 1.354264 1.683796 -4.454386
>> 1.358954 1.666026 -4.473235
>> 1.363419 1.647713 -4.480304
>> 1.367836 1.629025 -4.486194
>> 1.371957 1.611115 -4.505044
>> 1.375668 1.593476 -4.522715
>> 1.379044 1.575890 -4.539209
>> 1.382353 1.556975 -4.547455
>> Command: over line ( dist = 0.144790 )
>> Command: spin 1.428247
>> Mode: servo 5
>> Command: near ang ( dist = 0.307483 )
>> 1.385467 1.538265 -4.565127
>> 1.388147 1.520202 -4.580442
>> 1.390518 1.502334 -4.593401
>> 1.392728 1.483851 -4.605182
>> 1.394808 1.464524 -4.615785
>> 1.396569 1.446349 -4.638169
>> 1.397879 1.428726 -4.657018
>> 1.398890 1.410494 -4.674690
>> 1.399623 1.391069 -4.682937
>> 1.400181 1.372110 -4.691183
>> 1.400561 1.354207 -4.710033
>> 1.400604 1.336064 -4.731239
>> 1.400250 1.317335 -4.751266
>> 1.399509 1.298265 -4.758335
>> 1.398670 1.280024 -4.771294
>> 1.397602 1.261912 -4.787787
>> 1.396191 1.243234 -4.807815
>> 1.394361 1.224118 -4.818418
>> 1.392379 1.205492 -4.826664
>> 1.390324 1.187584 -4.846692
>> Command: near ang ( dist = 0.008247 )
>> Command: vel 0.300000
>> Command: dvel 1.000000
>> Command: w 3.000000
>> Command: dw 3.141593
>> Command: stop_line 1.011486 -0.043082 1.428247
>> Mode: servo 3
>> Command: over line ( dist = 1.277004 )
>> Command: spin 2.999043
>> Mode: servo 5
>> Command: near ang ( dist = -1.562550 )
>> 1.387878 1.169488 -4.866720
>> 1.385053 1.151328 -4.880857
>> 1.381814 1.132281 -4.883213
>> 1.378589 1.113590 -4.898528
>> 1.375166 1.095413 -4.918556
>> 1.371525 1.078001 -4.936227
>> 1.367471 1.060196 -4.944474
>> 1.363028 1.041393 -4.953899
>> 1.358547 1.023205 -4.971570
>> 1.353837 1.005440 -4.985708
>> 1.348908 0.987858 -4.998667
>> 1.343419 0.969210 -5.004557
>> 1.338058 0.951385 -5.018694
>> 1.332694 0.934424 -5.036366
>> 1.326918 0.917225 -5.052859
>> 1.320506 0.899124 -5.065818
>> 1.313655 0.880555 -5.070531
>> 1.307337 0.863674 -5.083490
>> 1.300843 0.846986 -5.097627
>> 1.293937 0.829955 -5.116476
>> Command: near ang ( dist = -1.832334 )
>> 1.286340 0.812190 -5.128257
>> 1.278725 0.794947 -5.137682
>> 1.271240 0.778421 -5.151819
>> 1.263321 0.761575 -5.168313
>> 1.255022 0.744651 -5.180094
>> 1.246364 0.727510 -5.185984
>> 1.238090 0.711364 -5.202477
>> 1.229439 0.695149 -5.226039
>> 1.220409 0.679143 -5.242533
>> 1.210818 0.662779 -5.248423
>> 1.201130 0.646472 -5.254314
>> 1.191651 0.630727 -5.275519
>> 1.181777 0.615087 -5.295547
>> 1.171787 0.599943 -5.312040
>> 1.160949 0.584090 -5.317931
>> 1.150354 0.568786 -5.334424
>> 1.139645 0.553850 -5.350918
>> 1.128762 0.539187 -5.366233
>> 1.117153 0.524038 -5.378014
>> 1.105149 0.508749 -5.390973
>> Command: near ang ( dist = -2.106831 )
>> 1.093909 0.494809 -5.412179
>> 1.082528 0.481291 -5.428672
>> 1.070538 0.467518 -5.443987
>> 1.057316 0.452790 -5.453412
>> 1.044512 0.438797 -5.468727
>> 1.032223 0.425772 -5.485221
>> 1.019639 0.412867 -5.505248
>> 1.006463 0.399886 -5.520564
>> 0.992664 0.386702 -5.527632
>> 0.979458 0.374262 -5.544126
>> 0.965700 0.361723 -5.560619
>> 0.951738 0.349414 -5.579469
>> 0.937186 0.337065 -5.586537
>> 0.923000 0.325197 -5.594784
>> 0.909262 0.313895 -5.614811
>> 0.894838 0.302506 -5.636017
>> 0.879894 0.291212 -5.648976
>> 0.864330 0.279763 -5.651332
>> 0.849215 0.268700 -5.666648
>> 0.834412 0.258209 -5.685497
>> Command: near ang ( dist = -2.401355 )
>> 0.819512 0.248066 -5.700813
>> 0.803967 0.237828 -5.710237
>> 0.787632 0.227291 -5.720840
>> 0.771984 0.217429 -5.738512
>> 0.756366 0.207967 -5.751471
>> 0.740627 0.198709 -5.762074
>> 0.723666 0.188973 -5.769142
>> 0.707355 0.179763 -5.786814
>> 0.691609 0.171235 -5.805663
>> 0.675496 0.162897 -5.819800
>> 0.658318 0.154313 -5.828047
>> 0.640753 0.145717 -5.835116
>> 0.624401 0.137857 -5.849253
>> 0.608047 0.130278 -5.866924
>> 0.590807 0.122656 -5.885774
>> 0.572883 0.115133 -5.894021
>> 0.555879 0.108160 -5.905801
>> 0.539122 0.101518 -5.918761
>> 0.521621 0.094842 -5.936432
>> 0.503450 0.088276 -5.948213
>> Command: near ang ( dist = -2.664071 )
>> 0.485537 0.082040 -5.956460
>> 0.468465 0.076255 -5.974131
>> 0.451070 0.070701 -5.994159
>> 0.433454 0.065463 -6.010652
>> 0.414733 0.060231 -6.014186
>> 0.396561 0.055221 -6.025967
>> 0.378788 0.050546 -6.047173
>> 0.361148 0.046304 -6.068379
>> 0.343307 0.042411 -6.078982
>> 0.324618 0.038541 -6.086050
>> 0.306480 0.034918 -6.106078
>> 0.288042 0.031618 -6.123749
>> 0.270129 0.028738 -6.140243
>> 0.251239 0.026019 -6.147311
>> 0.232563 0.023466 -6.161449
>> 0.214905 0.021306 -6.181476
>> 0.197208 0.019499 -6.199148
>> 0.178777 0.017947 -6.214463
>> 0.158914 0.016580 -6.222710
>> 0.140569 0.015469 -6.236847
>> Command: near ang ( dist = -2.952704 )
>> 0.122799 0.014645 -6.252162
>> 0.104901 0.014089 -6.271012
>> 0.086170 0.013861 -6.286327
>> 0.066850 0.013922 -6.295752
>> 0.048944 0.014147 -6.312245
>> 0.030573 0.014681 -6.326382
>> 0.012095 0.015480 -6.341697
>> -0.006958 0.016596 -6.348766
>> -0.025297 0.017800 -6.360547
>> -0.043268 0.019193 -6.382931
>> -0.061672 0.021035 -6.402959
>> -0.080153 0.023259 -6.412383
>> -0.099195 0.025733 -6.413561
>> -0.117418 0.028123 -6.430055
>> -0.135482 0.030795 -6.450082
>> -0.153488 0.033828 -6.467754
>> -0.172018 0.037288 -6.474822
>> -0.190638 0.040900 -6.485425
>> -0.208526 0.044568 -6.503097
>> -0.226462 0.048577 -6.517234
>> Command: near ang ( dist = 3.050094 )
>> -0.244569 0.052894 -6.526659
>> -0.263777 0.057666 -6.536084
>> -0.281571 0.062264 -6.554933
>> -0.298820 0.067071 -6.571426
>> -0.316102 0.072195 -6.584386
>> -0.334778 0.077996 -6.591454
>> -0.353412 0.083930 -6.603235
>> -0.370746 0.089675 -6.620907
>> -0.387641 0.095608 -6.637400
>> -0.405210 0.102105 -6.652715
>> -0.423336 0.109126 -6.660962
>> -0.440419 0.115905 -6.675099
>> -0.457404 0.122925 -6.689236
>> -0.474721 0.130370 -6.708086
>> -0.492431 0.138383 -6.718689
>> -0.509414 0.146286 -6.726935
>> -0.525267 0.153822 -6.744607
>> -0.541829 0.162057 -6.764634
>> -0.558432 0.170731 -6.782306
>> -0.575602 0.180091 -6.787018
>> Command: near ang ( dist = 2.780309 )
>> -0.591903 0.189078 -6.798799
>> -0.607687 0.198024 -6.817649
>> -0.623198 0.207205 -6.837676
>> -0.638923 0.216944 -6.848279
>> -0.655141 0.227227 -6.856526
>> -0.670778 0.237324 -6.877732
>> -0.686003 0.247618 -6.894225
>> -0.700863 0.258027 -6.906006
>> -0.716269 0.269091 -6.911897
>> -0.731514 0.280176 -6.928390
>> -0.745915 0.291016 -6.950774
>> -0.759978 0.302102 -6.967267
>> -0.774313 0.313791 -6.980226
>> -0.789307 0.326345 -6.984939
>> -0.803343 0.338209 -7.001432
>> -0.816738 0.349916 -7.019103
>> -0.830185 0.362095 -7.037953
>> -0.844088 0.375170 -7.052090
>> -0.857804 0.388441 -7.061515
>> -0.870387 0.400848 -7.075652
>> Command: near ang ( dist = 2.491676 )
>> -0.883373 0.414019 -7.090967
>> -0.896319 0.427557 -7.106283
>> -0.909536 0.441811 -7.114529
>> -0.921762 0.455215 -7.128666
>> -0.933719 0.468703 -7.148694
>> -0.945557 0.482607 -7.166366
>> -0.957521 0.497173 -7.175790
>> -0.969642 0.512219 -7.180503
>> -0.981105 0.526584 -7.199352
>> -0.992367 0.541257 -7.219380
>> -1.003123 0.555867 -7.235873
>> -1.013978 0.571133 -7.239408
>> -1.024846 0.586534 -7.253545
>> -1.035496 0.602086 -7.272394
>> -1.045722 0.617640 -7.288888
>> -1.055753 0.633459 -7.297134
>> -1.065964 0.649861 -7.306559
>> -1.075468 0.665453 -7.326587
>> -1.084540 0.681029 -7.341902
>> -1.093545 0.697050 -7.356039
>