harddifferential.txt 61.8 KB
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351 1352
; generated by Component: ARM Compiler 5.06 update 6 (build 750) Tool: ArmCC [4d3637]
; commandline ArmCC [--c99 --list --split_sections --debug -c --asm --interleave -o.\flash\obj\harddifferential.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\harddifferential.d --cpu=Cortex-M4.fp --apcs=interwork -O1 --diag_suppress=9931,870 -I..\..\Libraries\CMSIS\Include -I..\..\Libraries\CMSIS\Device\ST\STM32F4xx\Include -I..\..\Libraries\STM32F4xx_StdPeriph_Driver\inc -I..\..\uCOS-III\uC-CPU -I..\..\uCOS-III\uC-LIB -I..\..\uCOS-III\uCOS-III\Ports -I..\..\uCOS-III\uCOS-III\Source -I..\..\uCOS-III\uC-CPU\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uC-LIB\Ports\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uCOS-III\Ports\ARM-Cortex-M4\Generic\RealView -I..\..\User -I..\..\User\bsp -I..\..\User\bsp\inc -I..\..\User\libapp -I..\..\RL-ARM\Config -I..\..\RL-ARM\Driver -I..\..\RL-ARM\RL-RTX\inc -I..\..\User\bsp\BSP -I..\..\RL-ARM\RL-CAN -I..\..\Libraries\DSP_LIB\Include -I..\..\MODBUS\modbus\rtu -I..\..\MODBUS\BARE\port -I..\..\MODBUS\modbus\include -I..\..\User\bsp\BSP -I..\..\PLC -I..\..\Avoid -I..\..\User\parameter -I..\..\User\LaserMotionCtr -I..\..\User\W5100S -I..\..\User\bsp -I..\..\User\CHASSIS -I..\..\User\CONTROLFUNCTION -I..\..\User\DATAUPDATE -I..\..\User\HARAWARE -I..\..\User\MOTORDRIVER -I..\..\User\NAVAGATION -I..\..\User\PLATFORM -I..\..\User\SENSOR -I.\RTE\_Flash -IC:\Users\YDJ\AppData\Local\Arm\Packs\ARM\CMSIS\5.5.1\CMSIS\Core\Include -IC:\Users\YDJ\AppData\Local\Arm\Packs\Keil\STM32F4xx_DFP\2.13.0\Drivers\CMSIS\Device\ST\STM32F4xx\Include -D__UVISION_VERSION=527 -D_RTE_ -DSTM32F407xx -DUSE_STDPERIPH_DRIVER -DSTM32F40_41xxx -D__RTX -D__FPU_USED=1 --omf_browse=.\flash\obj\harddifferential.crf ..\..\User\CHASSIS\HardDifferential.c]
                          THUMB

                          AREA ||i.HardDifferentialSpeedPirouette||, CODE, READONLY, ALIGN=2

                  HardDifferentialSpeedPirouette PROC
