AGV / agv_xiangyanghuiqiang · Files

agv.txt 85.3 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 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469 1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521 1522 1523 1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581 1582 1583 1584 1585 1586 1587 1588 1589 1590 1591 1592 1593 1594 1595 1596 1597 1598 1599 1600 1601 1602 1603 1604 1605 1606 1607 1608 1609 1610 1611 1612 1613 1614 1615 1616 1617 1618 1619 1620 1621 1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638 1639 1640 1641 1642 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653 1654 1655 1656 1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674 1675 1676 1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693 1694 1695 1696 1697 1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1722 1723 1724 1725 1726 1727 1728 1729 1730 1731 1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743 1744 1745 1746 1747 1748 1749 1750 1751 1752 1753 1754 1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768 1769 1770 1771 1772 1773 1774 1775 1776 1777 1778 1779 1780 1781 1782 1783 1784 1785 1786 1787 1788 1789 1790 1791 1792 1793 1794 1795 1796 1797 1798 1799 1800 1801 1802 1803 1804 1805 1806 1807 1808 1809 1810 1811 1812 1813 1814 1815 
; 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\agv.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\agv.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.\RTE\_Flash -IC:\Users\软件部\AppData\Local\Arm\Packs\ARM\CMSIS\5.5.1\CMSIS\Core\Include -IC:\Users\软件部\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\agv.crf ..\..\User\LaserMotionCtr\agv.c]
                          THUMB

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

                  ClearSiteAction PROC