;;;265    //硬差速原地转弯
;;;266    void HardDifferentialSpeedPirouette()
000000  e92d5ff0          PUSH     {r4-r12,lr}
;;;267    {
000004  ed2d8b02          VPUSH    {d8}
;;;268        static float agvInAngle = 0,turnAngleDiff = 0,lastAngleDiff = 0;
;;;269        static u8 TurnStep = 0, TurnArriveFlag = 0;
;;;270        static int lastTime = 0;
;;;271    //		static int turnCount = 0;
;;;272        if (TurnStep == 0) //求出位置信息
;;;273        {
;;;274            navi.Private.TarAngle = CalculatingDirectionAngle(StartPoint, TargetPoint); //求出目标方向角度
;;;275    				
;;;276    				agv.Command.SetPlatformTurnAngle = fabs(CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle, navi.Private.TarAngle))*57.3;
;;;277            agv.Command.SetBaseSpeed = 0;
;;;278    
;;;279            TurnArriveFlag = 0;
;;;280    			
;;;281    				if(fabs(KincoStruct1.encodeSpeed) < 5 && fabs(KincoStruct2.encodeSpeed) < 5)
;;;282    				{
;;;283    					if(turnCount ++ >1000)
;;;284    					{
;;;285    						TurnStep = 1;
000008  2501              MOVS     r5,#1
00000a  4e8b              LDR      r6,|L1.568|
00000c  f8df822c          LDR      r8,|L1.572|
000010  f8dfa22c          LDR      r10,|L1.576|
000014  7830              LDRB     r0,[r6,#0]            ;272  ; TurnStep
000016  f8df922c          LDR      r9,|L1.580|
00001a  f8dfb22c          LDR      r11,|L1.584|
00001e  2700              MOVS     r7,#0
000020  ed9f8b8a          VLDR     d8,|L1.588|
000024  4c8b              LDR      r4,|L1.596|
000026  2800              CMP      r0,#0                 ;272
000028  d002              BEQ      |L1.48|
;;;286    						turnCount = 0;
;;;287    					}
;;;288    					
;;;289    				}
;;;290    
;;;291            
;;;292        }
;;;293        else if (TurnStep == 1) //开始转弯
00002a  2801              CMP      r0,#1
00002c  d057              BEQ      |L1.222|
00002e  e0e5              B        |L1.508|
                  |L1.48|
000030  4889              LDR      r0,|L1.600|
000032  edd02a02          VLDR     s5,[r0,#8]            ;274
000036  ed902a01          VLDR     s4,[r0,#4]            ;274
00003a  edd01a00          VLDR     s3,[r0,#0]            ;274
00003e  4887              LDR      r0,|L1.604|
000040  ed901a02          VLDR     s2,[r0,#8]            ;274
000044  edd00a01          VLDR     s1,[r0,#4]            ;274
000048  ed900a00          VLDR     s0,[r0,#0]            ;274
00004c  f7fffffe          BL       CalculatingDirectionAngle
000050  ed880a09          VSTR     s0,[r8,#0x24]         ;274
000054  ed9a1a02          VLDR     s2,[r10,#8]           ;276
000058  eef00a40          VMOV.F32 s1,s0                 ;276
00005c  eeb00a41          VMOV.F32 s0,s2                 ;276
000060  f7fffffe          BL       CalculatingCurrentAndTargetAngle
000064  ee100a10          VMOV     r0,s0                 ;276
000068  f7fffffe          BL       __aeabi_f2d
00006c  ec410b10          VMOV     d0,r0,r1              ;276
000070  f7fffffe          BL       __hardfp_fabs
000074  ed9f1b7a          VLDR     d1,|L1.608|
000078  ec510b10          VMOV     r0,r1,d0              ;276
00007c  ec532b11          VMOV     r2,r3,d1              ;276
000080  f7fffffe          BL       __aeabi_dmul
000084  f7fffffe          BL       __aeabi_d2f
000088  6220              STR      r0,[r4,#0x20]         ;276  ; agv
00008a  6027              STR      r7,[r4,#0]            ;277  ; agv
00008c  7077              STRB     r7,[r6,#1]            ;279
00008e  f8d90004          LDR      r0,[r9,#4]            ;281  ; KincoStruct1
000092  f7fffffe          BL       __aeabi_f2d
000096  ec410b10          VMOV     d0,r0,r1              ;281
00009a  f7fffffe          BL       __hardfp_fabs
00009e  ec532b18          VMOV     r2,r3,d8              ;281
0000a2  ec510b10          VMOV     r0,r1,d0              ;281
0000a6  f7fffffe          BL       __aeabi_cdcmple
0000aa  d217              BCS      |L1.220|
0000ac  f8db0004          LDR      r0,[r11,#4]           ;281  ; KincoStruct2
0000b0  f7fffffe          BL       __aeabi_f2d
0000b4  ec410b10          VMOV     d0,r0,r1              ;281
0000b8  f7fffffe          BL       __hardfp_fabs
0000bc  ec532b18          VMOV     r2,r3,d8              ;281
0000c0  ec510b10          VMOV     r0,r1,d0              ;281
0000c4  f7fffffe          BL       __aeabi_cdcmple
0000c8  d208              BCS      |L1.220|
0000ca  4967              LDR      r1,|L1.616|
0000cc  6808              LDR      r0,[r1,#0]            ;283  ; turnCount
0000ce  1c42              ADDS     r2,r0,#1              ;283
0000d0  600a              STR      r2,[r1,#0]            ;283  ; turnCount
0000d2  f5b07f7a          CMP      r0,#0x3e8             ;283
0000d6  dd01              BLE      |L1.220|
0000d8  7035              STRB     r5,[r6,#0]            ;285
0000da  600f              STR      r7,[r1,#0]            ;286  ; turnCount
                  |L1.220|
0000dc  e08e              B        |L1.508|
                  |L1.222|
;;;294        {
;;;295            if (agv.Command.CurDirection == 3 || agv.Command.CurDirection == 4) //前进左右转
0000de  7f20              LDRB     r0,[r4,#0x1c]  ; agv
0000e0  2803              CMP      r0,#3
0000e2  d001              BEQ      |L1.232|
0000e4  2804              CMP      r0,#4
0000e6  d11a              BNE      |L1.286|
                  |L1.232|
;;;296    				{
;;;297    					turnAngleDiff = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle, navi.Private.TarAngle);
0000e8  edd80a09          VLDR     s1,[r8,#0x24]
0000ec  ed9a0a02          VLDR     s0,[r10,#8]
0000f0  f7fffffe          BL       CalculatingCurrentAndTargetAngle
0000f4  ed860a08          VSTR     s0,[r6,#0x20]
;;;298    				
;;;299    					agvInAngle = fabs(CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle, navi.Private.TarAngle));
0000f8  edd80a09          VLDR     s1,[r8,#0x24]
0000fc  ed9a0a02          VLDR     s0,[r10,#8]
000100  f7fffffe          BL       CalculatingCurrentAndTargetAngle
000104  ee100a10          VMOV     r0,s0
000108  f7fffffe          BL       __aeabi_f2d
00010c  ec410b10          VMOV     d0,r0,r1
000110  f7fffffe          BL       __hardfp_fabs
000114  ec510b10          VMOV     r0,r1,d0
000118  f7fffffe          BL       __aeabi_d2f
00011c  61f0              STR      r0,[r6,#0x1c]  ; agvInAngle
                  |L1.286|
;;;300    				}
;;;301    //        else //后退左右转
;;;302    //				{
;;;303    //					turnAngleDiff = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle + PI, navi.Private.TarAngle);
;;;304    //					
;;;305    //					agvInAngle = fabs(CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle + PI, navi.Private.TarAngle));
;;;306    //				}
;;;307            if (agvInAngle >= PI * 2)
00011e  ed960a07          VLDR     s0,[r6,#0x1c]
000122  4952              LDR      r1,|L1.620|
000124  ee100a10          VMOV     r0,s0
000128  4288              CMP      r0,r1
00012a  db05              BLT      |L1.312|
;;;308    
;;;309                agvInAngle -= PI * 2;
00012c  eddf0a50          VLDR     s1,|L1.624|
000130  ee300a60          VSUB.F32 s0,s0,s1
000134  ed860a07          VSTR     s0,[r6,#0x1c]
                  |L1.312|
;;;310    
;;;311            if (agvInAngle >= ADVANCE_STOPSLOPE_ANGLE)
000138  ed960a07          VLDR     s0,[r6,#0x1c]
00013c  494d              LDR      r1,|L1.628|
00013e  ee100a10          VMOV     r0,s0
000142  4288              CMP      r0,r1
000144  db02              BLT      |L1.332|
;;;312            {
;;;313                agv.Command.SetBaseSpeed = TURNSPEED_MAX;
000146  2064              MOVS     r0,#0x64
000148  6020              STR      r0,[r4,#0]  ; agv
00014a  e027              B        |L1.412|
                  |L1.332|
;;;314            }
;;;315            else if ((agvInAngle < ADVANCE_STOPSLOPE_ANGLE) && (agvInAngle > TURNINGOFF_ANGLE))
00014c  ee100a10          VMOV     r0,s0
000150  4949              LDR      r1,|L1.632|
000152  4408              ADD      r0,r0,r1
000154  4949              LDR      r1,|L1.636|
000156  4288              CMP      r0,r1
000158  d20e              BCS      |L1.376|
;;;316            {
;;;317                agv.Command.SetBaseSpeed = (int)mapping(agvInAngle, TURNINGOFF_ANGLE, ADVANCE_STOPSLOPE_ANGLE, TURNSPEED_MIN, TURNSPEED_MAX);
00015a  ed9f2a49          VLDR     s4,|L1.640|
00015e  eef31a0e          VMOV.F32 s3,#30.00000000
000162  ed9f1a48          VLDR     s2,|L1.644|
000166  eddf0a48          VLDR     s1,|L1.648|
00016a  f7fffffe          BL       mapping
00016e  eebd0ac0          VCVT.S32.F32 s0,s0
000172  ed840a00          VSTR     s0,[r4,#0]
000176  e011              B        |L1.412|
                  |L1.376|
;;;318            }
;;;319            else if ((agvInAngle <= TURNINGOFF_ANGLE)||(turnAngleDiff*lastAngleDiff < 0))
000178  ee100a10          VMOV     r0,s0
00017c  493e              LDR      r1,|L1.632|
00017e  43c9              MVNS     r1,r1
000180  4288              CMP      r0,r1
000182  dd0a              BLE      |L1.410|
000184  ed960a08          VLDR     s0,[r6,#0x20]
000188  edd60a09          VLDR     s1,[r6,#0x24]
00018c  ee200a20          VMUL.F32 s0,s0,s1
000190  eeb50ac0          VCMPE.F32 s0,#0.0
000194  eef1fa10          VMRS     APSR_nzcv,FPSCR
000198  d200              BCS      |L1.412|
                  |L1.410|
;;;320            {
;;;321                TurnArriveFlag = 1;
00019a  7075              STRB     r5,[r6,#1]
                  |L1.412|
;;;322            }
;;;323    				lastAngleDiff = turnAngleDiff;
00019c  ed960a08          VLDR     s0,[r6,#0x20]
0001a0  ed860a09          VSTR     s0,[r6,#0x24]
;;;324            //出弯
;;;325            if (TurnArriveFlag == 1)
0001a4  7870              LDRB     r0,[r6,#1]  ; TurnArriveFlag
0001a6  2801              CMP      r0,#1
0001a8  d128              BNE      |L1.508|
;;;326            {
;;;327                //出弯
;;;328    					lastAngleDiff = 0;
0001aa  ed9f0a38          VLDR     s0,|L1.652|
0001ae  ed860a09          VSTR     s0,[r6,#0x24]
;;;329    					
;;;330    					agv.Command.SetBaseSpeed = 0;
0001b2  6027              STR      r7,[r4,#0]  ; agv
;;;331    
;;;332    					if (fabs(KincoStruct1.encodeSpeed) < 5 && fabs(KincoStruct2.encodeSpeed) < 5 && Rotate1.runState == 0)
0001b4  f8d90004          LDR      r0,[r9,#4]  ; KincoStruct1
0001b8  f7fffffe          BL       __aeabi_f2d
0001bc  ec410b10          VMOV     d0,r0,r1
0001c0  f7fffffe          BL       __hardfp_fabs
0001c4  ec532b18          VMOV     r2,r3,d8
0001c8  ec510b10          VMOV     r0,r1,d0
0001cc  f7fffffe          BL       __aeabi_cdcmple
0001d0  d211              BCS      |L1.502|
0001d2  f8db0004          LDR      r0,[r11,#4]  ; KincoStruct2
0001d6  f7fffffe          BL       __aeabi_f2d
0001da  ec410b10          VMOV     d0,r0,r1
0001de  f7fffffe          BL       __hardfp_fabs
0001e2  ec532b18          VMOV     r2,r3,d8
0001e6  ec510b10          VMOV     r0,r1,d0
0001ea  f7fffffe          BL       __aeabi_cdcmple
0001ee  d202              BCS      |L1.502|
0001f0  4827              LDR      r0,|L1.656|
0001f2  7d80              LDRB     r0,[r0,#0x16]  ; Rotate1
0001f4  b150              CBZ      r0,|L1.524|
                  |L1.502|
;;;333    					{
;;;334    							if (agv.Public.SystemTime - lastTime > 500)
;;;335    							{
;;;336    									if (agv.Command.CurDirection == 3 || agv.Command.CurDirection == 4)
;;;337    									{
;;;338    											traffic_land_marks.land_marks[agv.Command.standSiteID - 1].Direction = 1;
;;;339    
;;;340    											TurnStep = 0;
;;;341    									}
;;;342    //									else if (agv.Command.CurDirection == 5 || agv.Command.CurDirection == 6)
;;;343    //									{
;;;344    //											traffic_land_marks.land_marks[agv.Command.standSiteID - 1].Direction = 2;
;;;345    
;;;346    //											TurnStep = 0;
;;;347    //									}
;;;348    							}
;;;349    					}
;;;350    					else
;;;351    							lastTime = agv.Public.SystemTime;
0001f6  f8d400bc          LDR      r0,[r4,#0xbc]  ; agv
0001fa  62b0              STR      r0,[r6,#0x28]  ; lastTime
                  |L1.508|
;;;352            }
;;;353        }
;;;354        setMotorSpeedSlope(1.5);
0001fc  eeb70a08          VMOV.F32 s0,#1.50000000
000200  ecbd8b02          VPOP     {d8}
000204  e8bd5ff0          POP      {r4-r12,lr}
000208  f7ffbffe          B.W      setMotorSpeedSlope
                  |L1.524|
00020c  f8d400bc          LDR      r0,[r4,#0xbc]         ;334  ; agv
000210  6ab1              LDR      r1,[r6,#0x28]         ;334  ; lastTime
000212  1a40              SUBS     r0,r0,r1              ;334
000214  f5b07ffa          CMP      r0,#0x1f4             ;334
000218  d9f0              BLS      |L1.508|
00021a  7f20              LDRB     r0,[r4,#0x1c]         ;336  ; agv
00021c  2803              CMP      r0,#3                 ;336
00021e  d001              BEQ      |L1.548|
000220  2804              CMP      r0,#4                 ;336
000222  d1eb              BNE      |L1.508|
                  |L1.548|
000224  7da0              LDRB     r0,[r4,#0x16]         ;338  ; agv
000226  491b              LDR      r1,|L1.660|
000228  1e40              SUBS     r0,r0,#1              ;338
00022a  eb000040          ADD      r0,r0,r0,LSL #1       ;338
00022e  eb0100c0          ADD      r0,r1,r0,LSL #3       ;338
000232  8345              STRH     r5,[r0,#0x1a]         ;338
000234  7037              STRB     r7,[r6,#0]            ;340
000236  e7e1              B        |L1.508|
;;;355    }
;;;356    
                          ENDP

                  |L1.568|
                          DCD      ||.data||
                  |L1.572|
                          DCD      navi
                  |L1.576|
                          DCD      CurrentCenterPoint
                  |L1.580|
                          DCD      KincoStruct1
                  |L1.584|
                          DCD      KincoStruct2
                  |L1.588|
00024c  00000000          DCFD     0x4014000000000000 ; 5
000250  40140000
                  |L1.596|
                          DCD      agv
                  |L1.600|
                          DCD      TargetPoint
                  |L1.604|
                          DCD      StartPoint
                  |L1.608|
000260  66666666          DCFD     0x404ca66666666666 ; 57.299999999999997
000264  404ca666
                  |L1.616|
                          DCD      turnCount
                  |L1.620|
                          DCD      0x40c90fda
                  |L1.624|
000270  40c90fda          DCFS     0x40c90fda ; 6.2831850051879883
                  |L1.628|
                          DCD      0x3e99999a
                  |L1.632|
                          DCD      0xc30a3d70
                  |L1.636|
                          DCD      0x01a3d70a
                  |L1.640|
000280  42c80000          DCFS     0x42c80000 ; 100
                  |L1.644|
000284  3e99999a          DCFS     0x3e99999a ; 0.30000001192092896
                  |L1.648|
000288  3cf5c28f          DCFS     0x3cf5c28f ; 0.029999999329447746
                  |L1.652|
00028c  00000000          DCFS     0x00000000 ; 0
                  |L1.656|
                          DCD      Rotate1
                  |L1.660|
                          DCD      traffic_land_marks

                          AREA ||i.HardDifferentialSpeedRunTurnning||, CODE, READONLY, ALIGN=2

                  HardDifferentialSpeedRunTurnning PROC
;;;249    //硬差速弧线转弯
;;;250    void HardDifferentialSpeedRunTurnning()
000000  b570              PUSH     {r4-r6,lr}
;;;251    {
;;;252        if (fabs(navi.Public.AngleDifference) >= ADVANCE_STOPSLOPE_ANGLE) //如果agv角度差大于预停车角度差
;;;253        {
;;;254            agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed;
000002  4d20              LDR      r5,|L2.132|
000004  4c20              LDR      r4,|L2.136|
000006  6820              LDR      r0,[r4,#0]            ;252  ; navi
000008  f7fffffe          BL       __aeabi_f2d
00000c  ec410b10          VMOV     d0,r0,r1              ;252
000010  f7fffffe          BL       __hardfp_fabs
000014  ed9f1b1d          VLDR     d1,|L2.140|
000018  ec510b10          VMOV     r0,r1,d0              ;252
00001c  ec532b11          VMOV     r2,r3,d1              ;252
000020  f7fffffe          BL       __aeabi_cdrcmple
000024  d808              BHI      |L2.56|
000026  f9b50018          LDRSH    r0,[r5,#0x18]  ; agv
00002a  6028              STR      r0,[r5,#0]  ; agv
;;;255            setMotorSpeedSlope(0.5);
00002c  e8bd4070          POP      {r4-r6,lr}
000030  eeb60a00          VMOV.F32 s0,#0.50000000
000034  f7ffbffe          B.W      setMotorSpeedSlope
                  |L2.56|
;;;256        }
;;;257        else //如果agv角度差小于预停车角度差,开始减速至设定出弯速度
;;;258        {
;;;259            agv.Command.SetBaseSpeed = mapping(fabs(navi.Public.AngleDifference), 0, ADVANCE_STOPSLOPE_ANGLE, ARC_TURNSPEED_MIN, agv.Command.DispatchSpeed);
000038  6820              LDR      r0,[r4,#0]  ; navi
00003a  f7fffffe          BL       __aeabi_f2d
00003e  ec410b10          VMOV     d0,r0,r1
000042  f7fffffe          BL       __hardfp_fabs
000046  ec510b10          VMOV     r0,r1,d0
00004a  f7fffffe          BL       __aeabi_d2f
00004e  ee000a10          VMOV     s0,r0
000052  f9b50018          LDRSH    r0,[r5,#0x18]  ; agv
000056  eddf1a0f          VLDR     s3,|L2.148|
00005a  ee000a90          VMOV     s1,r0
00005e  ed9f1a0e          VLDR     s2,|L2.152|
000062  eeb82ae0          VCVT.F32.S32 s4,s1
000066  eddf0a0d          VLDR     s1,|L2.156|
00006a  f7fffffe          BL       mapping
00006e  eebd0ac0          VCVT.S32.F32 s0,s0
000072  ed850a00          VSTR     s0,[r5,#0]
;;;260            setMotorSpeedSlope(0.2);
000076  e8bd4070          POP      {r4-r6,lr}
00007a  ed9f0a09          VLDR     s0,|L2.160|
00007e  f7ffbffe          B.W      setMotorSpeedSlope
;;;261        }
;;;262    }
;;;263    
                          ENDP

000082  0000              DCW      0x0000
                  |L2.132|
                          DCD      agv
                  |L2.136|
                          DCD      navi
                  |L2.140|
00008c  33333333          DCFD     0x3fd3333333333333 ; 0.29999999999999999
000090  3fd33333
                  |L2.148|
000094  42c80000          DCFS     0x42c80000 ; 100
                  |L2.152|
000098  3e99999a          DCFS     0x3e99999a ; 0.30000001192092896
                  |L2.156|
00009c  00000000          DCFS     0x00000000 ; 0
                  |L2.160|
0000a0  3e4ccccd          DCFS     0x3e4ccccd ; 0.20000000298023224

                          AREA ||i.HardDifferentialSpeedStraight||, CODE, READONLY, ALIGN=2

                  HardDifferentialSpeedStraight PROC
;;;200    //硬差速直行
;;;201    void HardDifferentialSpeedStraight()
000000  b570              PUSH     {r4-r6,lr}
;;;202    {
000002  ed2d8b04          VPUSH    {d8-d9}
;;;203    	 float DIS = TwoPointDistance(StartPoint, TargetPoint);
000006  4881              LDR      r0,|L3.524|
000008  edd02a02          VLDR     s5,[r0,#8]
00000c  ed902a01          VLDR     s4,[r0,#4]
000010  edd01a00          VLDR     s3,[r0,#0]
000014  487e              LDR      r0,|L3.528|
000016  ed901a02          VLDR     s2,[r0,#8]
00001a  edd00a01          VLDR     s1,[r0,#4]
00001e  ed900a00          VLDR     s0,[r0,#0]
000022  f7fffffe          BL       TwoPointDistance
000026  eeb08a40          VMOV.F32 s16,s0
;;;204    	if(DIS > 10000)
00002a  ee180a10          VMOV     r0,s16
00002e  4979              LDR      r1,|L3.532|
;;;205    		navi.Public.DistanceAgvToTARGET = DIS - fabs(navi.Public.DistanceAgvToSTART) - 400;
000030  4c79              LDR      r4,|L3.536|
000032  4288              CMP      r0,r1                 ;204
000034  dd1c              BLE      |L3.112|
000036  68e0              LDR      r0,[r4,#0xc]  ; navi
000038  f7fffffe          BL       __aeabi_f2d
00003c  ec410b10          VMOV     d0,r0,r1
000040  f7fffffe          BL       __hardfp_fabs
000044  eeb09a40          VMOV.F32 s18,s0
000048  eef09a60          VMOV.F32 s19,s1
00004c  ee180a10          VMOV     r0,s16
000050  f7fffffe          BL       __aeabi_f2d
000054  ec532b19          VMOV     r2,r3,d9
000058  f7fffffe          BL       __aeabi_dsub
00005c  ed9f1b6f          VLDR     d1,|L3.540|
000060  ec532b11          VMOV     r2,r3,d1
000064  f7fffffe          BL       __aeabi_dsub
000068  f7fffffe          BL       __aeabi_d2f
00006c  6120              STR      r0,[r4,#0x10]  ; navi
00006e  e015              B        |L3.156|
                  |L3.112|
;;;206    	else
;;;207    		navi.Public.DistanceAgvToTARGET = DIS - fabs(navi.Public.DistanceAgvToSTART);
000070  68e0              LDR      r0,[r4,#0xc]  ; navi
000072  f7fffffe          BL       __aeabi_f2d
000076  ec410b10          VMOV     d0,r0,r1
00007a  f7fffffe          BL       __hardfp_fabs
00007e  eeb09a40          VMOV.F32 s18,s0
000082  eef09a60          VMOV.F32 s19,s1
000086  ee180a10          VMOV     r0,s16
00008a  f7fffffe          BL       __aeabi_f2d
00008e  ec532b19          VMOV     r2,r3,d9
000092  f7fffffe          BL       __aeabi_dsub
000096  f7fffffe          BL       __aeabi_d2f
00009a  6120              STR      r0,[r4,#0x10]  ; navi
                  |L3.156|
;;;208    	
;;;209        if (agv.Command.CurDirection == agv.Command.NextDirection) //如果当前方向和下次方向相同
00009c  4d61              LDR      r5,|L3.548|
00009e  7f29              LDRB     r1,[r5,#0x1c]  ; agv
0000a0  7f68              LDRB     r0,[r5,#0x1d]  ; agv
0000a2  4281              CMP      r1,r0
0000a4  d103              BNE      |L3.174|
;;;210        {
;;;211            agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed; //设定速度为系统下发速度
0000a6  f9b50018          LDRSH    r0,[r5,#0x18]  ; agv
0000aa  6028              STR      r0,[r5,#0]  ; agv
0000ac  e068              B        |L3.384|
                  |L3.174|
;;;212        }                                                         //如果记下来是弧线转弯,做减速处理
;;;213        else if (agv.Command.NextDirection == 7 || agv.Command.NextDirection == 8 || agv.Command.NextDirection == 9 || agv.Command.NextDirection == 10)
0000ae  2807              CMP      r0,#7
0000b0  d015              BEQ      |L3.222|
0000b2  2808              CMP      r0,#8
0000b4  d013              BEQ      |L3.222|
0000b6  2809              CMP      r0,#9
0000b8  d011              BEQ      |L3.222|
0000ba  280a              CMP      r0,#0xa
0000bc  d00f              BEQ      |L3.222|
;;;214        {
;;;215            if (navi.Public.DistanceAgvToTARGET >= agv.Parameter.AdvanceParkPos) //agv到终点距离大于预停车距离
;;;216            {
;;;217                agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed; //按系统下发速度走
;;;218            }
;;;219            else //agv到终点距离小于预停车距离,减速到设定自动转弯速度
;;;220            {
;;;221                agv.Command.SetBaseSpeed = mapping(navi.Public.DistanceAgvToTARGET, 0, agv.Parameter.AdvanceParkPos, agv.Command.NextDispatchSpeed, agv.Command.DispatchSpeed);
;;;222            }
;;;223        }
;;;224        else //停车或者原地转
;;;225        {
;;;226            if (navi.Public.DistanceAgvToTARGET >= agv.Parameter.AdvanceParkPos) //如果agv到目标点距离大于预停车距离
0000be  8f28              LDRH     r0,[r5,#0x38]  ; agv
0000c0  ed940a04          VLDR     s0,[r4,#0x10]
0000c4  ee000a90          VMOV     s1,r0
0000c8  eef80a60          VCVT.F32.U32 s1,s1
0000cc  eeb40ae0          VCMPE.F32 s0,s1
0000d0  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000d4  db2c              BLT      |L3.304|
;;;227            {
;;;228                agv.Command.SetBaseSpeed = agv.Command.DispatchSpeed; //设定速度等于系统下发速度
0000d6  f9b50018          LDRSH    r0,[r5,#0x18]  ; agv
0000da  6028              STR      r0,[r5,#0]  ; agv
0000dc  e050              B        |L3.384|
                  |L3.222|
0000de  8f28              LDRH     r0,[r5,#0x38]         ;215  ; agv
0000e0  ed940a04          VLDR     s0,[r4,#0x10]         ;215
0000e4  ee000a90          VMOV     s1,r0                 ;215
0000e8  eef80a60          VCVT.F32.U32 s1,s1                 ;215
0000ec  eeb40ae0          VCMPE.F32 s0,s1                 ;215
0000f0  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;215
0000f4  db03              BLT      |L3.254|
0000f6  f9b50018          LDRSH    r0,[r5,#0x18]         ;217  ; agv
0000fa  6028              STR      r0,[r5,#0]            ;217  ; agv
0000fc  e040              B        |L3.384|
                  |L3.254|
0000fe  f9b51018          LDRSH    r1,[r5,#0x18]         ;221  ; agv
000102  ee001a90          VMOV     s1,r1                 ;221
000106  f9b5101a          LDRSH    r1,[r5,#0x1a]         ;221  ; agv
00010a  eeb82ae0          VCVT.F32.S32 s4,s1                 ;221
00010e  ee001a90          VMOV     s1,r1                 ;221
000112  eef81ae0          VCVT.F32.S32 s3,s1                 ;221
000116  ee000a90          VMOV     s1,r0                 ;221
00011a  eeb81a60          VCVT.F32.U32 s2,s1                 ;221
00011e  eddf0a42          VLDR     s1,|L3.552|
000122  f7fffffe          BL       mapping
000126  eebd0ac0          VCVT.S32.F32 s0,s0                 ;221
00012a  ed850a00          VSTR     s0,[r5,#0]            ;221
00012e  e027              B        |L3.384|
                  |L3.304|
;;;229            }
;;;230            else if (navi.Public.DistanceAgvToTARGET <= agv.Parameter.AdvanceParkPos && navi.Public.DistanceAgvToTARGET > LIGHT_STOP_SIGNAL_DIS ) //小于预停车距离,大于到点判断范围10MM,驱动器不能给太小速度,不能直接到0,设置了预停车速度
000130  ee000a90          VMOV     s1,r0
000134  eef80a60          VCVT.F32.U32 s1,s1
000138  eeb40ae0          VCMPE.F32 s0,s1
00013c  eef1fa10          VMRS     APSR_nzcv,FPSCR
000140  d81c              BHI      |L3.380|
000142  ee101a10          VMOV     r1,s0
000146  4a39              LDR      r2,|L3.556|
000148  4291              CMP      r1,r2
00014a  dd17              BLE      |L3.380|
;;;231            {
;;;232                agv.Command.SetBaseSpeed = mapping(navi.Public.DistanceAgvToTARGET, LIGHT_STOP_SIGNAL_DIS, agv.Parameter.AdvanceParkPos, agv.Parameter.AdvanceParkSpeed, agv.Command.DispatchSpeed);
00014c  f9b51018          LDRSH    r1,[r5,#0x18]  ; agv
000150  ee001a90          VMOV     s1,r1
000154  8ee9              LDRH     r1,[r5,#0x36]  ; agv
000156  eeb82ae0          VCVT.F32.S32 s4,s1
00015a  ee001a90          VMOV     s1,r1
00015e  eef81a60          VCVT.F32.U32 s3,s1
000162  ee000a90          VMOV     s1,r0
000166  eeb81a60          VCVT.F32.U32 s2,s1
00016a  eef30a0e          VMOV.F32 s1,#30.00000000
00016e  f7fffffe          BL       mapping
000172  eebd0ac0          VCVT.S32.F32 s0,s0
000176  ed850a00          VSTR     s0,[r5,#0]
00017a  e001              B        |L3.384|
                  |L3.380|
;;;233            }
;;;234            else
;;;235                agv.Command.SetBaseSpeed = agv.Parameter.AdvanceParkSpeed;
00017c  8ee8              LDRH     r0,[r5,#0x36]  ; agv
00017e  6028              STR      r0,[r5,#0]  ; agv
                  |L3.384|
;;;236        }
;;;237        if (navi.Public.DistanceAgvToSTART < START_SPEEDUP_RANGE)
000180  492b              LDR      r1,|L3.560|
;;;238        {
;;;239            if (fabs(KincoStruct1.encodeSpeed - KincoStruct2.encodeSpeed) > START_SPEEDDIFF_LIMIT) //速度差太大
000182  68e0              LDR      r0,[r4,#0xc]  ; navi
000184  4e2b              LDR      r6,|L3.564|
000186  4c2c              LDR      r4,|L3.568|
000188  4288              CMP      r0,r1                 ;237
00018a  da37              BGE      |L3.508|
00018c  ed960a01          VLDR     s0,[r6,#4]
000190  edd40a01          VLDR     s1,[r4,#4]
000194  ee300a60          VSUB.F32 s0,s0,s1
000198  ee100a10          VMOV     r0,s0
00019c  f7fffffe          BL       __aeabi_f2d
0001a0  ec410b10          VMOV     d0,r0,r1
0001a4  f7fffffe          BL       __hardfp_fabs
0001a8  ed9f1b24          VLDR     d1,|L3.572|
0001ac  ec510b10          VMOV     r0,r1,d0
0001b0  ec532b11          VMOV     r2,r3,d1
0001b4  f7fffffe          BL       __aeabi_cdrcmple
0001b8  d218              BCS      |L3.492|
;;;240            {
;;;241                agv.Command.SetBaseSpeed = fabs(KincoStruct1.encodeSpeed + KincoStruct2.encodeSpeed) / 2;
0001ba  ed960a01          VLDR     s0,[r6,#4]
0001be  edd40a01          VLDR     s1,[r4,#4]
0001c2  ee300a20          VADD.F32 s0,s0,s1
0001c6  ee100a10          VMOV     r0,s0
0001ca  f7fffffe          BL       __aeabi_f2d
0001ce  ec410b10          VMOV     d0,r0,r1
0001d2  f7fffffe          BL       __hardfp_fabs
0001d6  ed9f1b1b          VLDR     d1,|L3.580|
0001da  ec510b10          VMOV     r0,r1,d0
0001de  ec532b11          VMOV     r2,r3,d1
0001e2  f7fffffe          BL       __aeabi_dmul
0001e6  f7fffffe          BL       __aeabi_d2iz
0001ea  6028              STR      r0,[r5,#0]  ; agv
                  |L3.492|
;;;242            }
;;;243            setMotorSpeedSlope(0.3);
0001ec  ed9f0a17          VLDR     s0,|L3.588|
0001f0  ecbd8b04          VPOP     {d8-d9}
0001f4  e8bd4070          POP      {r4-r6,lr}
0001f8  f7ffbffe          B.W      setMotorSpeedSlope
                  |L3.508|
;;;244        }
;;;245        else
;;;246            setMotorSpeedSlope(1.5);
0001fc  ecbd8b04          VPOP     {d8-d9}
000200  eeb70a08          VMOV.F32 s0,#1.50000000
000204  e8bd4070          POP      {r4-r6,lr}
000208  f7ffbffe          B.W      setMotorSpeedSlope
;;;247    }
;;;248    
                          ENDP

                  |L3.524|
                          DCD      TargetPoint
                  |L3.528|
                          DCD      StartPoint
                  |L3.532|
                          DCD      0x461c4000
                  |L3.536|
                          DCD      navi
                  |L3.540|
00021c  00000000          DCFD     0x4079000000000000 ; 400
000220  40790000
                  |L3.548|
                          DCD      agv
                  |L3.552|
000228  00000000          DCFS     0x00000000 ; 0
                  |L3.556|
                          DCD      0x41f00000
                  |L3.560|
                          DCD      0x43960000
                  |L3.564|
                          DCD      KincoStruct1
                  |L3.568|
                          DCD      KincoStruct2
                  |L3.572|
00023c  00000000          DCFD     0x4034000000000000 ; 20
000240  40340000
                  |L3.580|
000244  00000000          DCFD     0x3fe0000000000000 ; 0.5
000248  3fe00000
                  |L3.588|
00024c  3e99999a          DCFS     0x3e99999a ; 0.30000001192092896

                          AREA ||i.chassisControlAuto||, CODE, READONLY, ALIGN=2

                  chassisControlAuto PROC
;;;100    float SpeedDifValue;//根据转弯半径大小,计算固定速度差值
;;;101    void chassisControlAuto(void)
000000  b530              PUSH     {r4,r5,lr}
;;;102    {
;;;103        //根据转弯半径大小,计算固定速度差值
;;;104        SpeedDifValue = P_SETUP_WHEEL_Y_DIS * ((float)agv.Command.SetBaseSpeed) / (navi.Private.SetCalculationRadius) / 2;
000002  4978              LDR      r1,|L4.484|
000004  6808              LDR      r0,[r1,#0]  ; agv
000006  ee000a10          VMOV     s0,r0
00000a  eeb80ac0          VCVT.F32.S32 s0,s0
00000e  eddf0a76          VLDR     s1,|L4.488|
000012  4a76              LDR      r2,|L4.492|
000014  ee600a20          VMUL.F32 s1,s0,s1
000018  ed921a08          VLDR     s2,[r2,#0x20]
00001c  4a74              LDR      r2,|L4.496|
00001e  ee800a81          VDIV.F32 s0,s1,s2
000022  eef60a00          VMOV.F32 s1,#0.50000000
000026  ee600a20          VMUL.F32 s1,s0,s1
00002a  edc20a0b          VSTR     s1,[r2,#0x2c]
;;;105    
;;;106        switch (agv.Command.CurDirection)
00002e  7f0c              LDRB     r4,[r1,#0x1c]  ; agv
;;;107        {
;;;108        case 0:
;;;109            KincoStruct1.setSpeed = 0;
000030  ed9f1a70          VLDR     s2,|L4.500|
000034  4d6d              LDR      r5,|L4.492|
;;;110    
;;;111            KincoStruct2.setSpeed = 0;
;;;112            break;
;;;113    
;;;114        case 1:
;;;115            if (agv.Command.SetBaseSpeed == 0)
;;;116            {
;;;117                KincoStruct1.setSpeed = 0;
;;;118    
;;;119                KincoStruct2.setSpeed = 0;
;;;120            }
;;;121            else
;;;122            {
;;;123                KincoStruct1.setSpeed = agv.Command.SetBaseSpeed + navi.Private.OutputOffset; //设定速度加补偿值
;;;124    
;;;125                KincoStruct2.setSpeed = agv.Command.SetBaseSpeed - navi.Private.OutputOffset; //设定速度减补偿值
;;;126            }
;;;127            break;
;;;128    
;;;129        case 2:
;;;130            if (agv.Command.SetBaseSpeed == 0)
;;;131            {
;;;132                KincoStruct1.setSpeed = 0;
;;;133    
;;;134                KincoStruct2.setSpeed = 0;
;;;135            }
;;;136            else
;;;137            {
;;;138                KincoStruct1.setSpeed = -(agv.Command.SetBaseSpeed - navi.Private.OutputOffset); //设定速度加补偿值,后退要给负速度
;;;139    
;;;140                KincoStruct2.setSpeed = -(agv.Command.SetBaseSpeed + navi.Private.OutputOffset); //设定速度减补偿值,后退要给负速度
;;;141            }
;;;142            break;
;;;143    
;;;144        case 3:
;;;145            KincoStruct1.setSpeed = -agv.Command.SetBaseSpeed; //前进原地左转,左轮给负速度,以当前行进方向分左右
000036  4243              RSBS     r3,r0,#0
000038  496f              LDR      r1,|L4.504|
00003a  4a70              LDR      r2,|L4.508|
00003c  ed950a0a          VLDR     s0,[r5,#0x28]         ;123
000040  2c0b              CMP      r4,#0xb               ;106
000042  d270              BCS      |L4.294|
000044  e8dff004          TBB      [pc,r4]               ;106
000048  060b223b          DCB      0x06,0x0b,0x22,0x3b
00004c  48556270          DCB      0x48,0x55,0x62,0x70
000050  859ab100          DCB      0x85,0x9a,0xb1,0x00
000054  ed811a00          VSTR     s2,[r1,#0]            ;109
000058  ed821a00          VSTR     s2,[r2,#0]            ;111
;;;146    
;;;147            KincoStruct2.setSpeed = agv.Command.SetBaseSpeed; //前进原地左转,右轮给正速度,以当前行进方向分左右
;;;148            break;
;;;149    
;;;150        case 4:
;;;151            KincoStruct1.setSpeed = agv.Command.SetBaseSpeed; //前进原地右转,左轮给正速度,以当前行进方向分左右
;;;152    
;;;153            KincoStruct2.setSpeed = -agv.Command.SetBaseSpeed; //前进原地右转,右轮给负速度,以当前行进方向分左右
;;;154            break;
;;;155    
;;;156        case 5:
;;;157            KincoStruct1.setSpeed = agv.Command.SetBaseSpeed; //后退原地左转,左轮给正速度,以当前行进方向分左右
;;;158    
;;;159            KincoStruct2.setSpeed = -agv.Command.SetBaseSpeed; //后退原地左转,右轮给负速度,以当前行进方向分左右
;;;160            break;
;;;161    
;;;162        case 6:
;;;163            KincoStruct1.setSpeed = -agv.Command.SetBaseSpeed; //后退原地右转,左轮给负速度,以当前行进方向分左右
;;;164    
;;;165            KincoStruct2.setSpeed = agv.Command.SetBaseSpeed; //后退原地右转,右轮给正速度,以当前行进方向分左右
;;;166            break;
;;;167    
;;;168        case 7: //前进弧线左转,左轮设定速度-固定速度差-补偿值
;;;169            KincoStruct1.setSpeed = agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset;
;;;170            //前进弧线左转,右轮设定速度+固定速度差+补偿值
;;;171            KincoStruct2.setSpeed = agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset;
;;;172            break;
;;;173    
;;;174        case 8: //前进弧线右转,左轮设定速度+固定速度差+补偿值
;;;175            KincoStruct1.setSpeed = agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset;
;;;176            //前进弧线右转,左轮设定速度-固定速度差-补偿值
;;;177            KincoStruct2.setSpeed = agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset;
;;;178            break;
;;;179    
;;;180        case 9: //后退弧线左转,左轮设定速度+固定速度差-补偿值,给负速度
;;;181            KincoStruct1.setSpeed = -(agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset);
;;;182            //后退弧线左转,右轮设定速度-固定速度差+补偿值,给负速度
;;;183            KincoStruct2.setSpeed = -(agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset);
;;;184            break;
;;;185    
;;;186        case 10: //后退弧线右转,左轮设定速度-固定速度差+补偿值,给负速度
;;;187            KincoStruct1.setSpeed = -(agv.Command.SetBaseSpeed - SpeedDifValue - navi.Private.OutputOffset);
;;;188            //后退弧线右转,左轮设定速度+固定速度差-补偿值,给负速度
;;;189            KincoStruct2.setSpeed = -(agv.Command.SetBaseSpeed + SpeedDifValue + navi.Private.OutputOffset);
;;;190            break;
;;;191    
;;;192        default:
;;;193            KincoStruct1.setSpeed = 0;
;;;194    
;;;195            KincoStruct2.setSpeed = 0;
;;;196            break;
;;;197        }
;;;198    }
00005c  bd30              POP      {r4,r5,pc}
00005e  b180              CBZ      r0,|L4.130|
000060  ee000a90          VMOV     s1,r0                 ;123
000064  eef80ae0          VCVT.F32.S32 s1,s1                 ;123
000068  ee700a80          VADD.F32 s1,s1,s0              ;123
00006c  edc10a00          VSTR     s1,[r1,#0]            ;123
000070  ee000a90          VMOV     s1,r0                 ;125
000074  eef80ae0          VCVT.F32.S32 s1,s1                 ;125
000078  ee300ac0          VSUB.F32 s0,s1,s0              ;125
00007c  ed820a00          VSTR     s0,[r2,#0]            ;125
000080  bd30              POP      {r4,r5,pc}
                  |L4.130|
000082  ed811a00          VSTR     s2,[r1,#0]            ;117
000086  ed821a00          VSTR     s2,[r2,#0]            ;119
00008a  bd30              POP      {r4,r5,pc}
00008c  b190              CBZ      r0,|L4.180|
00008e  ee000a90          VMOV     s1,r0                 ;138
000092  eef80ae0          VCVT.F32.S32 s1,s1                 ;138
000096  ee700a60          VSUB.F32 s1,s0,s1              ;138
00009a  edc10a00          VSTR     s1,[r1,#0]            ;138
00009e  ee000a90          VMOV     s1,r0                 ;140
0000a2  eef80ae0          VCVT.F32.S32 s1,s1                 ;140
0000a6  ee300a80          VADD.F32 s0,s1,s0              ;140
0000aa  eeb10a40          VNEG.F32 s0,s0                 ;140
0000ae  ed820a00          VSTR     s0,[r2,#0]            ;140
0000b2  bd30              POP      {r4,r5,pc}
                  |L4.180|
0000b4  ed811a00          VSTR     s2,[r1,#0]            ;132
0000b8  ed821a00          VSTR     s2,[r2,#0]            ;134
0000bc  bd30              POP      {r4,r5,pc}
0000be  ee003a10          VMOV     s0,r3                 ;145
0000c2  eeb80ac0          VCVT.F32.S32 s0,s0                 ;145
0000c6  ed810a00          VSTR     s0,[r1,#0]            ;145
0000ca  ee000a10          VMOV     s0,r0                 ;147
0000ce  eeb80ac0          VCVT.F32.S32 s0,s0                 ;147
0000d2  ed820a00          VSTR     s0,[r2,#0]            ;147
0000d6  bd30              POP      {r4,r5,pc}
0000d8  ee000a10          VMOV     s0,r0                 ;151
0000dc  eeb80ac0          VCVT.F32.S32 s0,s0                 ;151
0000e0  ed810a00          VSTR     s0,[r1,#0]            ;151
0000e4  ee003a10          VMOV     s0,r3                 ;153
0000e8  eeb80ac0          VCVT.F32.S32 s0,s0                 ;153
0000ec  ed820a00          VSTR     s0,[r2,#0]            ;153
0000f0  bd30              POP      {r4,r5,pc}
0000f2  ee000a10          VMOV     s0,r0                 ;157
0000f6  eeb80ac0          VCVT.F32.S32 s0,s0                 ;157
0000fa  ed810a00          VSTR     s0,[r1,#0]            ;157
0000fe  ee003a10          VMOV     s0,r3                 ;159
000102  eeb80ac0          VCVT.F32.S32 s0,s0                 ;159
000106  ed820a00          VSTR     s0,[r2,#0]            ;159
00010a  bd30              POP      {r4,r5,pc}
00010c  ee003a10          VMOV     s0,r3                 ;163
000110  eeb80ac0          VCVT.F32.S32 s0,s0                 ;163
000114  ed810a00          VSTR     s0,[r1,#0]            ;163
000118  ee000a10          VMOV     s0,r0                 ;165
00011c  eeb80ac0          VCVT.F32.S32 s0,s0                 ;165
000120  ed820a00          VSTR     s0,[r2,#0]            ;165
                  |L4.292|
000124  bd30              POP      {r4,r5,pc}
                  |L4.294|
000126  e057              B        |L4.472|
000128  ee010a10          VMOV     s2,r0                 ;169
00012c  eeb81ac1          VCVT.F32.S32 s2,s2                 ;169
000130  ee311a60          VSUB.F32 s2,s2,s1              ;169
000134  ee311a40          VSUB.F32 s2,s2,s0              ;169
000138  ed811a00          VSTR     s2,[r1,#0]            ;169
00013c  ee010a10          VMOV     s2,r0                 ;171
000140  eeb81ac1          VCVT.F32.S32 s2,s2                 ;171
000144  ee710a20          VADD.F32 s1,s2,s1              ;171
000148  ee300a80          VADD.F32 s0,s1,s0              ;171
00014c  ed820a00          VSTR     s0,[r2,#0]            ;171
000150  e7e8              B        |L4.292|
000152  ee010a10          VMOV     s2,r0                 ;175
000156  eeb81ac1          VCVT.F32.S32 s2,s2                 ;175
00015a  ee311a20          VADD.F32 s2,s2,s1              ;175
00015e  ee311a00          VADD.F32 s2,s2,s0              ;175
000162  ed811a00          VSTR     s2,[r1,#0]            ;175
000166  ee010a10          VMOV     s2,r0                 ;177
00016a  eeb81ac1          VCVT.F32.S32 s2,s2                 ;177
00016e  ee710a60          VSUB.F32 s1,s2,s1              ;177
000172  ee300ac0          VSUB.F32 s0,s1,s0              ;177
000176  ed820a00          VSTR     s0,[r2,#0]            ;177
00017a  e7d3              B        |L4.292|
00017c  ee010a10          VMOV     s2,r0                 ;181
000180  eeb81ac1          VCVT.F32.S32 s2,s2                 ;181
000184  ee311a20          VADD.F32 s2,s2,s1              ;181
000188  ee311a00          VADD.F32 s2,s2,s0              ;181
00018c  eeb11a41          VNEG.F32 s2,s2                 ;181
000190  ed811a00          VSTR     s2,[r1,#0]            ;181
000194  ee010a10          VMOV     s2,r0                 ;183
000198  eeb81ac1          VCVT.F32.S32 s2,s2                 ;183
00019c  ee710a60          VSUB.F32 s1,s2,s1              ;183
0001a0  ee300a60          VSUB.F32 s0,s0,s1              ;183
0001a4  ed820a00          VSTR     s0,[r2,#0]            ;183
0001a8  e7bc              B        |L4.292|
0001aa  ee010a10          VMOV     s2,r0                 ;187
0001ae  eeb81ac1          VCVT.F32.S32 s2,s2                 ;187
0001b2  ee311a60          VSUB.F32 s2,s2,s1              ;187
0001b6  ee301a41          VSUB.F32 s2,s0,s2              ;187
0001ba  ed811a00          VSTR     s2,[r1,#0]            ;187
0001be  ee010a10          VMOV     s2,r0                 ;189
0001c2  eeb81ac1          VCVT.F32.S32 s2,s2                 ;189
0001c6  ee710a20          VADD.F32 s1,s2,s1              ;189
0001ca  ee300a80          VADD.F32 s0,s1,s0              ;189
0001ce  eeb10a40          VNEG.F32 s0,s0                 ;189
0001d2  ed820a00          VSTR     s0,[r2,#0]            ;189
0001d6  e7a5              B        |L4.292|
                  |L4.472|
0001d8  ed811a00          VSTR     s2,[r1,#0]            ;193
0001dc  ed821a00          VSTR     s2,[r2,#0]            ;195
0001e0  e7a0              B        |L4.292|
;;;199    
                          ENDP

0001e2  0000              DCW      0x0000
                  |L4.484|
                          DCD      agv
                  |L4.488|
0001e8  43b90000          DCFS     0x43b90000 ; 370
                  |L4.492|
                          DCD      navi
                  |L4.496|
                          DCD      ||.data||
                  |L4.500|
0001f4  00000000          DCFS     0x00000000 ; 0
                  |L4.504|
                          DCD      KincoStruct1
                  |L4.508|
                          DCD      KincoStruct2

                          AREA ||i.chassisControlManual||, CODE, READONLY, ALIGN=2

                  chassisControlManual PROC
;;;4      float arriveAngle = 0.015;
;;;5      void chassisControlManual(void)
000000  e92d41f0          PUSH     {r4-r8,lr}
;;;6      {
000004  ed2d8b04          VPUSH    {d8-d9}
;;;7      	static int i = 0;
;;;8      	static float lastAngle = 0,diffAngle = 0,alreadyTurnAngle = 0;
;;;9      	static float setSpeed = 0,lastSetSpeed = 0; 
;;;10     	alreadyTurnAngle = CurrentCenterPoint.CurAngle - lastAngle;
000008  488f              LDR      r0,|L5.584|
00000a  4c90              LDR      r4,|L5.588|
00000c  ed909a02          VLDR     s18,[r0,#8]
000010  ed940a02          VLDR     s0,[r4,#8]
000014  ee390a40          VSUB.F32 s0,s18,s0
000018  ed840a04          VSTR     s0,[r4,#0x10]
;;;11     	if(alreadyTurnAngle > PI)
00001c  ee100a10          VMOV     r0,s0
000020  498b              LDR      r1,|L5.592|
;;;12     			alreadyTurnAngle -= 2*PI;
000022  eddf0a8c          VLDR     s1,|L5.596|
000026  4288              CMP      r0,r1                 ;11
000028  dd03              BLE      |L5.50|
00002a  ee300a60          VSUB.F32 s0,s0,s1
00002e  ed840a04          VSTR     s0,[r4,#0x10]
                  |L5.50|
;;;13     		if(alreadyTurnAngle < -PI)
000032  ed940a04          VLDR     s0,[r4,#0x10]
000036  4988              LDR      r1,|L5.600|
000038  ee100a10          VMOV     r0,s0
00003c  4288              CMP      r0,r1
00003e  d903              BLS      |L5.72|
;;;14     			alreadyTurnAngle += 2*PI;
000040  ee300a20          VADD.F32 s0,s0,s1
000044  ed840a04          VSTR     s0,[r4,#0x10]
                  |L5.72|
;;;15     	diffAngle = fabs(fabs(alreadyTurnAngle) - agv.Command.SetChassisTurnAngle);//fabs(CurrentCenterPoint.CurAngle - lastAngle);
000048  4f84              LDR      r7,|L5.604|
00004a  6a78              LDR      r0,[r7,#0x24]  ; agv
00004c  f7fffffe          BL       __aeabi_f2d
000050  ec410b18          VMOV     d8,r0,r1
000054  6920              LDR      r0,[r4,#0x10]  ; alreadyTurnAngle
000056  f7fffffe          BL       __aeabi_f2d
00005a  ec410b10          VMOV     d0,r0,r1
00005e  f7fffffe          BL       __hardfp_fabs
000062  ec532b18          VMOV     r2,r3,d8
000066  ec510b10          VMOV     r0,r1,d0
00006a  f7fffffe          BL       __aeabi_dsub
00006e  ec410b10          VMOV     d0,r0,r1
000072  f7fffffe          BL       __hardfp_fabs
000076  ec510b10          VMOV     r0,r1,d0
00007a  f7fffffe          BL       __aeabi_d2f
00007e  ee000a10          VMOV     s0,r0
000082  ed840a03          VSTR     s0,[r4,#0xc]
;;;16     
;;;17         switch (agv.Command.HandMotorState)
000086  f897c005          LDRB     r12,[r7,#5]  ; agv
00008a  2200              MOVS     r2,#0
;;;18         {
;;;19         case 0x00:
;;;20     				KincoStruct1.setSpeed = 0;
00008c  ed9f1a74          VLDR     s2,|L5.608|
000090  4d74              LDR      r5,|L5.612|
;;;21             KincoStruct2.setSpeed = 0;
000092  4e75              LDR      r6,|L5.616|
;;;22     				lastAngle = CurrentCenterPoint.CurAngle;
;;;23             break;
;;;24     		case 0x01: //前进
;;;25             KincoStruct1.setSpeed = agv.Parameter.HandSpeed;
;;;26             KincoStruct2.setSpeed = agv.Parameter.HandSpeed;
;;;27     
;;;28             break;
;;;29         case 0x02: //后退
;;;30             KincoStruct1.setSpeed = -agv.Parameter.HandSpeed;
;;;31             KincoStruct2.setSpeed = -agv.Parameter.HandSpeed;
;;;32     				
;;;33     
;;;34             break;
;;;35             break;
;;;36         case 0x05: //前动力左转90
;;;37             
;;;38     				if(diffAngle >=0.5)
000094  f04f537c          MOV      r3,#0x3f000000
;;;39     				{
;;;40     					setSpeed = agv.Parameter.HandTurnSpeed;
;;;41     				}
;;;42     				else if(diffAngle >arriveAngle)
;;;43     				{
;;;44     					setSpeed = mapping(diffAngle,arriveAngle,0.5,30,agv.Parameter.HandTurnSpeed);
;;;45     				}
;;;46     				else
;;;47     				{
;;;48     					agv.Command.HandMotorState = 0;
;;;49     					setSpeed = 0;
;;;50     				}
;;;51     				
;;;52     				if(setSpeed > lastSetSpeed)
;;;53     				{
;;;54     					setSpeed = lastSetSpeed + 0.3;
000098  ed9f8b74          VLDR     d8,|L5.620|
00009c  4639              MOV      r1,r7                 ;15
00009e  f8df81ac          LDR      r8,|L5.588|
0000a2  f8b10042          LDRH     r0,[r1,#0x42]         ;40
0000a6  8f89              LDRH     r1,[r1,#0x3c]         ;25
0000a8  edd80a01          VLDR     s1,[r8,#4]            ;42
0000ac  f1bc0f07          CMP      r12,#7                ;17
0000b0  d20b              BCS      |L5.202|
0000b2  e8dff00c          TBB      [pc,r12]              ;17
0000b6  040b              DCB      0x04,0x0b
0000b8  18b9b926          DCB      0x18,0xb9,0xb9,0x26
0000bc  7000              DCB      0x70,0x00
0000be  ed851a00          VSTR     s2,[r5,#0]            ;20
0000c2  ed861a00          VSTR     s2,[r6,#0]            ;21
0000c6  ed849a02          VSTR     s18,[r4,#8]           ;22
                  |L5.202|
0000ca  e0ad              B        |L5.552|
0000cc  ee001a10          VMOV     s0,r1                 ;25
0000d0  eeb80a40          VCVT.F32.U32 s0,s0                 ;25
0000d4  ed850a00          VSTR     s0,[r5,#0]            ;25
0000d8  ee001a10          VMOV     s0,r1                 ;26
0000dc  eeb80a40          VCVT.F32.U32 s0,s0                 ;26
0000e0  ed860a00          VSTR     s0,[r6,#0]            ;26
0000e4  e0a0              B        |L5.552|
0000e6  4248              RSBS     r0,r1,#0              ;30
0000e8  ee000a10          VMOV     s0,r0                 ;30
0000ec  eeb80ac0          VCVT.F32.S32 s0,s0                 ;30
0000f0  ed850a00          VSTR     s0,[r5,#0]            ;30
0000f4  ee000a10          VMOV     s0,r0                 ;31
0000f8  eeb80ac0          VCVT.F32.S32 s0,s0                 ;31
0000fc  ed860a00          VSTR     s0,[r6,#0]            ;31
000100  e092              B        |L5.552|
000102  ee101a10          VMOV     r1,s0                 ;38
000106  4299              CMP      r1,r3                 ;38
000108  db06              BLT      |L5.280|
00010a  ee000a10          VMOV     s0,r0                 ;40
00010e  eeb80a40          VCVT.F32.U32 s0,s0                 ;40
000112  ed840a05          VSTR     s0,[r4,#0x14]         ;40
000116  e014              B        |L5.322|
                  |L5.280|
000118  eeb40ae0          VCMPE.F32 s0,s1                 ;42
00011c  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;42
000120  dd0c              BLE      |L5.316|
000122  ee010a10          VMOV     s2,r0                 ;44
000126  eef31a0e          VMOV.F32 s3,#30.00000000       ;44
00012a  eeb82a41          VCVT.F32.U32 s4,s2                 ;44
00012e  eeb61a00          VMOV.F32 s2,#0.50000000        ;44
000132  f7fffffe          BL       mapping
000136  ed840a05          VSTR     s0,[r4,#0x14]         ;44
00013a  e002              B        |L5.322|
                  |L5.316|
00013c  717a              STRB     r2,[r7,#5]            ;48
00013e  ed841a05          VSTR     s2,[r4,#0x14]         ;49
                  |L5.322|
000142  edd40a05          VLDR     s1,[r4,#0x14]         ;52
000146  ed940a06          VLDR     s0,[r4,#0x18]         ;52
00014a  eef40ac0          VCMPE.F32 s1,s0                 ;52
00014e  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;52
000152  dd0b              BLE      |L5.364|
000154  ee100a10          VMOV     r0,s0
000158  f7fffffe          BL       __aeabi_f2d
00015c  ec532b18          VMOV     r2,r3,d8
000160  f7fffffe          BL       __aeabi_dadd
000164  f7fffffe          BL       __aeabi_d2f
000168  6160              STR      r0,[r4,#0x14]  ; setSpeed
00016a  e00b              B        |L5.388|
                  |L5.364|
;;;55     				}
;;;56     				else if(setSpeed < lastSetSpeed)
00016c  d20a              BCS      |L5.388|
;;;57     				{
;;;58     					setSpeed = lastSetSpeed - 0.3;
00016e  ee100a10          VMOV     r0,s0
000172  f7fffffe          BL       __aeabi_f2d
000176  ec532b18          VMOV     r2,r3,d8
00017a  f7fffffe          BL       __aeabi_dsub
00017e  f7fffffe          BL       __aeabi_d2f
000182  6160              STR      r0,[r4,#0x14]  ; setSpeed
                  |L5.388|
;;;59     				}
;;;60     				KincoStruct1.setSpeed = -setSpeed;
000184  ed940a05          VLDR     s0,[r4,#0x14]
000188  eef10a40          VNEG.F32 s1,s0
00018c  edc50a00          VSTR     s1,[r5,#0]
;;;61             KincoStruct2.setSpeed = setSpeed;
000190  ed860a00          VSTR     s0,[r6,#0]
;;;62     
;;;63             break;
000194  e048              B        |L5.552|
;;;64         case 0x06: //前动力右转90
;;;65             if(diffAngle >=0.5)
000196  ee101a10          VMOV     r1,s0
00019a  4299              CMP      r1,r3
00019c  db06              BLT      |L5.428|
;;;66     				{
;;;67     					setSpeed = agv.Parameter.HandTurnSpeed;
00019e  ee000a10          VMOV     s0,r0
0001a2  eeb80a40          VCVT.F32.U32 s0,s0
0001a6  ed840a05          VSTR     s0,[r4,#0x14]
0001aa  e014              B        |L5.470|
                  |L5.428|
;;;68     				}
;;;69     				else if(diffAngle >arriveAngle)
0001ac  eeb40ae0          VCMPE.F32 s0,s1
0001b0  eef1fa10          VMRS     APSR_nzcv,FPSCR
0001b4  dd0c              BLE      |L5.464|
;;;70     				{
;;;71     					setSpeed = mapping(diffAngle,arriveAngle,0.5,30,agv.Parameter.HandTurnSpeed);
0001b6  ee010a10          VMOV     s2,r0
0001ba  eef31a0e          VMOV.F32 s3,#30.00000000
0001be  eeb82a41          VCVT.F32.U32 s4,s2
0001c2  eeb61a00          VMOV.F32 s2,#0.50000000
0001c6  f7fffffe          BL       mapping
0001ca  ed840a05          VSTR     s0,[r4,#0x14]
0001ce  e002              B        |L5.470|
                  |L5.464|
;;;72     				}
;;;73     				else
;;;74     				{
;;;75     					agv.Command.HandMotorState = 0;
0001d0  717a              STRB     r2,[r7,#5]
;;;76     					setSpeed = 0;
0001d2  ed841a05          VSTR     s2,[r4,#0x14]
                  |L5.470|
;;;77     				}
;;;78     				if(setSpeed > lastSetSpeed)
0001d6  edd40a05          VLDR     s1,[r4,#0x14]
0001da  ed940a06          VLDR     s0,[r4,#0x18]
0001de  eef40ac0          VCMPE.F32 s1,s0
0001e2  eef1fa10          VMRS     APSR_nzcv,FPSCR
0001e6  dd0b              BLE      |L5.512|
;;;79     				{
;;;80     					setSpeed = lastSetSpeed + 0.3;
0001e8  ee100a10          VMOV     r0,s0
0001ec  f7fffffe          BL       __aeabi_f2d
0001f0  ec532b18          VMOV     r2,r3,d8
0001f4  f7fffffe          BL       __aeabi_dadd
0001f8  f7fffffe          BL       __aeabi_d2f
0001fc  6160              STR      r0,[r4,#0x14]  ; setSpeed
0001fe  e00b              B        |L5.536|
                  |L5.512|
;;;81     				}
;;;82     				else if(setSpeed < lastSetSpeed)
000200  d20a              BCS      |L5.536|
;;;83     				{
;;;84     					setSpeed = lastSetSpeed - 0.3;
000202  ee100a10          VMOV     r0,s0
000206  f7fffffe          BL       __aeabi_f2d
00020a  ec532b18          VMOV     r2,r3,d8
00020e  f7fffffe          BL       __aeabi_dsub
000212  f7fffffe          BL       __aeabi_d2f
000216  6160              STR      r0,[r4,#0x14]  ; setSpeed
                  |L5.536|
;;;85     				}
;;;86     				KincoStruct1.setSpeed = setSpeed;
000218  ed940a05          VLDR     s0,[r4,#0x14]
00021c  ed850a00          VSTR     s0,[r5,#0]
;;;87             KincoStruct2.setSpeed = -setSpeed;
000220  eeb10a40          VNEG.F32 s0,s0
000224  ed860a00          VSTR     s0,[r6,#0]
                  |L5.552|
;;;88     				
;;;89             break;
;;;90         
;;;91         default:
;;;92             break;
;;;93         }
;;;94     		lastSetSpeed = setSpeed;
000228  ed940a05          VLDR     s0,[r4,#0x14]
00022c  ed840a06          VSTR     s0,[r4,#0x18]
;;;95         setMotorSpeedSlope(agv.Parameter.HandSpeedSlope);
000230  8ff8              LDRH     r0,[r7,#0x3e]  ; agv
000232  ecbd8b04          VPOP     {d8-d9}
000236  ee000a10          VMOV     s0,r0
00023a  e8bd41f0          POP      {r4-r8,lr}
00023e  eeb80a40          VCVT.F32.U32 s0,s0
000242  f7ffbffe          B.W      setMotorSpeedSlope
;;;96     
;;;97     }
;;;98     
                          ENDP

000246  0000              DCW      0x0000
                  |L5.584|
                          DCD      CurrentCenterPoint
                  |L5.588|
                          DCD      ||.data||
                  |L5.592|
                          DCD      0x40490fda
                  |L5.596|
000254  40c90fda          DCFS     0x40c90fda ; 6.2831850051879883
                  |L5.600|
                          DCD      0xc0490fda
                  |L5.604|
                          DCD      agv
                  |L5.608|
000260  00000000          DCFS     0x00000000 ; 0
                  |L5.612|
                          DCD      KincoStruct1
                  |L5.616|
                          DCD      KincoStruct2
                  |L5.620|
00026c  33333333          DCFD     0x3fd3333333333333 ; 0.29999999999999999
000270  3fd33333

                          AREA ||i.chassisGetAutoSpeed||, CODE, READONLY, ALIGN=1

                  chassisGetAutoSpeed PROC
;;;357    //无轨硬差速模型运动控制
;;;358    void chassisGetAutoSpeed(u8 *CurrentDir)
000000  7800              LDRB     r0,[r0,#0]
;;;359    {
;;;360        if (*CurrentDir == 1 || *CurrentDir == 2) //直行
000002  2801              CMP      r0,#1
000004  d013              BEQ      |L6.46|
000006  2802              CMP      r0,#2
000008  d011              BEQ      |L6.46|
;;;361        {
;;;362            HardDifferentialSpeedStraight(); //硬差速直行
;;;363        }
;;;364        else if (*CurrentDir == 3 || *CurrentDir == 4 || *CurrentDir == 5 || *CurrentDir == 6) //原地旋转
00000a  2803              CMP      r0,#3
00000c  d011              BEQ      |L6.50|
00000e  2804              CMP      r0,#4
000010  d00f              BEQ      |L6.50|
000012  2805              CMP      r0,#5
000014  d00d              BEQ      |L6.50|
000016  2806              CMP      r0,#6
000018  d00b              BEQ      |L6.50|
;;;365        {
;;;366            HardDifferentialSpeedPirouette(); //硬差速原地旋转
;;;367        }
;;;368        else if (*CurrentDir == 7 || *CurrentDir == 8 || *CurrentDir == 9 || *CurrentDir == 10) //弧线转弯
00001a  2807              CMP      r0,#7
00001c  d005              BEQ      |L6.42|
00001e  2808              CMP      r0,#8
000020  d003              BEQ      |L6.42|
000022  2809              CMP      r0,#9
000024  d001              BEQ      |L6.42|
000026  280a              CMP      r0,#0xa
000028  d105              BNE      |L6.54|
                  |L6.42|
;;;369        {
;;;370            HardDifferentialSpeedRunTurnning(); //弧线转弯
00002a  f7ffbffe          B.W      HardDifferentialSpeedRunTurnning
                  |L6.46|
00002e  f7ffbffe          B.W      HardDifferentialSpeedStraight
                  |L6.50|
000032  f7ffbffe          B.W      HardDifferentialSpeedPirouette
                  |L6.54|
;;;371        }
;;;372    }
000036  4770              BX       lr
;;;373    #endif
                          ENDP


                          AREA ||.data||, DATA, ALIGN=2

                  TurnStep
000000  00                DCB      0x00
                  TurnArriveFlag
000001  000000            DCB      0x00,0x00,0x00
                  arriveAngle
000004  3c75c28f          DCFS     0x3c75c28f ; 0.014999999664723873
                  lastAngle
000008  00000000          DCFS     0x00000000 ; 0
                  diffAngle
00000c  00000000          DCFS     0x00000000 ; 0
                  alreadyTurnAngle
000010  00000000          DCFS     0x00000000 ; 0
                  setSpeed
000014  00000000          DCFS     0x00000000 ; 0
                  lastSetSpeed
000018  00000000          DCFS     0x00000000 ; 0
                  agvInAngle
00001c  00000000          DCFS     0x00000000 ; 0
                  turnAngleDiff
000020  00000000          DCFS     0x00000000 ; 0
                  lastAngleDiff
000024  00000000          DCFS     0x00000000 ; 0
                  lastTime
                          DCD      0x00000000
                  SpeedDifValue
                          DCD      0x00000000

;*** Start embedded assembler ***

#line 1 "..\\..\\User\\CHASSIS\\HardDifferential.c"
	AREA ||.rev16_text||, CODE
	THUMB
	EXPORT |__asm___18_HardDifferential_c_c9731b9a____REV16|
#line 129 "..\\..\\Libraries\\CMSIS\\Include\\core_cmInstr.h"
|__asm___18_HardDifferential_c_c9731b9a____REV16| PROC
#line 130

 rev16 r0, r0
 bx lr
	ENDP
	AREA ||.revsh_text||, CODE
	THUMB
	EXPORT |__asm___18_HardDifferential_c_c9731b9a____REVSH|
#line 144
|__asm___18_HardDifferential_c_c9731b9a____REVSH| PROC
#line 145

 revsh r0, r0
 bx lr
	ENDP
	AREA ||.rrx_text||, CODE
	THUMB
	EXPORT |__asm___18_HardDifferential_c_c9731b9a____RRX|
#line 300
|__asm___18_HardDifferential_c_c9731b9a____RRX| PROC
#line 301

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***