;;;290    //清除站点动作
;;;291    void ClearSiteAction(agvControlInputValue *ControlInputValue)
000000  2100              MOVS     r1,#0
;;;292    {
;;;293    	ControlInputValue->CurDirection = 0;
000002  30b8              ADDS     r0,r0,#0xb8
000004  8301              STRH     r1,[r0,#0x18]
;;;294    	ControlInputValue->CurAgvSpeed = 0;
000006  82c1              STRH     r1,[r0,#0x16]
;;;295    	ControlInputValue->X0 = 0;
000008  61c1              STR      r1,[r0,#0x1c]
;;;296    	ControlInputValue->Y0 = 0;
00000a  6201              STR      r1,[r0,#0x20]
;;;297    	ControlInputValue->X1 = 0;
00000c  6241              STR      r1,[r0,#0x24]
;;;298    	ControlInputValue->Y1 = 0;
00000e  6281              STR      r1,[r0,#0x28]
;;;299    	ControlInputValue->X2 = 0;
000010  62c1              STR      r1,[r0,#0x2c]
;;;300    	ControlInputValue->Y2 = 0;
000012  6301              STR      r1,[r0,#0x30]
;;;301    	ControlInputValue->X3 = 0;
000014  6341              STR      r1,[r0,#0x34]
;;;302    	ControlInputValue->Y3 = 0;
000016  6381              STR      r1,[r0,#0x38]
;;;303    
;;;304    	ControlInputValue->OnlineLogo = 0;
000018  7001              STRB     r1,[r0,#0]
;;;305    	SySAlarmStatus.Off_linestatus = 0;
00001a  480b              LDR      r0,|L1.72|
00001c  7041              STRB     r1,[r0,#1]
;;;306    	DispatchData.CurAgvSpeed = 0;
00001e  480b              LDR      r0,|L1.76|
000020  8181              STRH     r1,[r0,#0xc]
;;;307    	DispatchData.CurDirection = 0;
000022  81c1              STRH     r1,[r0,#0xe]
;;;308    	DispatchData.CurSite = 0;
000024  8041              STRH     r1,[r0,#2]
;;;309    	DispatchData.FirstSerialNumber = 0;
000026  80c1              STRH     r1,[r0,#6]
;;;310    	DispatchData.NextSite = 0;
000028  8081              STRH     r1,[r0,#4]
;;;311    	DispatchData.SecondSerialNumber = 0;
00002a  8101              STRH     r1,[r0,#8]
;;;312    	DispatchData.ThirdSerialNumber = 0;
00002c  8141              STRH     r1,[r0,#0xa]
;;;313    	DispatchData.X0 = 0;
00002e  6141              STR      r1,[r0,#0x14]  ; DispatchData
;;;314    	DispatchData.Y0 = 0;
000030  6181              STR      r1,[r0,#0x18]  ; DispatchData
;;;315    	DispatchData.X1 = 0;
000032  61c1              STR      r1,[r0,#0x1c]  ; DispatchData
;;;316    	DispatchData.Y1 = 0;
000034  6201              STR      r1,[r0,#0x20]  ; DispatchData
;;;317    	DispatchData.X2 = 0;
000036  6241              STR      r1,[r0,#0x24]  ; DispatchData
;;;318    	DispatchData.Y2 = 0;
000038  6281              STR      r1,[r0,#0x28]  ; DispatchData
;;;319    	DispatchData.X3 = 0;
00003a  62c1              STR      r1,[r0,#0x2c]  ; DispatchData
;;;320    	DispatchData.Y3 = 0;
00003c  6301              STR      r1,[r0,#0x30]  ; DispatchData
;;;321    	
;;;322    	ControlOutputValue.AgvSettingFirstSpeed = 0;
00003e  4804              LDR      r0,|L1.80|
000040  8181              STRH     r1,[r0,#0xc]
;;;323    	ControlOutputValue.AgvSettingSecondSpeed = 0;
000042  81c1              STRH     r1,[r0,#0xe]
;;;324    }
000044  4770              BX       lr
;;;325    
                          ENDP

000046  0000              DCW      0x0000
                  |L1.72|
                          DCD      ||.data||+0x10
                  |L1.76|
                          DCD      DispatchData
                  |L1.80|
                          DCD      ControlOutputValue

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

                  GetLineActionInformation PROC
;;;326    //获取线路站点动作信息
;;;327    u8 GetLineActionInformation(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue)
000000  e92d41f0          PUSH     {r4-r8,lr}
;;;328    {
000004  460c              MOV      r4,r1
;;;329    	if(ControlInputValue->StandaloneOrOnlineMode)//单机
;;;330    	{
;;;331    		//单机调度
;;;332    		if(ControlInputValue->OnlineLogo == 0)
000006  2600              MOVS     r6,#0
000008  f89410b0          LDRB     r1,[r4,#0xb0]         ;329
00000c  f89400b8          LDRB     r0,[r4,#0xb8]
;;;333    		{
;;;334    			LaserControlSignal.standSiteID = 1;//ControlInputValue->agvSite;
000010  2501              MOVS     r5,#1
000012  2900              CMP      r1,#0                 ;329
000014  d07e              BEQ      |L2.276|
000016  498d              LDR      r1,|L2.588|
000018  b928              CBNZ     r0,|L2.38|
00001a  71cd              STRB     r5,[r1,#7]
;;;335    			LaserControlSignal.standLine   = 1;//ControlInputValue->agvLine;
00001c  720d              STRB     r5,[r1,#8]
;;;336    			
;;;337    			if(LaserControlSignal.standSiteID && LaserControlSignal.standLine)
;;;338    			{
;;;339    				//Uart_Printf(COM1,"单机线路上线成功\r\n");
;;;340    				ControlInputValue->OnlineLogo = 1;
00001e  f88450b8          STRB     r5,[r4,#0xb8]
;;;341    				ControlInputValue->OfflineLogo = 0;
000022  f88460bd          STRB     r6,[r4,#0xbd]
                  |L2.38|
;;;342    			}
;;;343    			else
;;;344    			{
;;;345    				return 0;
;;;346    			}
;;;347    		}
;;;348    		
;;;349    		ControlInputValue->OfflineLogo  = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].OffineStatus;		
000026  7a08              LDRB     r0,[r1,#8]  ; LaserControlSignal
000028  79cb              LDRB     r3,[r1,#7]  ; LaserControlSignal
00002a  1e40              SUBS     r0,r0,#1
00002c  ebc01000          RSB      r0,r0,r0,LSL #4
000030  eb000240          ADD      r2,r0,r0,LSL #1
000034  4886              LDR      r0,|L2.592|
000036  eb001502          ADD      r5,r0,r2,LSL #4
00003a  1e58              SUBS     r0,r3,#1
00003c  eb000040          ADD      r0,r0,r0,LSL #1
000040  eb0501c0          ADD      r1,r5,r0,LSL #3
000044  34bc              ADDS     r4,r4,#0xbc
000046  78c8              LDRB     r0,[r1,#3]
000048  7060              STRB     r0,[r4,#1]
;;;350    		ControlInputValue->CurDirection	= InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].WalkingDirection;
00004a  7848              LDRB     r0,[r1,#1]
00004c  82a0              STRH     r0,[r4,#0x14]
;;;351    		ControlInputValue->NextDirection = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID].WalkingDirection;
00004e  eb030243          ADD      r2,r3,r3,LSL #1
000052  eb0502c2          ADD      r2,r5,r2,LSL #3
000056  7856              LDRB     r6,[r2,#1]
000058  82e6              STRH     r6,[r4,#0x16]
;;;352    		ControlInputValue->CurAgvSpeed	= InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].WalkingSpeedSetting;
00005a  88ce              LDRH     r6,[r1,#6]
00005c  8266              STRH     r6,[r4,#0x12]
;;;353    		ControlInputValue->LiftStatus	= InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].LifttingStatus;
00005e  788e              LDRB     r6,[r1,#2]
000060  70a6              STRB     r6,[r4,#2]
;;;354    		ControlInputValue->LiftDIS		= InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].LifttingDistance;
000062  890e              LDRH     r6,[r1,#8]
000064  80a6              STRH     r6,[r4,#4]
;;;355    		ControlInputValue->LiftSpeed	= InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].LifttingSpeedSetting;
000066  894e              LDRH     r6,[r1,#0xa]
000068  80e6              STRH     r6,[r4,#6]
;;;356    		ControlInputValue->RadarEAR		= InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].AgvRadarP;
00006a  790e              LDRB     r6,[r1,#4]
00006c  f80469bc          STRB     r6,[r4],#-0xbc
;;;357    		
;;;358    		if((ControlInputValue->CurDirection == 3) | ((ControlInputValue->CurDirection == 4))|
000070  2803              CMP      r0,#3
000072  d031              BEQ      |L2.216|
000074  2600              MOVS     r6,#0
                  |L2.118|
000076  2804              CMP      r0,#4
000078  d030              BEQ      |L2.220|
00007a  2700              MOVS     r7,#0
                  |L2.124|
00007c  433e              ORRS     r6,r6,r7
00007e  2805              CMP      r0,#5
000080  d02e              BEQ      |L2.224|
000082  2700              MOVS     r7,#0
                  |L2.132|
000084  433e              ORRS     r6,r6,r7
000086  2806              CMP      r0,#6
000088  d02c              BEQ      |L2.228|
00008a  2700              MOVS     r7,#0
                  |L2.140|
00008c  433e              ORRS     r6,r6,r7
00008e  2807              CMP      r0,#7
000090  d02a              BEQ      |L2.232|
000092  2700              MOVS     r7,#0
                  |L2.148|
000094  433e              ORRS     r6,r6,r7
000096  2808              CMP      r0,#8
000098  d028              BEQ      |L2.236|
00009a  2700              MOVS     r7,#0
                  |L2.156|
00009c  433e              ORRS     r6,r6,r7
00009e  d027              BEQ      |L2.240|
;;;359    		  ((ControlInputValue->CurDirection == 5)) | ((ControlInputValue->CurDirection == 6))|
;;;360    		  ((ControlInputValue->CurDirection == 7)) | ((ControlInputValue->CurDirection == 8)))
;;;361    		{		
;;;362    			ControlInputValue->X0 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-2].X_coordinate;
0000a0  1e98              SUBS     r0,r3,#2
0000a2  eb000040          ADD      r0,r0,r0,LSL #1
0000a6  eb0500c0          ADD      r0,r5,r0,LSL #3
0000aa  68c6              LDR      r6,[r0,#0xc]
0000ac  f8446fd4          STR      r6,[r4,#0xd4]!
;;;363    			ControlInputValue->Y0 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-2].Y_coordinate;
0000b0  6900              LDR      r0,[r0,#0x10]
0000b2  6060              STR      r0,[r4,#4]
;;;364    			ControlInputValue->X1 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].X_coordinate;
0000b4  68c8              LDR      r0,[r1,#0xc]
0000b6  60a0              STR      r0,[r4,#8]
;;;365    			ControlInputValue->Y1 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].Y_coordinate;
0000b8  6908              LDR      r0,[r1,#0x10]
0000ba  60e0              STR      r0,[r4,#0xc]
;;;366    
;;;367    			ControlInputValue->X2 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID].X_coordinate;
0000bc  68d0              LDR      r0,[r2,#0xc]
0000be  6120              STR      r0,[r4,#0x10]
;;;368    			ControlInputValue->Y2 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID].Y_coordinate;
0000c0  6910              LDR      r0,[r2,#0x10]
0000c2  6160              STR      r0,[r4,#0x14]
;;;369    			ControlInputValue->X3 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID+1].X_coordinate;
0000c4  1c5b              ADDS     r3,r3,#1
0000c6  eb030043          ADD      r0,r3,r3,LSL #1
0000ca  eb0500c0          ADD      r0,r5,r0,LSL #3
0000ce  68c1              LDR      r1,[r0,#0xc]
0000d0  61a1              STR      r1,[r4,#0x18]
;;;370    			ControlInputValue->Y3 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID+1].Y_coordinate;
0000d2  6900              LDR      r0,[r0,#0x10]
0000d4  61e0              STR      r0,[r4,#0x1c]
0000d6  e0b2              B        |L2.574|
                  |L2.216|
0000d8  2601              MOVS     r6,#1                 ;358
0000da  e7cc              B        |L2.118|
                  |L2.220|
0000dc  2701              MOVS     r7,#1                 ;358
0000de  e7cd              B        |L2.124|
                  |L2.224|
0000e0  2701              MOVS     r7,#1                 ;358
0000e2  e7cf              B        |L2.132|
                  |L2.228|
0000e4  2701              MOVS     r7,#1                 ;358
0000e6  e7d1              B        |L2.140|
                  |L2.232|
0000e8  2701              MOVS     r7,#1                 ;358
0000ea  e7d3              B        |L2.148|
                  |L2.236|
0000ec  2701              MOVS     r7,#1                 ;358
0000ee  e7d5              B        |L2.156|
                  |L2.240|
;;;371    		}
;;;372    		else if((ControlInputValue->CurDirection == 1) | (ControlInputValue->CurDirection == 2))
0000f0  2801              CMP      r0,#1
0000f2  d010              BEQ      |L2.278|
0000f4  2300              MOVS     r3,#0
                  |L2.246|
0000f6  2802              CMP      r0,#2
0000f8  d00f              BEQ      |L2.282|
0000fa  2000              MOVS     r0,#0
                  |L2.252|
0000fc  4303              ORRS     r3,r3,r0
0000fe  d008              BEQ      |L2.274|
;;;373    		{
;;;374    			ControlInputValue->X0 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].X_coordinate;
000100  68c8              LDR      r0,[r1,#0xc]
000102  f8440fd4          STR      r0,[r4,#0xd4]!
;;;375    			ControlInputValue->Y0 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID-1].Y_coordinate;
000106  6908              LDR      r0,[r1,#0x10]
000108  6060              STR      r0,[r4,#4]
;;;376    			ControlInputValue->X1 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID].X_coordinate;
00010a  68d0              LDR      r0,[r2,#0xc]
00010c  60a0              STR      r0,[r4,#8]
;;;377    			ControlInputValue->Y1 = InputInformationList.engineeringInputInformation[LaserControlSignal.standLine-1][LaserControlSignal.standSiteID].Y_coordinate;
00010e  6910              LDR      r0,[r2,#0x10]
000110  60e0              STR      r0,[r4,#0xc]
                  |L2.274|
000112  e094              B        |L2.574|
                  |L2.276|
000114  e003              B        |L2.286|
                  |L2.278|
000116  2301              MOVS     r3,#1                 ;372
000118  e7ed              B        |L2.246|
                  |L2.282|
00011a  2001              MOVS     r0,#1                 ;372
00011c  e7ee              B        |L2.252|
                  |L2.286|
;;;378    		
;;;379    		}		
;;;380    //		Uart_Printf(COM1,"SPEED=%d	x1=%d	x2=%d	y1=%d	y2=%d\r\n",ControlInputValue->CurAgvSpeed,ControlInputValue->X0,ControlInputValue->X1,
;;;381    //		ControlInputValue->Y0,ControlInputValue->Y1);
;;;382    	}
;;;383    	else
;;;384    	{		
;;;385    		//第一次上线问题
;;;386    		if(ControlInputValue->OnlineLogo == 0)
00011e  b948              CBNZ     r0,|L2.308|
;;;387    		{	
;;;388    			if(agvFirstUpdataSiteID())
000120  f7fffffe          BL       agvFirstUpdataSiteID
000124  b350              CBZ      r0,|L2.380|
;;;389    			{
;;;390    				//上线成功
;;;391    				//Uart_Printf(COM1,"上线成功\r\n");
;;;392    				ControlInputValue->OnlineLogo = 1;
000126  f88450b8          STRB     r5,[r4,#0xb8]
;;;393    				ControlInputValue->OfflineLogo = 0;
00012a  f88460bd          STRB     r6,[r4,#0xbd]
;;;394    				HandlePlcData.SendPlcData.ClearSiteAck = 0;
00012e  4849              LDR      r0,|L2.596|
000130  f88060fb          STRB     r6,[r0,#0xfb]
                  |L2.308|
;;;395    			}
;;;396    			else 
;;;397    			{
;;;398    				//Uart_Printf(COM1,"上线失败\r\n");
;;;399    				return 0;
;;;400    			}
;;;401    		}
;;;402    		
;;;403    		ControlInputValue->OperationMode = DispatchData.OperationMode;
000134  4948              LDR      r1,|L2.600|
000136  7848              LDRB     r0,[r1,#1]  ; DispatchData
000138  f88400bb          STRB     r0,[r4,#0xbb]
;;;404    		ControlInputValue->CurDirection	= DispatchData.CurDirection;
00013c  f9b1000e          LDRSH    r0,[r1,#0xe]  ; DispatchData
000140  f8a400d0          STRH     r0,[r4,#0xd0]
;;;405    		
;;;406    		ControlInputValue->NextDirection = DispatchData.NextDirection;
000144  8a0a              LDRH     r2,[r1,#0x10]  ; DispatchData
000146  f8a420d2          STRH     r2,[r4,#0xd2]
;;;407    		if(ControlInputValue->CurDirection == 15)ControlInputValue->OfflineLogo = 1;
00014a  280f              CMP      r0,#0xf
00014c  d019              BEQ      |L2.386|
;;;408    		else ControlInputValue->OfflineLogo = 0;
00014e  f88460bd          STRB     r6,[r4,#0xbd]
                  |L2.338|
;;;409    
;;;410    		if((ControlInputValue->CurDirection == 1) || (ControlInputValue->CurDirection == 3) || (ControlInputValue->CurDirection == 4)
000152  2801              CMP      r0,#1
000154  d018              BEQ      |L2.392|
000156  2803              CMP      r0,#3
000158  d016              BEQ      |L2.392|
00015a  2804              CMP      r0,#4
00015c  d014              BEQ      |L2.392|
;;;411    			|| (ControlInputValue->CurDirection == 7) || (ControlInputValue->CurDirection == 8) || (ControlInputValue->CurDirection == 9) || (ControlInputValue->CurDirection == 10))
00015e  2807              CMP      r0,#7
000160  d012              BEQ      |L2.392|
000162  2808              CMP      r0,#8
000164  d010              BEQ      |L2.392|
000166  2809              CMP      r0,#9
000168  d00e              BEQ      |L2.392|
00016a  280a              CMP      r0,#0xa
00016c  d00c              BEQ      |L2.392|
;;;412    		ControlInputValue->CurAgvSpeed	= DispatchData.CurAgvSpeed;
;;;413    		else if((ControlInputValue->CurDirection == 2) || (ControlInputValue->CurDirection == 5) || (ControlInputValue->CurDirection == 6))
00016e  2802              CMP      r0,#2
000170  d00e              BEQ      |L2.400|
000172  2805              CMP      r0,#5
000174  d00c              BEQ      |L2.400|
000176  2806              CMP      r0,#6
000178  d00a              BEQ      |L2.400|
00017a  e00d              B        |L2.408|
                  |L2.380|
00017c  2000              MOVS     r0,#0                 ;399
                  |L2.382|
;;;414    		ControlInputValue->CurAgvSpeed	= -DispatchData.CurAgvSpeed;
;;;415    		
;;;416    		if((ControlInputValue->CurDirection == 3) | (ControlInputValue->CurDirection == 4)|
;;;417    		(ControlInputValue->CurDirection == 5) | (ControlInputValue->CurDirection == 6)|
;;;418    		(ControlInputValue->CurDirection == 7) | (ControlInputValue->CurDirection == 8) |
;;;419    		(ControlInputValue->CurDirection == 9) | (ControlInputValue->CurDirection == 10))
;;;420    		{
;;;421    			ControlInputValue->X0 = DispatchData.X0;
;;;422    			ControlInputValue->Y0 = DispatchData.Y0;
;;;423    			ControlInputValue->X1 = DispatchData.X1;
;;;424    			ControlInputValue->Y1 = DispatchData.Y1;
;;;425    			ControlInputValue->X2 = DispatchData.X2;
;;;426    			ControlInputValue->Y2 = DispatchData.Y2;
;;;427    			ControlInputValue->X3 = DispatchData.X3;
;;;428    			ControlInputValue->Y3 = DispatchData.Y3;
;;;429    		}
;;;430    		else if((ControlInputValue->CurDirection == 1) | (ControlInputValue->CurDirection == 2))
;;;431    		{
;;;432    			ControlInputValue->X0 = DispatchData.X1;
;;;433    			ControlInputValue->Y0 = DispatchData.Y1;
;;;434    			ControlInputValue->X1 = DispatchData.X2;
;;;435    			ControlInputValue->Y1 = DispatchData.Y2;		
;;;436    		}
;;;437    	}
;;;438    	return 1;
;;;439    }
00017e  e8bd81f0          POP      {r4-r8,pc}
                  |L2.386|
000182  f88450bd          STRB     r5,[r4,#0xbd]         ;407
000186  e7e4              B        |L2.338|
                  |L2.392|
000188  898a              LDRH     r2,[r1,#0xc]          ;412  ; DispatchData
00018a  f8a420ce          STRH     r2,[r4,#0xce]         ;412
00018e  e003              B        |L2.408|
                  |L2.400|
000190  898a              LDRH     r2,[r1,#0xc]          ;414  ; DispatchData
000192  4252              RSBS     r2,r2,#0              ;414
000194  f8a420ce          STRH     r2,[r4,#0xce]         ;414
                  |L2.408|
000198  2803              CMP      r0,#3                 ;416
00019a  d02f              BEQ      |L2.508|
00019c  2200              MOVS     r2,#0                 ;416
                  |L2.414|
00019e  2804              CMP      r0,#4                 ;416
0001a0  d02e              BEQ      |L2.512|
0001a2  2300              MOVS     r3,#0                 ;416
                  |L2.420|
0001a4  431a              ORRS     r2,r2,r3              ;416
0001a6  2805              CMP      r0,#5                 ;416
0001a8  d02c              BEQ      |L2.516|
0001aa  2300              MOVS     r3,#0                 ;416
                  |L2.428|
0001ac  431a              ORRS     r2,r2,r3              ;416
0001ae  2806              CMP      r0,#6                 ;416
0001b0  d02a              BEQ      |L2.520|
0001b2  2300              MOVS     r3,#0                 ;416
                  |L2.436|
0001b4  431a              ORRS     r2,r2,r3              ;416
0001b6  2807              CMP      r0,#7                 ;416
0001b8  d028              BEQ      |L2.524|
0001ba  2300              MOVS     r3,#0                 ;416
                  |L2.444|
0001bc  431a              ORRS     r2,r2,r3              ;416
0001be  2808              CMP      r0,#8                 ;416
0001c0  d026              BEQ      |L2.528|
0001c2  2300              MOVS     r3,#0                 ;416
                  |L2.452|
0001c4  431a              ORRS     r2,r2,r3              ;416
0001c6  2809              CMP      r0,#9                 ;416
0001c8  d024              BEQ      |L2.532|
0001ca  2300              MOVS     r3,#0                 ;416
                  |L2.460|
0001cc  431a              ORRS     r2,r2,r3              ;416
0001ce  280a              CMP      r0,#0xa               ;416
0001d0  d022              BEQ      |L2.536|
0001d2  2300              MOVS     r3,#0                 ;416
                  |L2.468|
0001d4  431a              ORRS     r2,r2,r3              ;416
0001d6  d021              BEQ      |L2.540|
0001d8  6948              LDR      r0,[r1,#0x14]         ;421  ; DispatchData
0001da  f8440fd4          STR      r0,[r4,#0xd4]!        ;421
0001de  6988              LDR      r0,[r1,#0x18]         ;422  ; DispatchData
0001e0  6060              STR      r0,[r4,#4]            ;422
0001e2  69c8              LDR      r0,[r1,#0x1c]         ;423  ; DispatchData
0001e4  60a0              STR      r0,[r4,#8]            ;423
0001e6  6a08              LDR      r0,[r1,#0x20]         ;424  ; DispatchData
0001e8  60e0              STR      r0,[r4,#0xc]          ;424
0001ea  6a48              LDR      r0,[r1,#0x24]         ;425  ; DispatchData
0001ec  6120              STR      r0,[r4,#0x10]         ;425
0001ee  6a88              LDR      r0,[r1,#0x28]         ;426  ; DispatchData
0001f0  6160              STR      r0,[r4,#0x14]         ;426
0001f2  6ac8              LDR      r0,[r1,#0x2c]         ;427  ; DispatchData
0001f4  61a0              STR      r0,[r4,#0x18]         ;427
0001f6  6b08              LDR      r0,[r1,#0x30]         ;428  ; DispatchData
0001f8  61e0              STR      r0,[r4,#0x1c]         ;428
0001fa  e020              B        |L2.574|
                  |L2.508|
0001fc  2201              MOVS     r2,#1                 ;416
0001fe  e7ce              B        |L2.414|
                  |L2.512|
000200  2301              MOVS     r3,#1                 ;416
000202  e7cf              B        |L2.420|
                  |L2.516|
000204  2301              MOVS     r3,#1                 ;416
000206  e7d1              B        |L2.428|
                  |L2.520|
000208  2301              MOVS     r3,#1                 ;416
00020a  e7d3              B        |L2.436|
                  |L2.524|
00020c  2301              MOVS     r3,#1                 ;416
00020e  e7d5              B        |L2.444|
                  |L2.528|
000210  2301              MOVS     r3,#1                 ;416
000212  e7d7              B        |L2.452|
                  |L2.532|
000214  2301              MOVS     r3,#1                 ;416
000216  e7d9              B        |L2.460|
                  |L2.536|
000218  2301              MOVS     r3,#1                 ;416
00021a  e7db              B        |L2.468|
                  |L2.540|
00021c  2801              CMP      r0,#1                 ;430
00021e  d010              BEQ      |L2.578|
000220  2200              MOVS     r2,#0                 ;430
                  |L2.546|
000222  2802              CMP      r0,#2                 ;430
000224  d00f              BEQ      |L2.582|
000226  2000              MOVS     r0,#0                 ;430
                  |L2.552|
000228  4302              ORRS     r2,r2,r0              ;430
00022a  d008              BEQ      |L2.574|
00022c  69c8              LDR      r0,[r1,#0x1c]         ;432  ; DispatchData
00022e  f8440fd4          STR      r0,[r4,#0xd4]!        ;432
000232  6a08              LDR      r0,[r1,#0x20]         ;433  ; DispatchData
000234  6060              STR      r0,[r4,#4]            ;433
000236  6a48              LDR      r0,[r1,#0x24]         ;434  ; DispatchData
000238  60a0              STR      r0,[r4,#8]            ;434
00023a  6a88              LDR      r0,[r1,#0x28]         ;435  ; DispatchData
00023c  60e0              STR      r0,[r4,#0xc]          ;435
                  |L2.574|
00023e  2001              MOVS     r0,#1                 ;438
000240  e79d              B        |L2.382|
                  |L2.578|
000242  2201              MOVS     r2,#1                 ;430
000244  e7ed              B        |L2.546|
                  |L2.582|
000246  2001              MOVS     r0,#1                 ;430
000248  e7ee              B        |L2.552|
;;;440    
                          ENDP

00024a  0000              DCW      0x0000
                  |L2.588|
                          DCD      ||.bss||+0x1c20
                  |L2.592|
                          DCD      ||.bss||
                  |L2.596|
                          DCD      HandlePlcData
                  |L2.600|
                          DCD      DispatchData

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

                  InitializeRequirementOutputFun PROC
;;;577    
;;;578    void InitializeRequirementOutputFun(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  b510              PUSH     {r4,lr}
;;;579    {
;;;580    	
;;;581    	//ControlInputValue->NavcitionMode = 2;
;;;582    	//固定参数初始化
;;;583    	ControlInputValue->NavcitionMode = 				pSetup[SETUP_NAV_TYPE];//导航方式
000002  4b31              LDR      r3,|L3.200|
000004  31a0              ADDS     r1,r1,#0xa0
000006  681b              LDR      r3,[r3,#0]  ; pSetup
000008  781c              LDRB     r4,[r3,#0]
00000a  f8014c63          STRB     r4,[r1,#-0x63]
;;;584    	ControlInputValue->TurnningMode = 				pSetup[SETUP_TURN_MODE];					//车体结构
00000e  791c              LDRB     r4,[r3,#4]
000010  f8014c64          STRB     r4,[r1,#-0x64]
;;;585    	
;;;586    	ControlInputValue->Wheelbase =					(float)pSetup[SETUP_MOTOR_WHEELBASE];				//轴距
000014  ed930a09          VLDR     s0,[r3,#0x24]
000018  eeb80a40          VCVT.F32.U32 s0,s0
00001c  ed010a18          VSTR     s0,[r1,#-0x60]
;;;587    	
;;;588    	ControlInputValue->WheelDistance = 				(float)pSetup[SETUP_MOTOR_TREAD];					//差速轮轮距
000020  ed930a0a          VLDR     s0,[r3,#0x28]
000024  eeb80a40          VCVT.F32.U32 s0,s0
000028  ed010a17          VSTR     s0,[r1,#-0x5c]
;;;589    	ControlInputValue->FwheelToZHOU =				(float)pSetup[SETUP_FRONTSTEERING_X_DIS];
00002c  ed930a35          VLDR     s0,[r3,#0xd4]
000030  eeb80a40          VCVT.F32.U32 s0,s0
000034  ed010a16          VSTR     s0,[r1,#-0x58]
;;;590    	ControlInputValue->BwheelToZHOU =				(float)pSetup[SETUP_BACKSTEERING_X_DIS];			//舵轮到中线距离
000038  ed930a36          VLDR     s0,[r3,#0xd8]
00003c  eeb80a40          VCVT.F32.U32 s0,s0
000040  ed010a15          VSTR     s0,[r1,#-0x54]
;;;591    	
;;;592    	ControlInputValue->LaserDistanceExtensionOne =	(float)pSetup[SETUP_FRONTDIS];
000044  ed930a50          VLDR     s0,[r3,#0x140]
000048  eeb80a40          VCVT.F32.U32 s0,s0
00004c  ed010a0e          VSTR     s0,[r1,#-0x38]
;;;593    	ControlInputValue->LaserDistanceExtensionTwo =	(float)pSetup[SETUP_BACKDIS];
000050  ed930a51          VLDR     s0,[r3,#0x144]
000054  eeb80a40          VCVT.F32.U32 s0,s0
000058  ed010a0d          VSTR     s0,[r1,#-0x34]
;;;594    	ControlInputValue->SlamOr350				 =	pSetup[SETUP_350_SLAM];
00005c  f893414c          LDRB     r4,[r3,#0x14c]
000060  f8014c24          STRB     r4,[r1,#-0x24]
;;;595    	ControlInputValue->StandaloneOrOnlineMode 	 =  pSetup[SETUP_StandaloneOrOnlineMode];		
000064  f8934148          LDRB     r4,[r3,#0x148]
000068  740c              STRB     r4,[r1,#0x10]
;;;596    	ControlInputValue->OmnidirectionalWalking    = 	pSetup[SETUP_OMNIDIRE_MOVE_OPEN];
00006a  f893316c          LDRB     r3,[r3,#0x16c]
00006e  f8013c22          STRB     r3,[r1,#-0x22]
;;;597    	
;;;598    	ControlInputValue->RudderPlaceAngle					= 0.015f;
000072  ed9f0a16          VLDR     s0,|L3.204|
000076  ed010a08          VSTR     s0,[r1,#-0x20]
;;;599    	LaserControlSignal.StraightHalfwayStatus			= 0;
00007a  4c15              LDR      r4,|L3.208|
00007c  2300              MOVS     r3,#0
00007e  70e3              STRB     r3,[r4,#3]
;;;600    	LaserControlSignal.TurnningHalfwayStatus			= 0;
000080  70a3              STRB     r3,[r4,#2]
;;;601    	LaserControlSignal.TurnningCalStep					= 0;
000082  7123              STRB     r3,[r4,#4]
;;;602    	LaserControlSignal.InSituTurnningStep				= 0;
000084  7163              STRB     r3,[r4,#5]
;;;603    
;;;604    	CalculationTurningSteering->VirtualIntermediateTarAngle	= 0;
000086  ed9f0a13          VLDR     s0,|L3.212|
00008a  ed800a1d          VSTR     s0,[r0,#0x74]
;;;605    	CalculationTurningSteering->SetTurnningAngle		= 0;
00008e  ed800a0d          VSTR     s0,[r0,#0x34]
;;;606    
;;;607    	ControlInputValue->QrcodeLocationingStatus			= 0;
000092  764b              STRB     r3,[r1,#0x19]
;;;608    	ControlInputValue->SlamOnline						= 0;
000094  f8013c23          STRB     r3,[r1,#-0x23]
;;;609    	ControlInputValue->OfflineLogo						= 0;
000098  774b              STRB     r3,[r1,#0x1d]
;;;610    	ControlInputValue->OnlineLogo						= 0;
00009a  760b              STRB     r3,[r1,#0x18]
;;;611    	ControlInputValue->CurSite							= 0;
00009c  848b              STRH     r3,[r1,#0x24]
;;;612    	ControlInputValue->NextSite							= 0;
00009e  84cb              STRH     r3,[r1,#0x26]
;;;613    	ControlInputValue->FirstSerialNumber				= 0;
0000a0  850b              STRH     r3,[r1,#0x28]
;;;614    	ControlInputValue->SecondSerialNumber				= 0;
0000a2  854b              STRH     r3,[r1,#0x2a]
;;;615    	ControlInputValue->ThirdSerialNumber				= 0;
0000a4  858b              STRH     r3,[r1,#0x2c]
;;;616    	ControlInputValue->CurAgvSpeed						= 0;
0000a6  85cb              STRH     r3,[r1,#0x2e]
;;;617    	ControlInputValue->CurDirection						= 0;
0000a8  860b              STRH     r3,[r1,#0x30]
;;;618    	ControlInputValue->X1								= 0;
0000aa  63cb              STR      r3,[r1,#0x3c]
;;;619    	ControlInputValue->Y1								= 0;
0000ac  640b              STR      r3,[r1,#0x40]
;;;620    	ControlInputValue->X2								= 0;
0000ae  644b              STR      r3,[r1,#0x44]
;;;621    	ControlInputValue->Y2								= 0;
0000b0  648b              STR      r3,[r1,#0x48]
;;;622    	ControlInputValue->X3								= 0;
0000b2  64cb              STR      r3,[r1,#0x4c]
;;;623    	ControlInputValue->Y3								= 0;
0000b4  650b              STR      r3,[r1,#0x50]
;;;624    	ControlInputValue->agvSite							= 0;
0000b6  744b              STRB     r3,[r1,#0x11]
;;;625    	ControlInputValue->agvLine							= 0;
0000b8  748b              STRB     r3,[r1,#0x12]
;;;626    	ControlInputValue->ClearSiteAndLine					= 0;	
0000ba  74cb              STRB     r3,[r1,#0x13]
;;;627    	
;;;628    	ControlOutputValue->SetMoveSpeed					= 0;
0000bc  6013              STR      r3,[r2,#0]
;;;629    	ControlOutputValue->SetLiftSpeedOne					= 0;
0000be  6053              STR      r3,[r2,#4]
;;;630    	ControlOutputValue->SetLiftSpeedTwo					= 0;
0000c0  6093              STR      r3,[r2,#8]
;;;631    	ControlOutputValue->AgvSettingFirstSpeed			= 0;
0000c2  8193              STRH     r3,[r2,#0xc]
;;;632    	ControlOutputValue->AgvSettingSecondSpeed			= 0;
0000c4  81d3              STRH     r3,[r2,#0xe]
;;;633    }
0000c6  bd10              POP      {r4,pc}
;;;634    
                          ENDP

                  |L3.200|
                          DCD      pSetup
                  |L3.204|
0000cc  3c75c28f          DCFS     0x3c75c28f ; 0.014999999664723873
                  |L3.208|
                          DCD      ||.bss||+0x1c20
                  |L3.212|
0000d4  00000000          DCFS     0x00000000 ; 0

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

                  SetMoveDriveSpeed PROC
;;;13     //速度设定 0-400
;;;14     void SetMoveDriveSpeed(int MoveSpeed,int LiftSpeed)
000000  e92d41ff          PUSH     {r0-r8,lr}
;;;15     {
;;;16     	int M_SPEED = 0,L_SPEED;
;;;17     	unsigned char SetDriveSpeed_A[] 	= {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
000004  a25c              ADR      r2,|L4.376|
000006  ca0c              LDM      r2,{r2,r3}
000008  e9cd2302          STRD     r2,r3,[sp,#8]
;;;18     	unsigned char SetDriveSpeed_B[] 	= {0x01,0x80,0x00,0x00,0x00,0x00,0x00,0x00};
00000c  a25c              ADR      r2,|L4.384|
00000e  ca0c              LDM      r2,{r2,r3}
000010  e9cd2300          STRD     r2,r3,[sp,#0]
;;;19     	
;;;20     	M_SPEED = MoveSpeed*10;
000014  eb000280          ADD      r2,r0,r0,LSL #2
000018  0055              LSLS     r5,r2,#1
;;;21     	
;;;22     	L_SPEED = LiftSpeed*10;
00001a  eb010281          ADD      r2,r1,r1,LSL #2
00001e  0056              LSLS     r6,r2,#1
000020  eb000080          ADD      r0,r0,r0,LSL #2       ;20
000024  0040              LSLS     r0,r0,#1              ;20
;;;23     	
;;;24     //	Uart_Printf(COM1,"M_SPEED= %d\r\n",M_SPEED);
;;;25     	if(M_SPEED && (L_SPEED == 0))
;;;26     	{
;;;27     		SetDriveSpeed_A[0] = 0;
;;;28     		SetDriveSpeed_B[0] = 0;
;;;29     		SetDriveSpeed_A[2] = M_SPEED & 0XFF;
000026  b2c3              UXTB     r3,r0
;;;30     		SetDriveSpeed_A[3] = (M_SPEED >> 8) & 0XFF;
000028  f3c02407          UBFX     r4,r0,#8,#8
;;;31     		SetDriveSpeed_B[2] = M_SPEED & 0XFF;
;;;32     		SetDriveSpeed_B[3] = (M_SPEED >> 8) & 0XFF;
;;;33     		
;;;34     		//交替发送 A B
;;;35     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_A,8,15);
00002c  f44f77f0          MOV      r7,#0x1e0
000030  2d00              CMP      r5,#0                 ;25
000032  d000              BEQ      |L4.54|
000034  b13e              CBZ      r6,|L4.70|
                  |L4.54|
000036  eb010081          ADD      r0,r1,r1,LSL #2       ;22
00003a  0042              LSLS     r2,r0,#1              ;22
;;;36     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_B,8,14);
;;;37     	}
;;;38     	else if((M_SPEED == 0) && L_SPEED)
;;;39     	{
;;;40     		if(L_SPEED > 0)
;;;41     		{
;;;42     			SetDriveSpeed_A[0] = 0x41;
;;;43     			SetDriveSpeed_B[0] = 0x41;
;;;44     			SetDriveSpeed_A[6] = L_SPEED & 0XFF;
00003c  b2d1              UXTB     r1,r2
;;;45     			SetDriveSpeed_A[7] = (L_SPEED >> 8) & 0XFF;
00003e  f3c22207          UBFX     r2,r2,#8,#8
000042  b1dd              CBZ      r5,|L4.124|
000044  e049              B        |L4.218|
                  |L4.70|
000046  2000              MOVS     r0,#0                 ;27
000048  f88d0008          STRB     r0,[sp,#8]            ;27
00004c  f88d0000          STRB     r0,[sp,#0]            ;28
000050  f88d300a          STRB     r3,[sp,#0xa]          ;29
000054  f88d400b          STRB     r4,[sp,#0xb]          ;30
000058  f88d3002          STRB     r3,[sp,#2]            ;31
00005c  f88d4003          STRB     r4,[sp,#3]            ;32
000060  230f              MOVS     r3,#0xf               ;35
000062  2208              MOVS     r2,#8                 ;35
000064  a902              ADD      r1,sp,#8              ;35
000066  4638              MOV      r0,r7                 ;35
000068  f7fffffe          BL       CAN1_Send_Msg
00006c  230e              MOVS     r3,#0xe               ;36
00006e  2208              MOVS     r2,#8                 ;36
000070  4669              MOV      r1,sp                 ;36
000072  4638              MOV      r0,r7                 ;36
000074  f7fffffe          BL       CAN1_Send_Msg
                  |L4.120|
;;;46     			SetDriveSpeed_B[6] = L_SPEED & 0XFF;
;;;47     			SetDriveSpeed_B[7] = (L_SPEED >> 8) & 0XFF;
;;;48     		}
;;;49     		else if(L_SPEED < 0)
;;;50     		{
;;;51     			SetDriveSpeed_A[0] = 0x81;
;;;52     			SetDriveSpeed_B[0] = 0x81;
;;;53     			SetDriveSpeed_A[6] = L_SPEED & 0XFF;
;;;54     			SetDriveSpeed_A[7] = (L_SPEED >> 8) & 0XFF;
;;;55     			SetDriveSpeed_B[6] = L_SPEED & 0XFF;
;;;56     			SetDriveSpeed_B[7] = (L_SPEED >> 8) & 0XFF;
;;;57     		}
;;;58     
;;;59     		//交替发送 A B
;;;60     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_A,8,15);
;;;61     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_B,8,14);	
;;;62     	}
;;;63     	else if(M_SPEED && L_SPEED)
;;;64     	{
;;;65     		if(L_SPEED > 0)
;;;66     		{
;;;67     			SetDriveSpeed_A[0] = 0x40;
;;;68     			SetDriveSpeed_B[0] = 0x40;
;;;69     			SetDriveSpeed_A[2] = M_SPEED & 0XFF;
;;;70     			SetDriveSpeed_A[3] = (M_SPEED >> 8) & 0XFF;
;;;71     			SetDriveSpeed_B[2] = M_SPEED & 0XFF;
;;;72     			SetDriveSpeed_B[3] = (M_SPEED >> 8) & 0XFF;			
;;;73     			SetDriveSpeed_A[6] = L_SPEED & 0XFF;
;;;74     			SetDriveSpeed_A[7] = (L_SPEED >> 8) & 0XFF;
;;;75     			SetDriveSpeed_B[6] = L_SPEED & 0XFF;
;;;76     			SetDriveSpeed_B[7] = (L_SPEED >> 8) & 0XFF;
;;;77     		}
;;;78     		else if(L_SPEED < 0)
;;;79     		{
;;;80     			SetDriveSpeed_A[0] = 0x80;
;;;81     			SetDriveSpeed_B[0] = 0x80;
;;;82     			SetDriveSpeed_A[2] = M_SPEED & 0XFF;
;;;83     			SetDriveSpeed_A[3] = (M_SPEED >> 8) & 0XFF;
;;;84     			SetDriveSpeed_B[2] = M_SPEED & 0XFF;
;;;85     			SetDriveSpeed_B[3] = (M_SPEED >> 8) & 0XFF;			
;;;86     			
;;;87     			SetDriveSpeed_A[6] = L_SPEED & 0XFF;
;;;88     			SetDriveSpeed_A[7] = (L_SPEED >> 8) & 0XFF;
;;;89     			SetDriveSpeed_B[6] = L_SPEED & 0XFF;
;;;90     			SetDriveSpeed_B[7] = (L_SPEED >> 8) & 0XFF;
;;;91     		}
;;;92     		//交替发送 A B
;;;93     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_A,8,15);
;;;94     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_B,8,14);		
;;;95     	}
;;;96     	else//((M_SPEED == 0) && (L_SPEED == 0))
;;;97     	{
;;;98     		//发送A delay(15) 发送B
;;;99     		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_A,8,15);
;;;100    		CAN1_Send_Msg(SPEEDCTR_ID,SetDriveSpeed_B,8,14);
;;;101    	}	
;;;102    }
000078  e8bd81ff          POP      {r0-r8,pc}
                  |L4.124|
00007c  2e00              CMP      r6,#0                 ;38
00007e  d02c              BEQ      |L4.218|
000080  dd0e              BLE      |L4.160|
000082  f04f0041          MOV      r0,#0x41              ;42
000086  f88d0008          STRB     r0,[sp,#8]            ;42
00008a  f88d0000          STRB     r0,[sp,#0]            ;43
00008e  f88d100e          STRB     r1,[sp,#0xe]          ;44
000092  f88d200f          STRB     r2,[sp,#0xf]          ;45
000096  f88d1006          STRB     r1,[sp,#6]            ;46
00009a  f88d2007          STRB     r2,[sp,#7]            ;47
00009e  e00d              B        |L4.188|
                  |L4.160|
0000a0  f04f0081          MOV      r0,#0x81              ;51
0000a4  f88d0008          STRB     r0,[sp,#8]            ;51
0000a8  f88d0000          STRB     r0,[sp,#0]            ;52
0000ac  f88d100e          STRB     r1,[sp,#0xe]          ;53
0000b0  f88d200f          STRB     r2,[sp,#0xf]          ;54
0000b4  f88d1006          STRB     r1,[sp,#6]            ;55
0000b8  f88d2007          STRB     r2,[sp,#7]            ;56
                  |L4.188|
0000bc  f04f030f          MOV      r3,#0xf               ;60
0000c0  f04f0208          MOV      r2,#8                 ;60
0000c4  a902              ADD      r1,sp,#8              ;60
0000c6  4638              MOV      r0,r7                 ;60
0000c8  f7fffffe          BL       CAN1_Send_Msg
0000cc  230e              MOVS     r3,#0xe               ;61
0000ce  2208              MOVS     r2,#8                 ;61
0000d0  4669              MOV      r1,sp                 ;61
0000d2  4638              MOV      r0,r7                 ;61
0000d4  f7fffffe          BL       CAN1_Send_Msg
0000d8  e7ce              B        |L4.120|
                  |L4.218|
0000da  b3e5              CBZ      r5,|L4.342|
0000dc  2e00              CMP      r6,#0                 ;63
0000de  d03e              BEQ      |L4.350|
0000e0  dd16              BLE      |L4.272|
0000e2  f04f0040          MOV      r0,#0x40              ;67
0000e6  f88d0008          STRB     r0,[sp,#8]            ;67
0000ea  f88d0000          STRB     r0,[sp,#0]            ;68
0000ee  f88d300a          STRB     r3,[sp,#0xa]          ;69
0000f2  f88d400b          STRB     r4,[sp,#0xb]          ;70
0000f6  f88d3002          STRB     r3,[sp,#2]            ;71
0000fa  f88d4003          STRB     r4,[sp,#3]            ;72
0000fe  f88d100e          STRB     r1,[sp,#0xe]          ;73
000102  f88d200f          STRB     r2,[sp,#0xf]          ;74
000106  f88d1006          STRB     r1,[sp,#6]            ;75
00010a  f88d2007          STRB     r2,[sp,#7]            ;76
00010e  e015              B        |L4.316|
                  |L4.272|
000110  f04f0080          MOV      r0,#0x80              ;80
000114  f88d0008          STRB     r0,[sp,#8]            ;80
000118  f88d0000          STRB     r0,[sp,#0]            ;81
00011c  f88d300a          STRB     r3,[sp,#0xa]          ;82
000120  f88d400b          STRB     r4,[sp,#0xb]          ;83
000124  f88d3002          STRB     r3,[sp,#2]            ;84
000128  f88d4003          STRB     r4,[sp,#3]            ;85
00012c  f88d100e          STRB     r1,[sp,#0xe]          ;87
000130  f88d200f          STRB     r2,[sp,#0xf]          ;88
000134  f88d1006          STRB     r1,[sp,#6]            ;89
000138  f88d2007          STRB     r2,[sp,#7]            ;90
                  |L4.316|
00013c  f04f030f          MOV      r3,#0xf               ;93
000140  f04f0208          MOV      r2,#8                 ;93
000144  a902              ADD      r1,sp,#8              ;93
000146  4638              MOV      r0,r7                 ;93
000148  f7fffffe          BL       CAN1_Send_Msg
00014c  230e              MOVS     r3,#0xe               ;94
00014e  2208              MOVS     r2,#8                 ;94
000150  4669              MOV      r1,sp                 ;94
000152  4638              MOV      r0,r7                 ;94
000154  e000              B        |L4.344|
                  |L4.342|
000156  e002              B        |L4.350|
                  |L4.344|
000158  f7fffffe          BL       CAN1_Send_Msg
00015c  e78c              B        |L4.120|
                  |L4.350|
00015e  230f              MOVS     r3,#0xf               ;99
000160  2208              MOVS     r2,#8                 ;99
000162  a902              ADD      r1,sp,#8              ;99
000164  4638              MOV      r0,r7                 ;99
000166  f7fffffe          BL       CAN1_Send_Msg
00016a  230e              MOVS     r3,#0xe               ;100
00016c  2208              MOVS     r2,#8                 ;100
00016e  4669              MOV      r1,sp                 ;100
000170  4638              MOV      r0,r7                 ;100
000172  f7fffffe          BL       CAN1_Send_Msg
000176  e77f              B        |L4.120|
;;;103    unsigned char agvNAV = 0;
                          ENDP

                  |L4.376|
000178  0100              DCB      1,0
00017a  00                DCB      0
00017b  00                DCB      0
00017c  00                DCB      0
00017d  00                DCB      0
00017e  00                DCB      0
00017f  00                DCB      0
                  |L4.384|
000180  018000            DCB      1,128,0
000183  00                DCB      0
000184  00                DCB      0
000185  00                DCB      0
000186  00                DCB      0
000187  00                DCB      0

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

                  agvControlParameterInput PROC
;;;107    //需求信息
;;;108    void agvControlParameterInput(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue)
000000  b530              PUSH     {r4,r5,lr}
;;;109    {
;;;110    	//实时参数
;;;111    	ControlInputValue->HighSp_F_offset = 			0.1f;			
000002  ed9f0a47          VLDR     s0,|L5.288|
000006  ed810a22          VSTR     s0,[r1,#0x88]
;;;112    	ControlInputValue->HighSp_F_angle = 			30.0f;			
00000a  eef30a0e          VMOV.F32 s1,#30.00000000
00000e  edc10a23          VSTR     s1,[r1,#0x8c]
;;;113    	ControlInputValue->HighSp_B_offset = 			0.1f;			
000012  ed810a24          VSTR     s0,[r1,#0x90]
;;;114    	ControlInputValue->HighSp_B_angle = 			40.0f;			
000016  eddf0a43          VLDR     s1,|L5.292|
00001a  edc10a25          VSTR     s1,[r1,#0x94]
;;;115    	ControlInputValue->HighSpCurve = 				0;			
00001e  ed9f1a42          VLDR     s2,|L5.296|
000022  ed811a26          VSTR     s2,[r1,#0x98]
;;;116    
;;;117    	ControlInputValue->LowSp_F_offset = 			0.1f;//HandlePlcData.RecvPlcData.P;			
000026  ed810a27          VSTR     s0,[r1,#0x9c]
;;;118    	ControlInputValue->LowSp_F_angle = 				40;//HandlePlcData.RecvPlcData.I;		
00002a  edc10a28          VSTR     s1,[r1,#0xa0]
;;;119    	ControlInputValue->LowSp_B_offset = 			0.1f;			
00002e  ed810a29          VSTR     s0,[r1,#0xa4]
;;;120    	ControlInputValue->LowSp_B_angle = 				40;			
000032  edc10a2a          VSTR     s1,[r1,#0xa8]
;;;121    	ControlInputValue->LowSpCurve = 				0.1f;			
000036  ed810a2b          VSTR     s0,[r1,#0xac]
;;;122    	
;;;123    		//可调节参数
;;;124    	ControlInputValue->SensorToFrontAxle=			pSetup[SETUP_FRONTSTEERING_X_DIS];			//传感器距离前桥垂直距离
00003a  4a3c              LDR      r2,|L5.300|
00003c  6813              LDR      r3,[r2,#0]  ; pSetup
00003e  ed930a35          VLDR     s0,[r3,#0xd4]
000042  eeb80a40          VCVT.F32.U32 s0,s0
000046  ed810a17          VSTR     s0,[r1,#0x5c]
;;;125    	ControlInputValue->SensorToCenterLine=			pSetup[SETUP_FRONTSTEERING_X_DIS];			//传感器距离行走中线垂直距离
00004a  ed930a35          VLDR     s0,[r3,#0xd4]
00004e  eeb80a40          VCVT.F32.U32 s0,s0
000052  ed810a18          VSTR     s0,[r1,#0x60]
;;;126    	ControlInputValue->SensorAngleDeviation=		pSetup[SETUP_FRONTSTEERING_X_DIS];			//传感器与AGV角度偏差
000056  ed930a35          VLDR     s0,[r3,#0xd4]
00005a  eeb80a40          VCVT.F32.U32 s0,s0
00005e  ed810a19          VSTR     s0,[r1,#0x64]
;;;127    	//实时数据
;;;128    			
;;;129    	agvNAV = HandlePlcData.RecvPlcData.NavcitionMode;	
000062  4a33              LDR      r2,|L5.304|
000064  4d33              LDR      r5,|L5.308|
000066  7b94              LDRB     r4,[r2,#0xe]  ; HandlePlcData
000068  702c              STRB     r4,[r5,#0]
;;;130    	//1.激光导航 X、Y、W
;;;131    	ControlInputValue->AgvXoffset			= HandlePlcData.RecvPlcData.agv_X_offset;
00006a  ed920a00          VLDR     s0,[r2,#0]
00006e  ed810a14          VSTR     s0,[r1,#0x50]
;;;132    	ControlInputValue->AgvYoffset			= HandlePlcData.RecvPlcData.agv_Y_offset;
000072  ed920a01          VLDR     s0,[r2,#4]
000076  ed810a15          VSTR     s0,[r1,#0x54]
;;;133    	ControlInputValue->AgvWoffset			= HandlePlcData.RecvPlcData.agv_W_offset;
00007a  ed920a02          VLDR     s0,[r2,#8]
00007e  ed810a16          VSTR     s0,[r1,#0x58]
;;;134    	
;;;135    	//2.二维码
;;;136    	
;;;137    	
;;;138    	//3.惯导
;;;139    	
;;;140    	
;;;141    	//PLC
;;;142    	ControlInputValue->agvControlStatus		= HandlePlcData.RecvPlcData.ManualAutomaticSignal;		
000082  7b14              LDRB     r4,[r2,#0xc]  ; HandlePlcData
000084  f88140b4          STRB     r4,[r1,#0xb4]
;;;143    	ControlInputValue->agvStartAndStop		= HandlePlcData.RecvPlcData.agvstart;		
000088  7b54              LDRB     r4,[r2,#0xd]  ; HandlePlcData
00008a  3a40              SUBS     r2,r2,#0x40
00008c  f88140b5          STRB     r4,[r1,#0xb5]
;;;144    	
;;;145    	if(pSetup[SETUP_REMODEL_AGV_OPEN])
000090  f8d33168          LDR      r3,[r3,#0x168]
;;;146    	{
;;;147    		CalculationTurningSteering->VirtualIntermediateAcAngle = ControlInputValue->AgvFirstRudderWheelAngle / 180.0f *PI;	
000094  ed9f0a28          VLDR     s0,|L5.312|
000098  eddf0a28          VLDR     s1,|L5.316|
00009c  2b00              CMP      r3,#0                 ;145
00009e  d008              BEQ      |L5.178|
0000a0  ed911a09          VLDR     s2,[r1,#0x24]
0000a4  eec11a00          VDIV.F32 s3,s2,s0
0000a8  ee210aa0          VMUL.F32 s0,s3,s1
0000ac  ed800a1c          VSTR     s0,[r0,#0x70]
;;;148    	}
;;;149    	else
;;;150    	{
;;;151    		ControlInputValue->Direction			= HandlePlcData.RecvPlcData.Direction;
;;;152    		ControlInputValue->AgvActualFirstSpeed = HandlePlcData.RecvPlcData.FirstWheelSetSpeed; 
;;;153    		ControlInputValue->AgvActualSecondSpeed = HandlePlcData.RecvPlcData.SecondWheelSetSpeed; 
;;;154    		ControlInputValue->AgvActualThirdSpeed = HandlePlcData.RecvPlcData.ThirdWheelSetSpeed; 
;;;155    		ControlInputValue->AgvActualFourthSpeed = HandlePlcData.RecvPlcData.FourthWheelSetSpeed; 
;;;156    	
;;;157    		ControlInputValue->AgvFirstRudderWheelAngle	= (90.0f - HandlePlcData.RecvPlcData.FirstWheelTarAngle) /180.0f *PI;
;;;158    		ControlInputValue->AgvSecondRudderWheelAngle = (90.0f - HandlePlcData.RecvPlcData.SecondWheelTarAngle) /180.0f *PI;
;;;159    		
;;;160    		ControlInputValue->AgvThirdRudderWheelAngle	= (180.0f - HandlePlcData.RecvPlcData.ThirdWheelTarAngle) /180.0f *PI;
;;;161    		ControlInputValue->AgvFourthRudderWheelAngle = (180.0f - HandlePlcData.RecvPlcData.FourthWheelTarAngle) /180.0f *PI;	
;;;162    	}
;;;163    
;;;164    //	ControlInputValue->AgvActualFirstDis	= HandlePlcData.RecvPlcData.FrontWheelAcDis;
;;;165    //	ControlInputValue->AgvActualSecondDis	= HandlePlcData.RecvPlcData.BackWheelAcDis;
;;;166    }
0000b0  bd30              POP      {r4,r5,pc}
                  |L5.178|
0000b2  f892007d          LDRB     r0,[r2,#0x7d]         ;151  ; HandlePlcData
0000b6  f88100ba          STRB     r0,[r1,#0xba]         ;151
0000ba  8810              LDRH     r0,[r2,#0]            ;152  ; HandlePlcData
0000bc  8188              STRH     r0,[r1,#0xc]          ;152
0000be  8850              LDRH     r0,[r2,#2]            ;153  ; HandlePlcData
0000c0  81c8              STRH     r0,[r1,#0xe]          ;153
0000c2  8890              LDRH     r0,[r2,#4]            ;154  ; HandlePlcData
0000c4  8208              STRH     r0,[r1,#0x10]         ;154
0000c6  88d0              LDRH     r0,[r2,#6]            ;155  ; HandlePlcData
0000c8  8248              STRH     r0,[r1,#0x12]         ;155
0000ca  edd21a02          VLDR     s3,[r2,#8]            ;157
0000ce  ed9f1a1c          VLDR     s2,|L5.320|
0000d2  ee312a61          VSUB.F32 s4,s2,s3              ;157
0000d6  eec21a00          VDIV.F32 s3,s4,s0              ;157
0000da  ee611aa0          VMUL.F32 s3,s3,s1              ;157
0000de  edc11a09          VSTR     s3,[r1,#0x24]         ;157
0000e2  edd21a03          VLDR     s3,[r2,#0xc]          ;158
0000e6  ee711a61          VSUB.F32 s3,s2,s3              ;158
0000ea  ee811a80          VDIV.F32 s2,s3,s0              ;158
0000ee  ee211a20          VMUL.F32 s2,s2,s1              ;158
0000f2  ed811a0a          VSTR     s2,[r1,#0x28]         ;158
0000f6  ed921a04          VLDR     s2,[r2,#0x10]         ;160
0000fa  ee701a41          VSUB.F32 s3,s0,s2              ;160
0000fe  ee811a80          VDIV.F32 s2,s3,s0              ;160
000102  ee211a20          VMUL.F32 s2,s2,s1              ;160
000106  ed811a0b          VSTR     s2,[r1,#0x2c]         ;160
00010a  ed921a05          VLDR     s2,[r2,#0x14]         ;161
00010e  ee301a41          VSUB.F32 s2,s0,s2              ;161
000112  eec11a00          VDIV.F32 s3,s2,s0              ;161
000116  ee210aa0          VMUL.F32 s0,s3,s1              ;161
00011a  ed810a0c          VSTR     s0,[r1,#0x30]         ;161
00011e  bd30              POP      {r4,r5,pc}
;;;167    
                          ENDP

                  |L5.288|
000120  3dcccccd          DCFS     0x3dcccccd ; 0.10000000149011612
                  |L5.292|
000124  42200000          DCFS     0x42200000 ; 40
                  |L5.296|
000128  00000000          DCFS     0x00000000 ; 0
                  |L5.300|
                          DCD      pSetup
                  |L5.304|
                          DCD      HandlePlcData+0x40
                  |L5.308|
                          DCD      ||.data||
                  |L5.312|
000138  43340000          DCFS     0x43340000 ; 180
                  |L5.316|
00013c  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L5.320|
000140  42b40000          DCFS     0x42b40000 ; 90

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

                  agvControlParameterOutput PROC
;;;168    //更新信息
;;;169    void agvControlParameterOutput(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  b5f0              PUSH     {r4-r7,lr}
;;;170    {
;;;171    	//驱动器走CAN总线
;;;172    //	if(ControlInputValue->agvControlStatus)
;;;173    //	{
;;;174    //		Driver_first_motor.Command.speed = HandlePlcData.RecvPlcData.FirstWheelSetSpeed;
;;;175    //		Driver_second_motor.Command.speed = HandlePlcData.RecvPlcData.SecondWheelSetSpeed;
;;;176    //	}
;;;177    //	else
;;;178    //	{
;;;179    //		Driver_first_motor.Command.speed = ControlOutputValue->AgvSettingFirstSpeed;
;;;180    //		Driver_second_motor.Command.speed = ControlOutputValue->AgvSettingSecondSpeed;	
;;;181    //	}
;;;182    //	HandlePlcData.SendPlcData.QR_X				= (int)ControlInputValue->X_coordinates;
;;;183    //	HandlePlcData.SendPlcData.QR_Y				= (int)ControlInputValue->Y_coordinates;
;;;184    //	HandlePlcData.SendPlcData.QR_W				= ControlInputValue->W_coordinates;
;;;185    	
;;;186    	HandlePlcData.SendPlcData.SlamOnlineStatus = ControlInputValue->SlamOnline;
000002  4b55              LDR      r3,|L6.344|
000004  f891407d          LDRB     r4,[r1,#0x7d]
000008  f88340fa          STRB     r4,[r3,#0xfa]
;;;187    	HandlePlcData.SendPlcData.FirstWheelTarAngle = -(CalculationTurningSteering->VirtualIntermediateTarAngle * 180.0f/PI) + 90; 
00000c  edd00a1d          VLDR     s1,[r0,#0x74]
000010  ed9f0a52          VLDR     s0,|L6.348|
000014  ee201a80          VMUL.F32 s2,s1,s0
000018  eddf0a51          VLDR     s1,|L6.352|
00001c  eec11a20          VDIV.F32 s3,s2,s1
000020  ed9f1a50          VLDR     s2,|L6.356|
000024  ee311a61          VSUB.F32 s2,s2,s3
000028  ed831a38          VSTR     s2,[r3,#0xe0]
;;;188    	
;;;189    	HandlePlcData.SendPlcData.FirstWheelAcSpeed = ControlOutputValue->AgvSettingFirstSpeed;
00002c  8994              LDRH     r4,[r2,#0xc]
00002e  f8a340a8          STRH     r4,[r3,#0xa8]
;;;190    	HandlePlcData.SendPlcData.SecondWheelAcSpeed = ControlOutputValue->AgvSettingSecondSpeed;
000032  89d4              LDRH     r4,[r2,#0xe]
000034  f8a340aa          STRH     r4,[r3,#0xaa]
;;;191    	
;;;192    	HandlePlcData.SendPlcData.CurrentID = DispatchData.CurSite;
000038  4c4b              LDR      r4,|L6.360|
00003a  8864              LDRH     r4,[r4,#2]  ; DispatchData
00003c  f8a340f4          STRH     r4,[r3,#0xf4]
;;;193    
;;;194    	HandlePlcData.SendPlcData.AgvOffset = ControlOutputValue->AgvOffset;
000040  ed921a09          VLDR     s2,[r2,#0x24]
000044  ed831a44          VSTR     s2,[r3,#0x110]
;;;195    	HandlePlcData.SendPlcData.AgvAngleOffset = ControlOutputValue->AgvAngleOffset;
000048  ed921a0a          VLDR     s2,[r2,#0x28]
00004c  ed831a45          VSTR     s2,[r3,#0x114]
;;;196    	
;;;197    	//改造叉车
;;;198    	//角度	
;;;199    	if(HandlePlcData.RecvPlcData.FirstLiftSetSpeed > 0)
000050  f9b36018          LDRSH    r6,[r3,#0x18]  ; HandlePlcData
;;;200    	{
;;;201    		RemodelThrModel.LiftPosition = 	abs(HandlePlcData.RecvPlcData.FirstLiftSetSpeed);	
;;;202    		RemodelThrModel.LiftDirection = 1;
000054  2701              MOVS     r7,#1
;;;203    	}
;;;204    	else if(HandlePlcData.RecvPlcData.FirstLiftSetSpeed < 0)
;;;205    	{
;;;206    		RemodelThrModel.LiftPosition = 	abs(HandlePlcData.RecvPlcData.FirstLiftSetSpeed);
;;;207    		RemodelThrModel.LiftDirection = 2;
000056  2500              MOVS     r5,#0
000058  4c44              LDR      r4,|L6.364|
00005a  f04f0c02          MOV      r12,#2
00005e  2e00              CMP      r6,#0                 ;199
000060  dd02              BLE      |L6.104|
000062  81a6              STRH     r6,[r4,#0xc]          ;201
000064  72a7              STRB     r7,[r4,#0xa]          ;202
000066  e008              B        |L6.122|
                  |L6.104|
000068  da05              BGE      |L6.118|
00006a  f1c60600          RSB      r6,r6,#0              ;206
00006e  81a6              STRH     r6,[r4,#0xc]          ;206
000070  f884c00a          STRB     r12,[r4,#0xa]
000074  e001              B        |L6.122|
                  |L6.118|
;;;208    	}
;;;209    	else 
;;;210    	{
;;;211    		RemodelThrModel.LiftDirection = 0;
000076  72a5              STRB     r5,[r4,#0xa]
;;;212    		RemodelThrModel.LiftPosition = 0;
000078  81a5              STRH     r5,[r4,#0xc]
                  |L6.122|
;;;213    	}
;;;214    	
;;;215    	if(ControlInputValue->agvControlStatus)
00007a  f89160b4          LDRB     r6,[r1,#0xb4]
00007e  b3a6              CBZ      r6,|L6.234|
;;;216    	{
;;;217    		RemodelThrModel.AnglePosition = -(CalculationTurningSteering->VirtualIntermediateTarAngle * 180.0f/PI)*100; 
000080  ed901a1d          VLDR     s2,[r0,#0x74]
000084  ee211a00          VMUL.F32 s2,s2,s0
000088  ee810a20          VDIV.F32 s0,s2,s1
00008c  eddf0a38          VLDR     s1,|L6.368|
000090  ee200a20          VMUL.F32 s0,s0,s1
000094  eebd0ac0          VCVT.S32.F32 s0,s0
000098  ee100a10          VMOV     r0,s0
00009c  8120              STRH     r0,[r4,#8]
;;;218    		
;;;219    		if(ControlInputValue->agvStartAndStop)
00009e  f89100b5          LDRB     r0,[r1,#0xb5]
0000a2  b128              CBZ      r0,|L6.176|
;;;220    		{
;;;221    			RemodelThrModel.MoveSpeed	  = abs(ControlOutputValue->SetMoveSpeed);
0000a4  6810              LDR      r0,[r2,#0]
0000a6  2800              CMP      r0,#0
0000a8  da00              BGE      |L6.172|
0000aa  4240              RSBS     r0,r0,#0
                  |L6.172|
0000ac  8060              STRH     r0,[r4,#2]
0000ae  e000              B        |L6.178|
                  |L6.176|
;;;222    		}
;;;223    		else RemodelThrModel.MoveSpeed = 0;
0000b0  8065              STRH     r5,[r4,#2]
                  |L6.178|
;;;224    		
;;;225    		if((ControlInputValue->CurDirection == 1) || (ControlInputValue->CurDirection == 3) || (ControlInputValue->CurDirection == 4))
0000b2  f9b100d0          LDRSH    r0,[r1,#0xd0]
0000b6  2801              CMP      r0,#1
0000b8  d018              BEQ      |L6.236|
0000ba  2803              CMP      r0,#3
0000bc  d016              BEQ      |L6.236|
0000be  2804              CMP      r0,#4
0000c0  d014              BEQ      |L6.236|
;;;226    		{
;;;227    			RemodelThrModel.MoveDirection = 1;
;;;228    		}
;;;229    		else if((ControlInputValue->CurDirection == 2) || (ControlInputValue->CurDirection == 5) || (ControlInputValue->CurDirection == 6))
0000c2  2802              CMP      r0,#2
0000c4  d014              BEQ      |L6.240|
0000c6  2805              CMP      r0,#5
0000c8  d012              BEQ      |L6.240|
0000ca  2806              CMP      r0,#6
0000cc  d010              BEQ      |L6.240|
;;;230    		{
;;;231    			RemodelThrModel.MoveDirection = 2;
;;;232    		}
;;;233    		else RemodelThrModel.MoveDirection = 0;
0000ce  7025              STRB     r5,[r4,#0]
                  |L6.208|
;;;234    		
;;;235    		if(RemodelThrModel.LiftDirection)
0000d0  7aa0              LDRB     r0,[r4,#0xa]  ; RemodelThrModel
0000d2  b108              CBZ      r0,|L6.216|
;;;236    		{
;;;237    			RemodelThrModel.MoveDirection = 0;	
0000d4  7025              STRB     r5,[r4,#0]
;;;238    			RemodelThrModel.MoveSpeed = 0;
0000d6  8065              STRH     r5,[r4,#2]
                  |L6.216|
;;;239    		}
;;;240    		
;;;241    		if(HandlePlcData.RecvPlcData.RadarSignal == 1)//减速
0000d8  f8930058          LDRB     r0,[r3,#0x58]  ; HandlePlcData
0000dc  2801              CMP      r0,#1
0000de  d00a              BEQ      |L6.246|
;;;242    		{
;;;243    			RemodelThrModel.MoveSpeed	  = abs(HandlePlcData.RecvPlcData.RadarDecSpeed);
;;;244    		}
;;;245    		else if(HandlePlcData.RecvPlcData.RadarSignal == 2)//停车
0000e0  2802              CMP      r0,#2
0000e2  d101              BNE      |L6.232|
;;;246    		{
;;;247    			RemodelThrModel.MoveSpeed = 0;
0000e4  8065              STRH     r5,[r4,#2]
;;;248    			RemodelThrModel.MoveDirection = 0;	 
0000e6  7025              STRB     r5,[r4,#0]
                  |L6.232|
;;;249    		}
;;;250    		//
;;;251    	}
;;;252    	else
;;;253    	{
;;;254    		if(HandlePlcData.RecvPlcData.FirstWheelSetSpeed > 0)
;;;255    		{
;;;256    			RemodelThrModel.MoveSpeed = abs(HandlePlcData.RecvPlcData.FirstWheelSetSpeed);
;;;257    			RemodelThrModel.AnglePosition = (short)(HandlePlcData.RecvPlcData.FirstWheelTarAngle - 90) * 100;
;;;258    			RemodelThrModel.MoveDirection = 1;
;;;259    		}
;;;260    		else if(HandlePlcData.RecvPlcData.FirstWheelSetSpeed < 0)
;;;261    		{
;;;262    			RemodelThrModel.MoveSpeed = abs(HandlePlcData.RecvPlcData.FirstWheelSetSpeed);
;;;263    			RemodelThrModel.AnglePosition = (short)(HandlePlcData.RecvPlcData.FirstWheelTarAngle - 90) * 100;
;;;264    
;;;265    			RemodelThrModel.MoveDirection = 2;
;;;266    		}
;;;267    		else
;;;268    		{
;;;269    			RemodelThrModel.MoveSpeed = 0;
;;;270    			RemodelThrModel.MoveDirection = 0;
;;;271    			RemodelThrModel.AnglePosition = 0;
;;;272    
;;;273    		}
;;;274    	}
;;;275    	
;;;276    //	static int iii = 0;
;;;277    //	if(iii++ >= 50)
;;;278    //	{
;;;279    //		Uart_Printf(COM1,"pSetup=%d	status=%d	speed=%d	POS=%d	dir=%d\r\n",pSetup[SETUP_REMODEL_AGV_OPEN],
;;;280    //		ControlInputValue->agvControlStatus,RemodelThrModel.MoveSpeed,
;;;281    //		RemodelThrModel.AnglePosition,RemodelThrModel.MoveDirection);
;;;282    //		iii = 0;
;;;283    //	}	
;;;284    //	Uart_Printf(COM1,"angle=%f	speed=%d	ang=%d	sp=%d	%d	dir=%d	ControlInputValue->agvStartAndStop=%d\r\n",
;;;285    //	CalculationTurningSteering->VirtualIntermediateTarAngle * 180.0f/PI,ControlOutputValue->SetMoveSpeed,
;;;286    //	RemodelThrModel.AnglePosition,RemodelThrModel.MoveSpeed,RemodelThrModel.MoveDirection,ControlInputValue->CurDirection,ControlInputValue->agvStartAndStop);
;;;287    }
0000e8  bdf0              POP      {r4-r7,pc}
                  |L6.234|
0000ea  e00b              B        |L6.260|
                  |L6.236|
0000ec  7027              STRB     r7,[r4,#0]            ;227
0000ee  e7ef              B        |L6.208|
                  |L6.240|
0000f0  f884c000          STRB     r12,[r4,#0]           ;231
0000f4  e7ec              B        |L6.208|
                  |L6.246|
0000f6  f9b3005a          LDRSH    r0,[r3,#0x5a]         ;243  ; HandlePlcData
0000fa  2800              CMP      r0,#0                 ;243
0000fc  da00              BGE      |L6.256|
0000fe  4240              RSBS     r0,r0,#0              ;243
                  |L6.256|
000100  8060              STRH     r0,[r4,#2]            ;243
000102  e7f1              B        |L6.232|
                  |L6.260|
000104  4914              LDR      r1,|L6.344|
000106  eddf0a17          VLDR     s1,|L6.356|
00010a  f9b30000          LDRSH    r0,[r3,#0]            ;254  ; HandlePlcData
00010e  ed910a02          VLDR     s0,[r1,#8]            ;257
000112  2800              CMP      r0,#0                 ;254
000114  ee300a60          VSUB.F32 s0,s0,s1              ;257
000118  eebd0ac0          VCVT.S32.F32 s0,s0                 ;257
00011c  dd09              BLE      |L6.306|
00011e  8060              STRH     r0,[r4,#2]            ;256
000120  ee100a10          VMOV     r0,s0                 ;257
000124  f04f0164          MOV      r1,#0x64              ;257
000128  fb10f001          SMULBB   r0,r0,r1              ;257
00012c  8120              STRH     r0,[r4,#8]            ;257
00012e  7027              STRB     r7,[r4,#0]            ;258
000130  e7da              B        |L6.232|
                  |L6.306|
000132  da0c              BGE      |L6.334|
000134  f1c00000          RSB      r0,r0,#0              ;262
000138  8060              STRH     r0,[r4,#2]            ;262
00013a  ee100a10          VMOV     r0,s0                 ;263
00013e  f04f0164          MOV      r1,#0x64              ;263
000142  fb10f001          SMULBB   r0,r0,r1              ;263
000146  8120              STRH     r0,[r4,#8]            ;263
000148  f884c000          STRB     r12,[r4,#0]           ;265
00014c  e7cc              B        |L6.232|
                  |L6.334|
00014e  8065              STRH     r5,[r4,#2]            ;269
000150  7025              STRB     r5,[r4,#0]            ;270
000152  8125              STRH     r5,[r4,#8]            ;271
000154  e7c8              B        |L6.232|
;;;288    
                          ENDP

000156  0000              DCW      0x0000
                  |L6.344|
                          DCD      HandlePlcData
                  |L6.348|
00015c  43340000          DCFS     0x43340000 ; 180
                  |L6.352|
000160  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L6.356|
000164  42b40000          DCFS     0x42b40000 ; 90
                  |L6.360|
                          DCD      DispatchData
                  |L6.364|
                          DCD      RemodelThrModel
                  |L6.368|
000170  c2c80000          DCFS     0xc2c80000 ; -100

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

                  agvLaserMotionControlFunction PROC
;;;444    
;;;445    void agvLaserMotionControlFunction(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  e92d4ff0          PUSH     {r4-r11,lr}
;;;446    {
000004  b089              SUB      sp,sp,#0x24
000006  4682              MOV      r10,r0
000008  460c              MOV      r4,r1
00000a  4616              MOV      r6,r2
;;;447    //	Uart_Printf(COM1,"当前站点=%d	next=%d	dir=%d	转弯步骤=%d\r\n",
;;;448    //	DispatchData.CurSite,DispatchData.NextSite,ControlInputValue->CurDirection,LaserControlSignal.TurnningCalStep);
;;;449    //	
;;;450    	static int ii = 0;
;;;451    	if(ii++ > 200)
00000c  4f71              LDR      r7,|L7.468|
00000e  2500              MOVS     r5,#0
000010  6878              LDR      r0,[r7,#4]  ; ii
000012  1c41              ADDS     r1,r0,#1
000014  6079              STR      r1,[r7,#4]  ; ii
000016  28c8              CMP      r0,#0xc8
000018  dd26              BLE      |L7.104|
;;;452    	{
;;;453    		ii = 0;		
00001a  607d              STR      r5,[r7,#4]  ; ii
;;;454    		Uart_Printf(COM1,"当前站点=%d	next=%d	dir=%d	x=%d y=%d	w=%f	上线状态=%d	%d\r\n",
00001c  ed9a0a19          VLDR     s0,[r10,#0x64]
000020  eddf0a6d          VLDR     s1,|L7.472|
000024  ee600a20          VMUL.F32 s1,s0,s1
000028  ed9f1a6c          VLDR     s2,|L7.476|
00002c  ee800a81          VDIV.F32 s0,s1,s2
000030  ee100a10          VMOV     r0,s0
000034  f7fffffe          BL       __aeabi_f2d
000038  f89420b8          LDRB     r2,[r4,#0xb8]
00003c  f894307d          LDRB     r3,[r4,#0x7d]
000040  e9cd3206          STRD     r3,r2,[sp,#0x18]
000044  e9cd0104          STRD     r0,r1,[sp,#0x10]
000048  e9d41000          LDRD     r1,r0,[r4,#0]
00004c  f9b420d0          LDRSH    r2,[r4,#0xd0]
000050  e9cd2100          STRD     r2,r1,[sp,#0]
000054  9002              STR      r0,[sp,#8]
000056  4862              LDR      r0,|L7.480|
000058  a162              ADR      r1,|L7.484|
00005a  f9b03004          LDRSH    r3,[r0,#4]  ; DispatchData
00005e  f9b02002          LDRSH    r2,[r0,#2]  ; DispatchData
000062  2001              MOVS     r0,#1
000064  f7fffffe          BL       Uart_Printf
                  |L7.104|
;;;455    		DispatchData.CurSite,DispatchData.NextSite,
;;;456    		ControlInputValue->CurDirection,
;;;457    		ControlInputValue->X_coordinates,
;;;458    		ControlInputValue->Y_coordinates,CalculationTurningSteering->CurAngle*180.0f/PI,
;;;459    		ControlInputValue->SlamOnline,ControlInputValue->OnlineLogo);
;;;460    	}
;;;461    //		
;;;462    	if(ControlInputValue->agvControlStatus == 0)//手动模式
000068  f89400b4          LDRB     r0,[r4,#0xb4]
00006c  f04f0801          MOV      r8,#1                 ;454
000070  b398              CBZ      r0,|L7.218|
;;;463    	{
;;;464    //		ControlInputValue->OnlineLogo						= 0;
;;;465    //		ControlInputValue->NavcitionMode					= 2;
;;;466    		LaserControlSignal.standSiteID = 1;//ControlInputValue->agvSite;
;;;467    		LaserControlSignal.standLine   = 1;//ControlInputValue->agvLine;		
;;;468    		LaserControlSignal.InSituTurnningStep = 0;
;;;469    		LaserControlSignal.TurnningCalStep = 0;
;;;470    		ControlInputValue->QrcodeLocationingStatus = 0;
;;;471    		
;;;472    		SLAMIntegratedLaserAndIntegralData(CalculationTurningSteering,ControlInputValue);
;;;473    		return;
;;;474    	}
;;;475    	//4.报警信号总结
;;;476    	if(HandlePlcData.RecvPlcData.ClearSiteAction)
000072  f8df91ac          LDR      r9,|L7.544|
;;;477    	{
;;;478    		ControlInputValue->QrcodeLocationingStatus = 0;
;;;479    		Uart_Printf(COM1,"清除站点动作\r\n");
;;;480    		ClearSiteAction(ControlInputValue);
;;;481    		ControlInputValue->OnlineLogo = 0;
;;;482    		HandlePlcData.SendPlcData.ClearSiteAck = 1;
;;;483    		SySAlarmStatus.Off_linestatus = 0;
000076  f8dfb15c          LDR      r11,|L7.468|
00007a  f8990055          LDRB     r0,[r9,#0x55]         ;476  ; HandlePlcData
00007e  f10b0b10          ADD      r11,r11,#0x10
000082  b1a0              CBZ      r0,|L7.174|
000084  f88450b9          STRB     r5,[r4,#0xb9]         ;478
000088  a166              ADR      r1,|L7.548|
00008a  2001              MOVS     r0,#1                 ;479
00008c  f7fffffe          BL       Uart_Printf
000090  4620              MOV      r0,r4                 ;480
000092  f7fffffe          BL       ClearSiteAction
000096  f88450b8          STRB     r5,[r4,#0xb8]         ;481
00009a  f88980fb          STRB     r8,[r9,#0xfb]         ;482
00009e  f88b5001          STRB     r5,[r11,#1]
;;;484    		SySAlarmStatus.OutOffDistanceStatus = 0;
0000a2  f88b5002          STRB     r5,[r11,#2]
;;;485    		CalculationTurningSteering->CurveOffset = 0;
0000a6  ed9f0a63          VLDR     s0,|L7.564|
0000aa  ed8a0a12          VSTR     s0,[r10,#0x48]
                  |L7.174|
;;;486    	}		
;;;487    	
;;;488    	//0.动作下发
;;;489    	if(GetLineActionInformation(CalculationTurningSteering,ControlInputValue) == 0)
0000ae  4621              MOV      r1,r4
0000b0  4650              MOV      r0,r10
0000b2  f7fffffe          BL       GetLineActionInformation
0000b6  b308              CBZ      r0,|L7.252|
;;;490    	{
;;;491    		ControlOutputValue->SetMoveSpeed = 0;
;;;492    		return;
;;;493    	}		
;;;494    	
;;;495    	//2.获取激光以及积分数据
;;;496    	if(ControlInputValue->SlamOr350)//SLAM
0000b8  f894007c          LDRB     r0,[r4,#0x7c]
0000bc  b120              CBZ      r0,|L7.200|
;;;497    	{
;;;498    		if(SLAMIntegratedLaserAndIntegralData(CalculationTurningSteering,ControlInputValue) == 0)
0000be  4621              MOV      r1,r4
0000c0  4650              MOV      r0,r10
0000c2  f7fffffe          BL       SLAMIntegratedLaserAndIntegralData
0000c6  b1e8              CBZ      r0,|L7.260|
                  |L7.200|
;;;499    		{	
;;;500    			ControlOutputValue->SetMoveSpeed = 0;
;;;501    			return;
;;;502    		}	
;;;503    	}
;;;504    	else//NAV350
;;;505    	{
;;;506    		
;;;507    	}
;;;508    	
;;;509    	agvDispatchLineControlFunction(CalculationTurningSteering,ControlInputValue,ControlOutputValue);
0000c8  4632              MOV      r2,r6
0000ca  4621              MOV      r1,r4
0000cc  4650              MOV      r0,r10
0000ce  f7fffffe          BL       agvDispatchLineControlFunction
;;;510    	
;;;511    	if(ControlInputValue->OperationMode == 0)
0000d2  f89400bb          LDRB     r0,[r4,#0xbb]
0000d6  b1b8              CBZ      r0,|L7.264|
0000d8  e057              B        |L7.394|
                  |L7.218|
0000da  e7ff              B        |L7.220|
                  |L7.220|
0000dc  4856              LDR      r0,|L7.568|
0000de  f8808007          STRB     r8,[r0,#7]            ;466
0000e2  f8808008          STRB     r8,[r0,#8]            ;467
0000e6  7145              STRB     r5,[r0,#5]            ;468
0000e8  7105              STRB     r5,[r0,#4]            ;469
0000ea  f88450b9          STRB     r5,[r4,#0xb9]         ;470
0000ee  b009              ADD      sp,sp,#0x24           ;472
0000f0  4621              MOV      r1,r4                 ;472
0000f2  4650              MOV      r0,r10                ;472
0000f4  e8bd4ff0          POP      {r4-r11,lr}           ;472
0000f8  f7ffbffe          B.W      SLAMIntegratedLaserAndIntegralData
                  |L7.252|
0000fc  6035              STR      r5,[r6,#0]            ;491
                  |L7.254|
;;;512    	{
;;;513    		//3.车型选择 导航计算
;;;514    		switch(ControlInputValue->TurnningMode)
;;;515    		{	
;;;516    			case 0://硬差速
;;;517    				
;;;518    				if((ControlInputValue->NavcitionMode == 2) 
;;;519    				| (ControlInputValue->CurDirection == 3) | (ControlInputValue->CurDirection == 4) 
;;;520    				|(ControlInputValue->CurDirection == 5) | (ControlInputValue->CurDirection == 6) 
;;;521    				|(ControlInputValue->CurDirection == 7) | (ControlInputValue->CurDirection == 8))
;;;522    				automaticControlDifferentialSpeed(CalculationTurningSteering,ControlInputValue,ControlOutputValue);		
;;;523    				
;;;524    				break;
;;;525    			case 1://转向架差速
;;;526    
;;;527    				break;
;;;528    			case 2://单舵轮	
;;;529    				automaticControl(CalculationTurningSteering,ControlInputValue,ControlOutputValue);
;;;530    				
;;;531    				break;
;;;532    			case 3://双舵轮
;;;533    				
;;;534    				break;
;;;535    			case 4://三舵轮
;;;536    					
;;;537    				break;
;;;538    			case 5:
;;;539    				break;
;;;540    			case 6:
;;;541    				break;
;;;542    			case 7:
;;;543    				break;
;;;544    			default:
;;;545    				break;
;;;546    		}
;;;547    	}	
;;;548    	
;;;549    	//二维码参照定位
;;;550    	if(ControlInputValue->NavcitionMode == 7)
;;;551    	{
;;;552    		extern CamerParameter camerparameter;
;;;553    		static int CurQRcodeID = 0,LastQRcodeID = 0;
;;;554    		
;;;555    		CurQRcodeID = camerparameter.id;	
;;;556    		if((CurQRcodeID == 4) && (LastQRcodeID == 3))
;;;557    		{
;;;558    			//二维码定位启动
;;;559    			Uart_Printf(COM1,"启动二维码辅助定位\r\n");
;;;560    			ControlInputValue->QrcodeLocationingStatus = 1;	
;;;561    		}
;;;562    		LastQRcodeID = CurQRcodeID;
;;;563    		
;;;564    		if(ControlInputValue->QrcodeLocationingStatus)
;;;565    		{
;;;566    			QecodeSiteChange(ControlInputValue,ControlOutputValue);
;;;567    		}	
;;;568    	}	
;;;569    	
;;;570    	if(HandlePlcData.RecvPlcData.ClearSiteAction)
;;;571    	{
;;;572    		SySAlarmStatus.Off_linestatus = 0;
;;;573    		SySAlarmStatus.OutOffDistanceStatus = 0;
;;;574    		ControlOutputValue->SetMoveSpeed = 0;
;;;575    	}	
;;;576    }
0000fe  b009              ADD      sp,sp,#0x24
000100  e8bd8ff0          POP      {r4-r11,pc}
                  |L7.260|
000104  6035              STR      r5,[r6,#0]            ;500
000106  e7fa              B        |L7.254|
                  |L7.264|
000108  f894003c          LDRB     r0,[r4,#0x3c]         ;514
00010c  2803              CMP      r0,#3                 ;514
00010e  d23c              BCS      |L7.394|
000110  e8dff000          TBB      [pc,r0]               ;514
000114  023b3600          DCB      0x02,0x3b,0x36,0x00
000118  f894003d          LDRB     r0,[r4,#0x3d]         ;518
00011c  2802              CMP      r0,#2                 ;518
00011e  d021              BEQ      |L7.356|
000120  2100              MOVS     r1,#0                 ;518
                  |L7.290|
000122  f9b400d0          LDRSH    r0,[r4,#0xd0]         ;518
000126  2803              CMP      r0,#3                 ;518
000128  d01e              BEQ      |L7.360|
00012a  2200              MOVS     r2,#0                 ;518
                  |L7.300|
00012c  4311              ORRS     r1,r1,r2              ;518
00012e  2804              CMP      r0,#4                 ;518
000130  d01c              BEQ      |L7.364|
000132  2200              MOVS     r2,#0                 ;518
                  |L7.308|
000134  4311              ORRS     r1,r1,r2              ;518
000136  2805              CMP      r0,#5                 ;518
000138  d01a              BEQ      |L7.368|
00013a  2200              MOVS     r2,#0                 ;518
                  |L7.316|
00013c  4311              ORRS     r1,r1,r2              ;518
00013e  2806              CMP      r0,#6                 ;518
000140  d018              BEQ      |L7.372|
000142  2200              MOVS     r2,#0                 ;518
                  |L7.324|
000144  4311              ORRS     r1,r1,r2              ;518
000146  2807              CMP      r0,#7                 ;518
000148  d016              BEQ      |L7.376|
00014a  2200              MOVS     r2,#0                 ;518
                  |L7.332|
00014c  4311              ORRS     r1,r1,r2              ;518
00014e  2808              CMP      r0,#8                 ;518
000150  d014              BEQ      |L7.380|
000152  2000              MOVS     r0,#0                 ;518
                  |L7.340|
000154  4301              ORRS     r1,r1,r0              ;518
000156  d018              BEQ      |L7.394|
000158  4632              MOV      r2,r6                 ;522
00015a  4621              MOV      r1,r4                 ;522
00015c  4650              MOV      r0,r10                ;522
00015e  f7fffffe          BL       automaticControlDifferentialSpeed
000162  e012              B        |L7.394|
                  |L7.356|
000164  2101              MOVS     r1,#1                 ;518
000166  e7dc              B        |L7.290|
                  |L7.360|
000168  2201              MOVS     r2,#1                 ;518
00016a  e7df              B        |L7.300|
                  |L7.364|
00016c  2201              MOVS     r2,#1                 ;518
00016e  e7e1              B        |L7.308|
                  |L7.368|
000170  2201              MOVS     r2,#1                 ;518
000172  e7e3              B        |L7.316|
                  |L7.372|
000174  2201              MOVS     r2,#1                 ;518
000176  e7e5              B        |L7.324|
                  |L7.376|
000178  2201              MOVS     r2,#1                 ;518
00017a  e7e7              B        |L7.332|
                  |L7.380|
00017c  2001              MOVS     r0,#1                 ;518
00017e  e7e9              B        |L7.340|
000180  4632              MOV      r2,r6                 ;529
000182  4621              MOV      r1,r4                 ;529
000184  4650              MOV      r0,r10                ;529
000186  f7fffffe          BL       automaticControl
                  |L7.394|
00018a  f894003d          LDRB     r0,[r4,#0x3d]         ;550
00018e  2807              CMP      r0,#7                 ;550
000190  d116              BNE      |L7.448|
000192  482a              LDR      r0,|L7.572|
000194  8840              LDRH     r0,[r0,#2]            ;555  ; camerparameter
000196  60b8              STR      r0,[r7,#8]            ;555  ; CurQRcodeID
000198  2804              CMP      r0,#4                 ;556
00019a  d108              BNE      |L7.430|
00019c  68f8              LDR      r0,[r7,#0xc]          ;556  ; LastQRcodeID
00019e  2803              CMP      r0,#3                 ;556
0001a0  d105              BNE      |L7.430|
0001a2  a127              ADR      r1,|L7.576|
0001a4  2001              MOVS     r0,#1                 ;559
0001a6  f7fffffe          BL       Uart_Printf
0001aa  f88480b9          STRB     r8,[r4,#0xb9]         ;560
                  |L7.430|
0001ae  68b8              LDR      r0,[r7,#8]            ;562  ; CurQRcodeID
0001b0  60f8              STR      r0,[r7,#0xc]          ;562  ; LastQRcodeID
0001b2  f89400b9          LDRB     r0,[r4,#0xb9]         ;564
0001b6  b118              CBZ      r0,|L7.448|
0001b8  4631              MOV      r1,r6                 ;566
0001ba  4620              MOV      r0,r4                 ;566
0001bc  f7fffffe          BL       QecodeSiteChange
                  |L7.448|
0001c0  f8990055          LDRB     r0,[r9,#0x55]         ;570  ; HandlePlcData
0001c4  2800              CMP      r0,#0                 ;570
0001c6  d09a              BEQ      |L7.254|
0001c8  f88b5001          STRB     r5,[r11,#1]           ;572
0001cc  f88b5002          STRB     r5,[r11,#2]           ;573
0001d0  6035              STR      r5,[r6,#0]            ;574
0001d2  e794              B        |L7.254|
;;;577    
                          ENDP

                  |L7.468|
                          DCD      ||.data||
                  |L7.472|
0001d8  43340000          DCFS     0x43340000 ; 180
                  |L7.476|
0001dc  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L7.480|
                          DCD      DispatchData
                  |L7.484|
0001e4  b5b1c7b0          DCB      181,177,199,176,213,190,181,227,"=%d\tnext=%d\tdir=%d\tx"
0001e8  d5beb5e3
0001ec  3d256409
0001f0  6e657874
0001f4  3d256409
0001f8  6469723d
0001fc  25640978
000200  3d256420          DCB      "=%d y=%d\tw=%f\t",201,207,207,223,215,180,204,172,"=%d\t"
000204  793d2564
000208  09773d25
00020c  6609c9cf
000210  cfdfd7b4
000214  ccac3d25
000218  6409    
00021a  25640d0a          DCB      "%d\r\n",0
00021e  00      
00021f  00                DCB      0
                  |L7.544|
                          DCD      HandlePlcData
                  |L7.548|
000224  c7e5b3fd          DCB      199,229,179,253,213,190,181,227,182,175,215,247,"\r\n",0
000228  d5beb5e3
00022c  b6afd7f7
000230  0d0a00  
000233  00                DCB      0
                  |L7.564|
000234  00000000          DCFS     0x00000000 ; 0
                  |L7.568|
                          DCD      ||.bss||+0x1c20
                  |L7.572|
                          DCD      camerparameter
                  |L7.576|
000240  c6f4b6af          DCB      198,244,182,175,182,254,206,172,194,235,184,168,214,250,182
000244  b6feceac
000248  c2ebb8a8
00024c  d6fab6  
00024f  a8cebb0d          DCB      168,206,187,"\r\n",0
000253  0a00    
000255  00                DCB      0
000256  00                DCB      0
000257  00                DCB      0

                          AREA ||.bss||, DATA, NOINIT, ALIGN=2

                  InputInformationList
                          %        7200
                  LaserControlSignal
                          %        9

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

                  agvNAV
000000  00000000          DCB      0x00,0x00,0x00,0x00
                  ||ii||
                          DCD      0x00000000
                  CurQRcodeID
                          DCD      0x00000000
                  LastQRcodeID
                          DCD      0x00000000
                  SySAlarmStatus
                          DCD      0x00000000
000014  0000              DCB      0x00,0x00

;*** Start embedded assembler ***

#line 1 "..\\..\\User\\LaserMotionCtr\\agv.c"
	AREA ||.rev16_text||, CODE
	THUMB
	EXPORT |__asm___5_agv_c_738a854f____REV16|
#line 129 "..\\..\\Libraries\\CMSIS\\Include\\core_cmInstr.h"
|__asm___5_agv_c_738a854f____REV16| PROC
#line 130

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

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

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***