kiva.txt 70.1 KB

Edit Raw Blame History

; 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\kiva.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\kiva.d --cpu=Cortex-M4.fp --apcs=interwork -O1 --diag_suppress=9931,870 -I..\..\Libraries\CMSIS\Include -I..\..\Libraries\CMSIS\Device\ST\STM32F4xx\Include -I..\..\Libraries\STM32F4xx_StdPeriph_Driver\inc -I..\..\uCOS-III\uC-CPU -I..\..\uCOS-III\uC-LIB -I..\..\uCOS-III\uCOS-III\Ports -I..\..\uCOS-III\uCOS-III\Source -I..\..\uCOS-III\uC-CPU\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uC-LIB\Ports\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uCOS-III\Ports\ARM-Cortex-M4\Generic\RealView -I..\..\User -I..\..\User\bsp -I..\..\User\bsp\inc -I..\..\User\libapp -I..\..\RL-ARM\Config -I..\..\RL-ARM\Driver -I..\..\RL-ARM\RL-RTX\inc -I..\..\User\bsp\BSP -I..\..\RL-ARM\RL-CAN -I..\..\Libraries\DSP_LIB\Include -I..\..\MODBUS\modbus\rtu -I..\..\MODBUS\BARE\port -I..\..\MODBUS\modbus\include -I..\..\User\bsp\BSP -I..\..\PLC -I..\..\Avoid -I..\..\User\parameter -I..\..\User\LaserMotionCtr -I..\..\User\W5100S -I..\..\User\bsp -I..\..\User\CHASSIS -I..\..\User\CONTROLFUNCTION -I..\..\User\DATAUPDATE -I..\..\User\HARAWARE -I..\..\User\MOTORDRIVER -I..\..\User\NAVAGATION -I..\..\User\PLATFORM -I..\..\User\SENSOR -I.\RTE\_Flash -IC:\Users\YDJ\AppData\Local\Arm\Packs\ARM\CMSIS\5.5.1\CMSIS\Core\Include -IC:\Users\YDJ\AppData\Local\Arm\Packs\Keil\STM32F4xx_DFP\2.13.0\Drivers\CMSIS\Device\ST\STM32F4xx\Include -D__UVISION_VERSION=527 -D_RTE_ -DSTM32F407xx -DUSE_STDPERIPH_DRIVER -DSTM32F40_41xxx -D__RTX -D__FPU_USED=1 --omf_browse=.\flash\obj\kiva.crf ..\..\User\PLATFORM\KIVA.c]
                          THUMB

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

                  CalculateAngleOffset PROC
;;;24     //在静止下计算，在运动中补偿
;;;25     float CalculateAngleOffset(float curAngle)
000000  b570              PUSH     {r4-r6,lr}
;;;26     {
000002  ed2d8b08          VPUSH    {d8-d11}
000006  eeb08a40          VMOV.F32 s16,s0
;;;27     	static float allowOffset = 20;
;;;28     	int dir = 1;//
;;;29     	float angle = 0;
;;;30     	if(fabs(curAngle) < allowOffset)
;;;31     	{
;;;32     		if(curAngle > 0)//左偏
;;;33     		{
;;;34     			dir = 1;
;;;35     			angle = fabs(curAngle);
;;;36     		}
;;;37     		else//右偏
;;;38     		{
;;;39     			dir = -1;
00000a  f04f36ff          MOV      r6,#0xffffffff
;;;40     			angle = fabs(curAngle);
;;;41     		}
;;;42     	}
;;;43     	else if(fabs(curAngle - 90) < allowOffset)
00000e  ed9f0a89          VLDR     s0,|L1.564|
000012  4d89              LDR      r5,|L1.568|
000014  ee78ba40          VSUB.F32 s23,s16,s0
;;;44     	{
;;;45     		if(curAngle > 90)//左偏
;;;46     		{
;;;47     			dir = 1;
;;;48     			angle = fabs(curAngle - 90);
;;;49     		}
;;;50     		else//右偏
;;;51     		{
;;;52     			dir = -1;
;;;53     			angle = fabs(curAngle - 90);
;;;54     		}
;;;55     	}
;;;56     	else if(fabs(fabs(curAngle) - 180) < allowOffset)
000018  ed9f9b88          VLDR     d9,|L1.572|
;;;57     	{
;;;58     		if(curAngle > 0)//右偏
;;;59     		{
;;;60     			dir = -1;
;;;61     			angle = fabs(fabs(curAngle) - 180);
;;;62     		}
;;;63     		else//左偏
;;;64     		{
;;;65     			dir = 1;
;;;66     			angle = fabs(fabs(curAngle) - 180);
;;;67     		}
;;;68     	}
;;;69     	else if(fabs(curAngle + 90) < allowOffset)
00001c  ee38ba00          VADD.F32 s22,s16,s0
000020  2401              MOVS     r4,#1                 ;28
000022  eddf8a88          VLDR     s17,|L1.580|
000026  69e8              LDR      r0,[r5,#0x1c]         ;30  ; allowOffset
000028  f7fffffe          BL       __aeabi_f2d
00002c  ec410b1a          VMOV     d10,r0,r1             ;30
000030  ee180a10          VMOV     r0,s16                ;30
000034  f7fffffe          BL       __aeabi_f2d
000038  ec410b10          VMOV     d0,r0,r1              ;30
00003c  f7fffffe          BL       __hardfp_fabs
000040  ec532b1a          VMOV     r2,r3,d10             ;30
000044  ec510b10          VMOV     r0,r1,d0              ;30
000048  f7fffffe          BL       __aeabi_cdcmple
00004c  d223              BCS      |L1.150|
00004e  eeb58ac0          VCMPE.F32 s16,#0.0              ;32
000052  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;32
000056  dd0e              BLE      |L1.118|
000058  ee180a10          VMOV     r0,s16                ;35
00005c  f7fffffe          BL       __aeabi_f2d
000060  ec410b10          VMOV     d0,r0,r1              ;35
000064  f7fffffe          BL       __hardfp_fabs
000068  ec510b10          VMOV     r0,r1,d0              ;35
00006c  f7fffffe          BL       __aeabi_d2f
000070  ee080a90          VMOV     s17,r0                ;35
000074  e0d4              B        |L1.544|
                  |L1.118|
000076  4634              MOV      r4,r6                 ;39
000078  ee180a10          VMOV     r0,s16                ;40
00007c  f7fffffe          BL       __aeabi_f2d
000080  ec410b10          VMOV     d0,r0,r1              ;40
000084  f7fffffe          BL       __hardfp_fabs
000088  ec510b10          VMOV     r0,r1,d0              ;40
00008c  f7fffffe          BL       __aeabi_d2f
000090  ee080a90          VMOV     s17,r0                ;40
000094  e0c4              B        |L1.544|
                  |L1.150|
000096  69e8              LDR      r0,[r5,#0x1c]         ;43  ; allowOffset
000098  f7fffffe          BL       __aeabi_f2d
00009c  ec410b1a          VMOV     d10,r0,r1             ;43
0000a0  ee1b0a90          VMOV     r0,s23                ;43
0000a4  f7fffffe          BL       __aeabi_f2d
0000a8  ec410b10          VMOV     d0,r0,r1              ;43
0000ac  f7fffffe          BL       __hardfp_fabs
0000b0  ec532b1a          VMOV     r2,r3,d10             ;43
0000b4  ec510b10          VMOV     r0,r1,d0              ;43
0000b8  f7fffffe          BL       __aeabi_cdcmple
0000bc  d223              BCS      |L1.262|
0000be  ee180a10          VMOV     r0,s16                ;45
0000c2  4961              LDR      r1,|L1.584|
0000c4  4288              CMP      r0,r1                 ;45
0000c6  dd0e              BLE      |L1.230|
0000c8  ee1b0a90          VMOV     r0,s23                ;48
0000cc  f7fffffe          BL       __aeabi_f2d
0000d0  ec410b10          VMOV     d0,r0,r1              ;48
0000d4  f7fffffe          BL       __hardfp_fabs
0000d8  ec510b10          VMOV     r0,r1,d0              ;48
0000dc  f7fffffe          BL       __aeabi_d2f
0000e0  ee080a90          VMOV     s17,r0                ;48
0000e4  e09c              B        |L1.544|
                  |L1.230|
0000e6  4634              MOV      r4,r6                 ;52
0000e8  ee1b0a90          VMOV     r0,s23                ;53
0000ec  f7fffffe          BL       __aeabi_f2d
0000f0  ec410b10          VMOV     d0,r0,r1              ;53
0000f4  f7fffffe          BL       __hardfp_fabs
0000f8  ec510b10          VMOV     r0,r1,d0              ;53
0000fc  f7fffffe          BL       __aeabi_d2f
000100  ee080a90          VMOV     s17,r0                ;53
000104  e08c              B        |L1.544|
                  |L1.262|
000106  69e8              LDR      r0,[r5,#0x1c]         ;56  ; allowOffset
000108  f7fffffe          BL       __aeabi_f2d
00010c  ec410b1a          VMOV     d10,r0,r1             ;56
000110  ee180a10          VMOV     r0,s16                ;56
000114  f7fffffe          BL       __aeabi_f2d
000118  ec410b10          VMOV     d0,r0,r1              ;56
00011c  f7fffffe          BL       __hardfp_fabs
000120  ec532b19          VMOV     r2,r3,d9              ;56
000124  ec510b10          VMOV     r0,r1,d0              ;56
000128  f7fffffe          BL       __aeabi_dsub
00012c  ec410b10          VMOV     d0,r0,r1              ;56
000130  f7fffffe          BL       __hardfp_fabs
000134  ec532b1a          VMOV     r2,r3,d10             ;56
000138  ec510b10          VMOV     r0,r1,d0              ;56
00013c  f7fffffe          BL       __aeabi_cdcmple
000140  d237              BCS      |L1.434|
000142  eeb58ac0          VCMPE.F32 s16,#0.0              ;58
000146  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;58
00014a  dd19              BLE      |L1.384|
00014c  4634              MOV      r4,r6                 ;60
00014e  ee180a10          VMOV     r0,s16                ;61
000152  f7fffffe          BL       __aeabi_f2d
000156  ec410b10          VMOV     d0,r0,r1              ;61
00015a  f7fffffe          BL       __hardfp_fabs
00015e  ec532b19          VMOV     r2,r3,d9              ;61
000162  ec510b10          VMOV     r0,r1,d0              ;61
000166  f7fffffe          BL       __aeabi_dsub
00016a  ec410b10          VMOV     d0,r0,r1              ;61
00016e  f7fffffe          BL       __hardfp_fabs
000172  ec510b10          VMOV     r0,r1,d0              ;61
000176  f7fffffe          BL       __aeabi_d2f
00017a  ee080a90          VMOV     s17,r0                ;61
00017e  e04f              B        |L1.544|
                  |L1.384|
000180  ee180a10          VMOV     r0,s16                ;66
000184  f7fffffe          BL       __aeabi_f2d
000188  ec410b10          VMOV     d0,r0,r1              ;66
00018c  f7fffffe          BL       __hardfp_fabs
000190  ec532b19          VMOV     r2,r3,d9              ;66
000194  ec510b10          VMOV     r0,r1,d0              ;66
000198  f7fffffe          BL       __aeabi_dsub
00019c  ec410b10          VMOV     d0,r0,r1              ;66
0001a0  f7fffffe          BL       __hardfp_fabs
0001a4  ec510b10          VMOV     r0,r1,d0              ;66
0001a8  f7fffffe          BL       __aeabi_d2f
0001ac  ee080a90          VMOV     s17,r0                ;66
0001b0  e036              B        |L1.544|
                  |L1.434|
0001b2  69e8              LDR      r0,[r5,#0x1c]  ; allowOffset
0001b4  f7fffffe          BL       __aeabi_f2d
0001b8  ec410b19          VMOV     d9,r0,r1
0001bc  ee1b0a10          VMOV     r0,s22
0001c0  f7fffffe          BL       __aeabi_f2d
0001c4  ec410b10          VMOV     d0,r0,r1
0001c8  f7fffffe          BL       __hardfp_fabs
0001cc  ec532b19          VMOV     r2,r3,d9
0001d0  ec510b10          VMOV     r0,r1,d0
0001d4  f7fffffe          BL       __aeabi_cdcmple
0001d8  d222              BCS      |L1.544|
;;;70     	{
;;;71     		if(curAngle > -90)//左偏
0001da  ee180a10          VMOV     r0,s16
0001de  491b              LDR      r1,|L1.588|
0001e0  4288              CMP      r0,r1
0001e2  d20e              BCS      |L1.514|
;;;72     		{
;;;73     			dir = 1;
;;;74     			angle = fabs(curAngle + 90);
0001e4  ee1b0a10          VMOV     r0,s22
0001e8  f7fffffe          BL       __aeabi_f2d
0001ec  ec410b10          VMOV     d0,r0,r1
0001f0  f7fffffe          BL       __hardfp_fabs
0001f4  ec510b10          VMOV     r0,r1,d0
0001f8  f7fffffe          BL       __aeabi_d2f
0001fc  ee080a90          VMOV     s17,r0
000200  e00e              B        |L1.544|
                  |L1.514|
;;;75     		}
;;;76     		else//右偏
;;;77     		{
;;;78     			dir = -1;
000202  4634              MOV      r4,r6
;;;79     			angle = fabs(curAngle + 90);
000204  ee1b0a10          VMOV     r0,s22
000208  f7fffffe          BL       __aeabi_f2d
00020c  ec410b10          VMOV     d0,r0,r1
000210  f7fffffe          BL       __hardfp_fabs
000214  ec510b10          VMOV     r0,r1,d0
000218  f7fffffe          BL       __aeabi_d2f
00021c  ee080a90          VMOV     s17,r0
                  |L1.544|
;;;80     		}
;;;81     	}
;;;82     	return dir*angle;
000220  ee004a10          VMOV     s0,r4
000224  eeb80ac0          VCVT.F32.S32 s0,s0
000228  ee200a28          VMUL.F32 s0,s0,s17
;;;83     }
00022c  ecbd8b08          VPOP     {d8-d11}
000230  bd70              POP      {r4-r6,pc}
;;;84
                          ENDP

000232  0000              DCW      0x0000
                  |L1.564|
000234  42b40000          DCFS     0x42b40000 ; 90
                  |L1.568|
                          DCD      ||.data||
                  |L1.572|
00023c  00000000          DCFD     0x4066800000000000 ; 180
000240  40668000
                  |L1.580|
000244  00000000          DCFS     0x00000000 ; 0
                  |L1.584|
                          DCD      0x42b40000
                  |L1.588|
                          DCD      0xc2b40000

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

                  RotateRunAuto PROC
;;;151    float angleOffset = 0;
;;;152    void RotateRunAuto() //举升自动运行,运行状态0不动  1左转90  2左转180  3右转90  4右转180  5左转联动90  6左转联动180  7右转联动90  8右转联动180
000000  e92d41f0          PUSH     {r4-r8,lr}
;;;153    {
000004  ed2d8b04          VPUSH    {d8-d9}
;;;154    	static float lastAngle = 0,diffAngle = 0,alreadyTurnAngle = 0,lastPalstance = 0;
;;;155    	static int countNum = 1000;
;;;156    	static u8 TurnFlag = 0;
;;;157    	float PalstanceMax = (float)agv.Parameter.HandTurnSpeed/230 + palstanceOffset;
000008  4cef              LDR      r4,|L2.968|
00000a  f8b40042          LDRH     r0,[r4,#0x42]  ; agv
00000e  ee000a10          VMOV     s0,r0
000012  eef80a40          VCVT.F32.U32 s1,s0
000016  ed9f1aed          VLDR     s2,|L2.972|
00001a  4ded              LDR      r5,|L2.976|
00001c  ee800a81          VDIV.F32 s0,s1,s2
000020  edd50a02          VLDR     s1,[r5,#8]
000024  ee309a20          VADD.F32 s18,s0,s1
;;;158    	if(!Rotate1.runState)
000028  f8df83a8          LDR      r8,|L2.980|
00002c  f8980016          LDRB     r0,[r8,#0x16]  ; Rotate1
000030  2800              CMP      r0,#0
000032  d105              BNE      |L2.64|
;;;159    	{
;;;160    		angleOffset = CalculateAngleOffset(Rotate1.encodeAngle);
000034  ed980a03          VLDR     s0,[r8,#0xc]
000038  f7fffffe          BL       CalculateAngleOffset
00003c  ed850a06          VSTR     s0,[r5,#0x18]
                  |L2.64|
;;;161    	}
;;;162
;;;163    	if(!(Rotate1.runState == 1 || Rotate1.runState == 2))
000040  f8980016          LDRB     r0,[r8,#0x16]  ; Rotate1
000044  2600              MOVS     r6,#0
000046  2801              CMP      r0,#1
000048  d031              BEQ      |L2.174|
00004a  2802              CMP      r0,#2
00004c  d02f              BEQ      |L2.174|
;;;164    	{
;;;165    		if(!TurnFlag)
00004e  78a9              LDRB     r1,[r5,#2]  ; TurnFlag
000050  b109              CBZ      r1,|L2.86|
;;;166    		{
;;;167    			if(agv.Command.HandMotorState == 5)
;;;168    			{
;;;169    				Rotate1.runState = 6;
;;;170    				TurnFlag = 1;
;;;171    				countNum = 0;
;;;172    			}
;;;173    			else if(agv.Command.HandMotorState == 6)
;;;174    			{
;;;175    				Rotate1.runState = 5;
;;;176    				TurnFlag = 1;
;;;177    				countNum = 0;
;;;178    			}
;;;179    			else if(agv.Command.CurDirection == 3)
;;;180    			{
;;;181    				Rotate1.runState = 4;
;;;182    				TurnFlag = 1;
;;;183    				countNum = 1000;
;;;184    			}
;;;185    			else if(agv.Command.CurDirection == 4)
;;;186    			{
;;;187    				Rotate1.runState = 3;
;;;188    				TurnFlag = 1;
;;;189    				countNum = 1000;
;;;190    			}
;;;191    		}
;;;192    		else if(Rotate1.runState == 0 && agv.Command.CurDirection != 4 && agv.Command.CurDirection != 3)
000052  b330              CBZ      r0,|L2.162|
000054  e02b              B        |L2.174|
                  |L2.86|
000056  7960              LDRB     r0,[r4,#5]            ;167  ; agv
000058  2101              MOVS     r1,#1                 ;158
00005a  2805              CMP      r0,#5                 ;167
00005c  d009              BEQ      |L2.114|
00005e  2806              CMP      r0,#6                 ;173
000060  d00d              BEQ      |L2.126|
000062  7f20              LDRB     r0,[r4,#0x1c]         ;179  ; agv
000064  f44f727a          MOV      r2,#0x3e8             ;183
000068  2803              CMP      r0,#3                 ;179
00006a  d00e              BEQ      |L2.138|
00006c  2804              CMP      r0,#4                 ;185
00006e  d012              BEQ      |L2.150|
000070  e01d              B        |L2.174|
                  |L2.114|
000072  2006              MOVS     r0,#6                 ;169
000074  f8880016          STRB     r0,[r8,#0x16]         ;169
000078  70a9              STRB     r1,[r5,#2]            ;170
00007a  632e              STR      r6,[r5,#0x30]         ;171  ; countNum
00007c  e017              B        |L2.174|
                  |L2.126|
00007e  2005              MOVS     r0,#5                 ;175
000080  f8880016          STRB     r0,[r8,#0x16]         ;175
000084  70a9              STRB     r1,[r5,#2]            ;176
000086  632e              STR      r6,[r5,#0x30]         ;177  ; countNum
000088  e011              B        |L2.174|
                  |L2.138|
00008a  2004              MOVS     r0,#4                 ;181
00008c  f8880016          STRB     r0,[r8,#0x16]         ;181
000090  70a9              STRB     r1,[r5,#2]            ;182
000092  632a              STR      r2,[r5,#0x30]         ;183  ; countNum
000094  e00b              B        |L2.174|
                  |L2.150|
000096  2003              MOVS     r0,#3                 ;187
000098  f8880016          STRB     r0,[r8,#0x16]         ;187
00009c  70a9              STRB     r1,[r5,#2]            ;188
00009e  632a              STR      r2,[r5,#0x30]         ;189  ; countNum
0000a0  e005              B        |L2.174|
                  |L2.162|
0000a2  7f20              LDRB     r0,[r4,#0x1c]  ; agv
0000a4  2804              CMP      r0,#4
0000a6  d002              BEQ      |L2.174|
0000a8  2803              CMP      r0,#3
0000aa  d000              BEQ      |L2.174|
;;;193    		{
;;;194    			TurnFlag = 0;
0000ac  70ae              STRB     r6,[r5,#2]
                  |L2.174|
;;;195    		}
;;;196    	}
;;;197
;;;198    	  alreadyTurnAngle = Rotate1.encodeAngle - lastAngle;
0000ae  ed980a03          VLDR     s0,[r8,#0xc]
0000b2  edd50a08          VLDR     s1,[r5,#0x20]
0000b6  ee300a60          VSUB.F32 s0,s0,s1
0000ba  ed850a0a          VSTR     s0,[r5,#0x28]
;;;199    		if(Rotate1.setSpeed < 0)//左转补偿
0000be  edd80a01          VLDR     s1,[r8,#4]
0000c2  eef50ac0          VCMPE.F32 s1,#0.0
0000c6  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000ca  d212              BCS      |L2.242|
;;;200    		{
;;;201    			if(alreadyTurnAngle > 0)
0000cc  eeb50ac0          VCMPE.F32 s0,#0.0
0000d0  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000d4  dd06              BLE      |L2.228|
;;;202    			{
;;;203    				alreadyTurnAngle += angleOffset;
0000d6  edd50a06          VLDR     s1,[r5,#0x18]
0000da  ee300a20          VADD.F32 s0,s0,s1
0000de  ed850a0a          VSTR     s0,[r5,#0x28]
0000e2  e01d              B        |L2.288|
                  |L2.228|
;;;204    			}
;;;205    			else
;;;206    				alreadyTurnAngle -= angleOffset;
0000e4  edd50a06          VLDR     s1,[r5,#0x18]
0000e8  ee300a60          VSUB.F32 s0,s0,s1
0000ec  ed850a0a          VSTR     s0,[r5,#0x28]
0000f0  e016              B        |L2.288|
                  |L2.242|
;;;207    		}
;;;208    		else if(Rotate1.setSpeed > 0)
0000f2  eef50ac0          VCMPE.F32 s1,#0.0
0000f6  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000fa  dd11              BLE      |L2.288|
;;;209    		{
;;;210    			if(alreadyTurnAngle > 0)
0000fc  eeb50ac0          VCMPE.F32 s0,#0.0
000100  eef1fa10          VMRS     APSR_nzcv,FPSCR
000104  dd06              BLE      |L2.276|
;;;211    			{
;;;212    				alreadyTurnAngle -= angleOffset;
000106  edd50a06          VLDR     s1,[r5,#0x18]
00010a  ee300a60          VSUB.F32 s0,s0,s1
00010e  ed850a0a          VSTR     s0,[r5,#0x28]
000112  e005              B        |L2.288|
                  |L2.276|
;;;213    			}
;;;214    			else
;;;215    				alreadyTurnAngle += angleOffset;
000114  edd50a06          VLDR     s1,[r5,#0x18]
000118  ee300a20          VADD.F32 s0,s0,s1
00011c  ed850a0a          VSTR     s0,[r5,#0x28]
                  |L2.288|
;;;216    		}
;;;217    		if(alreadyTurnAngle > 180)
000120  ed950a0a          VLDR     s0,[r5,#0x28]
000124  49ac              LDR      r1,|L2.984|
000126  ee100a10          VMOV     r0,s0
;;;218    			alreadyTurnAngle -= 360;
00012a  eddf0aac          VLDR     s1,|L2.988|
00012e  4288              CMP      r0,r1                 ;217
000130  dd03              BLE      |L2.314|
000132  ee300a60          VSUB.F32 s0,s0,s1
000136  ed850a0a          VSTR     s0,[r5,#0x28]
                  |L2.314|
;;;219    		if(alreadyTurnAngle < -180)
00013a  ed950a0a          VLDR     s0,[r5,#0x28]
00013e  49a8              LDR      r1,|L2.992|
000140  ee100a10          VMOV     r0,s0
000144  4288              CMP      r0,r1
000146  d903              BLS      |L2.336|
;;;220    			alreadyTurnAngle += 360;
000148  ee300a20          VADD.F32 s0,s0,s1
00014c  ed850a0a          VSTR     s0,[r5,#0x28]
                  |L2.336|
;;;221    		diffAngle = fabs(fabs(alreadyTurnAngle) - agv.Command.SetPlatformTurnAngle);//fabs(Rotate1.encodeAngle - lastAngle);
000150  6a20              LDR      r0,[r4,#0x20]  ; agv
000152  f7fffffe          BL       __aeabi_f2d
000156  ec410b18          VMOV     d8,r0,r1
00015a  6aa8              LDR      r0,[r5,#0x28]  ; alreadyTurnAngle
00015c  f7fffffe          BL       __aeabi_f2d
000160  ec410b10          VMOV     d0,r0,r1
000164  f7fffffe          BL       __hardfp_fabs
000168  ec532b18          VMOV     r2,r3,d8
00016c  ec510b10          VMOV     r0,r1,d0
000170  f7fffffe          BL       __aeabi_dsub
000174  ec410b10          VMOV     d0,r0,r1
000178  f7fffffe          BL       __hardfp_fabs
00017c  ec510b10          VMOV     r0,r1,d0
000180  f7fffffe          BL       __aeabi_d2f
000184  ee000a10          VMOV     s0,r0
000188  ed850a09          VSTR     s0,[r5,#0x24]
;;;222
;;;223    		if(Rotate1.runState == 3 || Rotate1.runState == 4 || Rotate1.runState == 5 || Rotate1.runState == 6)
00018c  f8980016          LDRB     r0,[r8,#0x16]  ; Rotate1
000190  2803              CMP      r0,#3
000192  d005              BEQ      |L2.416|
000194  2804              CMP      r0,#4
000196  d003              BEQ      |L2.416|
000198  2805              CMP      r0,#5
00019a  d001              BEQ      |L2.416|
00019c  2806              CMP      r0,#6
00019e  d11d              BNE      |L2.476|
                  |L2.416|
;;;224    		{
;;;225    			if(diffAngle > 30)//平台旋转角度
0001a0  ee100a10          VMOV     r0,s0
0001a4  498f              LDR      r1,|L2.996|
0001a6  4288              CMP      r0,r1
0001a8  dd02              BLE      |L2.432|
;;;226    			{
;;;227    				setPalstance = PalstanceMax;
0001aa  ed859a05          VSTR     s18,[r5,#0x14]
0001ae  e015              B        |L2.476|
                  |L2.432|
;;;228    			}
;;;229    			else if(diffAngle > 3)
0001b0  ee100a10          VMOV     r0,s0
0001b4  498c              LDR      r1,|L2.1000|
0001b6  4288              CMP      r0,r1
0001b8  dd0c              BLE      |L2.468|
;;;230    			{
;;;231    				setPalstance = mapping(diffAngle,3,30,palstanceMin,PalstanceMax);
0001ba  eeb02a49          VMOV.F32 s4,s18
0001be  edd51a03          VLDR     s3,[r5,#0xc]
0001c2  eeb31a0e          VMOV.F32 s2,#30.00000000
0001c6  eef00a08          VMOV.F32 s1,#3.00000000
0001ca  f7fffffe          BL       mapping
0001ce  ed850a05          VSTR     s0,[r5,#0x14]
0001d2  e003              B        |L2.476|
                  |L2.468|
;;;232    			}
;;;233    			else
;;;234    			{
;;;235    				setPalstance = palstanceMin;
0001d4  ed950a03          VLDR     s0,[r5,#0xc]
0001d8  ed850a05          VSTR     s0,[r5,#0x14]
                  |L2.476|
;;;236    			}
;;;237    		}
;;;238    		if(turnCount >= countNum)
;;;239    		{
;;;240    			if(setPalstance > lastPalstance)
;;;241    			{
;;;242    				setPalstance = lastPalstance + 0.005;
;;;243    			}
;;;244    			else if(setPalstance < lastPalstance)
;;;245    			{
;;;246    				setPalstance = lastPalstance - 0.01;
;;;247    			}
;;;248    			if(setPalstance < 0)
;;;249    			{
;;;250    				setPalstance = 0;
0001dc  ed9f8a83          VLDR     s16,|L2.1004|
0001e0  6b2f              LDR      r7,[r5,#0x30]         ;238  ; countNum
0001e2  686c              LDR      r4,[r5,#4]            ;238  ; turnCount
0001e4  42bc              CMP      r4,r7                 ;238
0001e6  db32              BLT      |L2.590|
0001e8  edd50a05          VLDR     s1,[r5,#0x14]         ;240
0001ec  ed950a0b          VLDR     s0,[r5,#0x2c]         ;240
0001f0  eef40ac0          VCMPE.F32 s1,s0                 ;240
0001f4  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;240
0001f8  dd0d              BLE      |L2.534|
0001fa  ee100a10          VMOV     r0,s0                 ;242
0001fe  f7fffffe          BL       __aeabi_f2d
000202  ed9f1b7b          VLDR     d1,|L2.1008|
000206  ec532b11          VMOV     r2,r3,d1              ;242
00020a  f7fffffe          BL       __aeabi_dadd
00020e  f7fffffe          BL       __aeabi_d2f
000212  6168              STR      r0,[r5,#0x14]         ;242  ; setPalstance
000214  e00d              B        |L2.562|
                  |L2.534|
000216  d20c              BCS      |L2.562|
000218  ee100a10          VMOV     r0,s0                 ;246
00021c  f7fffffe          BL       __aeabi_f2d
000220  ed9f1b75          VLDR     d1,|L2.1016|
000224  ec532b11          VMOV     r2,r3,d1              ;246
000228  f7fffffe          BL       __aeabi_dsub
00022c  f7fffffe          BL       __aeabi_d2f
000230  6168              STR      r0,[r5,#0x14]         ;246  ; setPalstance
                  |L2.562|
000232  ed950a05          VLDR     s0,[r5,#0x14]         ;248
000236  eeb50ac0          VCMPE.F32 s0,#0.0               ;248
00023a  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;248
00023e  d201              BCS      |L2.580|
000240  ed858a05          VSTR     s16,[r5,#0x14]
                  |L2.580|
;;;251    			}
;;;252    			lastPalstance = setPalstance;
000244  ed950a05          VLDR     s0,[r5,#0x14]
000248  ed850a0b          VSTR     s0,[r5,#0x2c]
00024c  e003              B        |L2.598|
                  |L2.590|
;;;253    		}
;;;254    		else
;;;255    		{
;;;256    			lastPalstance = 0;
00024e  ed858a0b          VSTR     s16,[r5,#0x2c]
;;;257    			setPalstance = 0;
000252  ed858a05          VSTR     s16,[r5,#0x14]
                  |L2.598|
;;;258    		}
;;;259    		switch (Rotate1.runState)
000256  f8980016          LDRB     r0,[r8,#0x16]  ; Rotate1
00025a  eeb30a0e          VMOV.F32 s0,#30.00000000       ;231
;;;260    		{
;;;261    			case 0:
;;;262    				Rotate1.setSpeed = 0;
;;;263    				lastAngle = Rotate1.encodeAngle;
;;;264    				break;
;;;265    //			case 1://左转90度
;;;266    //				Rotate1.setSpeed = -100000;
;;;267    //				if(fabs(diffAngle - 90) <=1)
;;;268    //				{
;;;269    //					Rotate1.setSpeed = 0;
;;;270    //					Rotate1.runState = 0;
;;;271    //				}
;;;272    //				break;
;;;273    			case 2://左转180度
;;;274    				Rotate1.setSpeed = -50000;
;;;275    				if(fabs(diffAngle) <=setArriveAngle)
;;;276    				{
;;;277    					Rotate1.setSpeed = 0;
;;;278    					Rotate1.runState = 0;
;;;279    				}
;;;280    				break;
;;;281    			case 3://左转联动
;;;282    				if(turnCount ++ >= countNum)
;;;283    				{
;;;284    					Rotate1.setSpeed = -setPalstance*2418000/2/PI*29/30;//IMU.palstance
00025e  eddf0a68          VLDR     s1,|L2.1024|
000262  eefe2a00          VMOV.F32 s5,#-0.50000000
000266  ed9f1a67          VLDR     s2,|L2.1028|
00026a  eef31a0d          VMOV.F32 s3,#29.00000000
;;;285    					if(diffAngle < setArriveAngle)
;;;286    					{
;;;287    						turnCount = 0;
;;;288    						Rotate1.setSpeed = 0;
;;;289    						Rotate1.runState = 0;
;;;290    					}
;;;291    				}
;;;292    				break;
;;;293    			case 4://右转联动
;;;294    				if(turnCount ++ >= countNum)
;;;295    				{
;;;296    					Rotate1.setSpeed = setPalstance*2418000/2/PI*29/30;
00026e  eeb62a00          VMOV.F32 s4,#0.50000000
000272  2807              CMP      r0,#7                 ;259
000274  d20b              BCS      |L2.654|
000276  e8dff000          TBB      [pc,r0]               ;259
00027a  040a              DCB      0x04,0x0a
00027c  0e2a4b6c          DCB      0x0e,0x2a,0x4b,0x6c
000280  8900              DCB      0x89,0x00
000282  ed888a01          VSTR     s16,[r8,#4]           ;262
000286  ed980a03          VLDR     s0,[r8,#0xc]          ;263
00028a  ed850a08          VSTR     s0,[r5,#0x20]         ;263
                  |L2.654|
;;;297    					if(diffAngle < setArriveAngle)
;;;298    					{
;;;299    						turnCount = 0;
;;;300    						Rotate1.setSpeed = 0;
;;;301    						Rotate1.runState = 0;
;;;302    					}
;;;303    				}
;;;304    				break;
;;;305    			case 5://左转联动
;;;306    				Rotate1.setSpeed = -setPalstance*2418000/2/PI*29/30;//IMU.palstance
;;;307    				if(diffAngle < setArriveAngle)
;;;308    				{
;;;309    					turnCount = 0;
;;;310    					Rotate1.setSpeed = 0;
;;;311    					Rotate1.runState = 0;
;;;312    				}
;;;313    				break;
;;;314    			case 6://右转联动
;;;315    				Rotate1.setSpeed = setPalstance*2418000/2/PI*29/30;
;;;316    				if(diffAngle < setArriveAngle)
;;;317    				{
;;;318    					turnCount = 0;
;;;319    					Rotate1.setSpeed = 0;
;;;320    					Rotate1.runState = 0;
;;;321    				}
;;;322    				break;
;;;323    			case 7:
;;;324    				break;
;;;325    			case 8:
;;;326    				break;
;;;327    			default:
;;;328    				break;
;;;329
;;;330    		}
;;;331    }
00028e  ecbd8b04          VPOP     {d8-d9}
000292  e8bd81f0          POP      {r4-r8,pc}
000296  ed9f0a5c          VLDR     s0,|L2.1032|
00029a  ed880a01          VSTR     s0,[r8,#4]            ;274
00029e  6928              LDR      r0,[r5,#0x10]         ;275  ; setArriveAngle
0002a0  f7fffffe          BL       __aeabi_f2d
0002a4  ec410b19          VMOV     d9,r0,r1              ;275
0002a8  6a68              LDR      r0,[r5,#0x24]         ;275  ; diffAngle
0002aa  f7fffffe          BL       __aeabi_f2d
0002ae  ec410b10          VMOV     d0,r0,r1              ;275
0002b2  f7fffffe          BL       __hardfp_fabs
0002b6  ec532b19          VMOV     r2,r3,d9              ;275
0002ba  ec510b10          VMOV     r0,r1,d0              ;275
0002be  f7fffffe          BL       __aeabi_cdcmple
0002c2  d8e4              BHI      |L2.654|
0002c4  ed888a01          VSTR     s16,[r8,#4]           ;277
0002c8  f8886016          STRB     r6,[r8,#0x16]         ;278
                  |L2.716|
0002cc  e7df              B        |L2.654|
0002ce  1c60              ADDS     r0,r4,#1              ;282
0002d0  6068              STR      r0,[r5,#4]            ;282  ; turnCount
0002d2  42bc              CMP      r4,r7                 ;282
0002d4  dbdb              BLT      |L2.654|
0002d6  ed952a05          VLDR     s4,[r5,#0x14]         ;284
0002da  ee620a20          VMUL.F32 s1,s4,s1              ;284
0002de  ee600aa2          VMUL.F32 s1,s1,s5              ;284
0002e2  ee802a81          VDIV.F32 s4,s1,s2              ;284
0002e6  ee620a21          VMUL.F32 s1,s4,s3              ;284
0002ea  ee801a80          VDIV.F32 s2,s1,s0              ;284
0002ee  ed881a01          VSTR     s2,[r8,#4]            ;284
0002f2  ed950a09          VLDR     s0,[r5,#0x24]         ;285
0002f6  edd50a04          VLDR     s1,[r5,#0x10]         ;285
0002fa  eeb40ae0          VCMPE.F32 s0,s1                 ;285
0002fe  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;285
000302  d2c4              BCS      |L2.654|
000304  606e              STR      r6,[r5,#4]            ;287  ; turnCount
000306  ed888a01          VSTR     s16,[r8,#4]           ;288
00030a  f8886016          STRB     r6,[r8,#0x16]         ;289
00030e  e7be              B        |L2.654|
000310  1c60              ADDS     r0,r4,#1              ;294
000312  6068              STR      r0,[r5,#4]            ;294  ; turnCount
000314  42bc              CMP      r4,r7                 ;294
000316  dbba              BLT      |L2.654|
000318  edd52a05          VLDR     s5,[r5,#0x14]         ;296
00031c  ee620aa0          VMUL.F32 s1,s5,s1              ;296
000320  ee600a82          VMUL.F32 s1,s1,s4              ;296
000324  ee802a81          VDIV.F32 s4,s1,s2              ;296
000328  ee620a21          VMUL.F32 s1,s4,s3              ;296
00032c  ee801a80          VDIV.F32 s2,s1,s0              ;296
000330  ed881a01          VSTR     s2,[r8,#4]            ;296
000334  ed950a09          VLDR     s0,[r5,#0x24]         ;297
000338  edd50a04          VLDR     s1,[r5,#0x10]         ;297
00033c  eeb40ae0          VCMPE.F32 s0,s1                 ;297
000340  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;297
000344  d2a3              BCS      |L2.654|
000346  606e              STR      r6,[r5,#4]            ;299  ; turnCount
000348  ed888a01          VSTR     s16,[r8,#4]           ;300
00034c  f8886016          STRB     r6,[r8,#0x16]         ;301
000350  e79d              B        |L2.654|
000352  ed952a05          VLDR     s4,[r5,#0x14]         ;306
000356  ee620a20          VMUL.F32 s1,s4,s1              ;306
00035a  ee600aa2          VMUL.F32 s1,s1,s5              ;306
00035e  ee802a81          VDIV.F32 s4,s1,s2              ;306
000362  ee620a21          VMUL.F32 s1,s4,s3              ;306
000366  ee801a80          VDIV.F32 s2,s1,s0              ;306
00036a  ed881a01          VSTR     s2,[r8,#4]            ;306
00036e  ed950a09          VLDR     s0,[r5,#0x24]         ;307
000372  edd50a04          VLDR     s1,[r5,#0x10]         ;307
000376  eeb40ae0          VCMPE.F32 s0,s1                 ;307
00037a  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;307
00037e  d286              BCS      |L2.654|
000380  606e              STR      r6,[r5,#4]            ;309  ; turnCount
000382  ed888a01          VSTR     s16,[r8,#4]           ;310
000386  f8886016          STRB     r6,[r8,#0x16]         ;311
00038a  e780              B        |L2.654|
00038c  edd52a05          VLDR     s5,[r5,#0x14]         ;315
000390  ee620aa0          VMUL.F32 s1,s5,s1              ;315
000394  ee600a82          VMUL.F32 s1,s1,s4              ;315
000398  ee802a81          VDIV.F32 s4,s1,s2              ;315
00039c  ee620a21          VMUL.F32 s1,s4,s3              ;315
0003a0  ee801a80          VDIV.F32 s2,s1,s0              ;315
0003a4  ed881a01          VSTR     s2,[r8,#4]            ;315
0003a8  ed950a09          VLDR     s0,[r5,#0x24]         ;316
0003ac  edd50a04          VLDR     s1,[r5,#0x10]         ;316
0003b0  eeb40ae0          VCMPE.F32 s0,s1                 ;316
0003b4  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;316
0003b8  d288              BCS      |L2.716|
0003ba  606e              STR      r6,[r5,#4]            ;318  ; turnCount
0003bc  ed888a01          VSTR     s16,[r8,#4]           ;319
0003c0  f8886016          STRB     r6,[r8,#0x16]         ;320
0003c4  e763              B        |L2.654|
;;;332
                          ENDP

0003c6  0000              DCW      0x0000
                  |L2.968|
                          DCD      agv
                  |L2.972|
0003cc  43660000          DCFS     0x43660000 ; 230
                  |L2.976|
                          DCD      ||.data||
                  |L2.980|
                          DCD      Rotate1
                  |L2.984|
                          DCD      0x43340000
                  |L2.988|
0003dc  43b40000          DCFS     0x43b40000 ; 360
                  |L2.992|
                          DCD      0xc3340000
                  |L2.996|
                          DCD      0x41f00000
                  |L2.1000|
                          DCD      0x40400000
                  |L2.1004|
0003ec  00000000          DCFS     0x00000000 ; 0
                  |L2.1008|
0003f0  47ae147b          DCFD     0x3f747ae147ae147b ; 0.0050000000000000001
0003f4  3f747ae1
                  |L2.1016|
0003f8  47ae147b          DCFD     0x3f847ae147ae147b ; 0.01
0003fc  3f847ae1
                  |L2.1024|
000400  4a139540          DCFS     0x4a139540 ; 2418000
                  |L2.1028|
000404  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L2.1032|
000408  c7435000          DCFS     0xc7435000 ; -50000

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

                  X_Input PROC
;;;363
;;;364    void X_Input()
000000  b570              PUSH     {r4-r6,lr}
;;;365    {
;;;366    	static int lastTime = 0;
;;;367    	agv.Public.i_RadarSigFront_1 			= MasterInput.IN1 = !X1;//前壁障减速1
000002  2000              MOVS     r0,#0
000004  f7fffffe          BL       GPIO_ReadInput_24V
000008  4d26              LDR      r5,|L3.164|
00000a  4c27              LDR      r4,|L3.168|
00000c  f0800001          EOR      r0,r0,#1
000010  7028              STRB     r0,[r5,#0]
000012  7120              STRB     r0,[r4,#4]
;;;368    	agv.Public.i_RadarSigFront_2 			= MasterInput.IN2 = !X2;//前壁障减速2
000014  2001              MOVS     r0,#1
000016  f7fffffe          BL       GPIO_ReadInput_24V
00001a  f0800001          EOR      r0,r0,#1
00001e  7068              STRB     r0,[r5,#1]
000020  7160              STRB     r0,[r4,#5]
;;;369    	agv.Public.i_RadarSigFront_3 			= MasterInput.IN3 = !X3;//前壁障减速3
000022  2002              MOVS     r0,#2
000024  f7fffffe          BL       GPIO_ReadInput_24V
000028  f0800001          EOR      r0,r0,#1
00002c  70a8              STRB     r0,[r5,#2]
00002e  71a0              STRB     r0,[r4,#6]
;;;370
;;;371    //	agv.Public.i_RadarSigBack_1 			= MasterInput.IN4 = !X4;//后壁障减速1
;;;372    //	agv.Public.i_RadarSigBack_2 			= MasterInput.IN5 = !X5;//后壁障减速2
;;;373    //	agv.Public.i_RadarSigBack_3 			= MasterInput.IN6 = !X6;//后壁障减速3
;;;374
;;;375    	agv.Public.i_QuickStopSig 				= MasterInput.IN7 = !X7;// 急停 常开
000030  2006              MOVS     r0,#6
000032  f7fffffe          BL       GPIO_ReadInput_24V
000036  f0800001          EOR      r0,r0,#1
00003a  71a8              STRB     r0,[r5,#6]
00003c  7020              STRB     r0,[r4,#0]
;;;376    	agv.Public.i_BumperSig 						= MasterInput.IN8 = X8;//防撞条
00003e  2007              MOVS     r0,#7
000040  f7fffffe          BL       GPIO_ReadInput_24V
000044  71e8              STRB     r0,[r5,#7]
000046  70e0              STRB     r0,[r4,#3]
;;;377    	agv.Public.i_StartSig 						= MasterInput.IN9 = X9;// 启动
000048  2008              MOVS     r0,#8
00004a  f7fffffe          BL       GPIO_ReadInput_24V
00004e  7228              STRB     r0,[r5,#8]
000050  7060              STRB     r0,[r4,#1]
;;;378    	agv.Public.i_StopSig 							= MasterInput.IN10 = X10;//暂停
000052  2009              MOVS     r0,#9
000054  f7fffffe          BL       GPIO_ReadInput_24V
000058  7268              STRB     r0,[r5,#9]
00005a  70a0              STRB     r0,[r4,#2]
;;;379    //	agv.Public.i_LightStopFlagL 			= MasterInput.IN11 = X11;//左停靠
;;;380    //	agv.Public.i_LightStopFlagR 			= MasterInput.IN12 = X12;//右停靠
;;;381    //
;;;382    //	agv.Public.i_Lift1UpLimit					= MasterInput.IN13 = X13;//升降平台上限位
;;;383    	agv.Public.i_Lift1DownLimit				= MasterInput.IN14 = X11;//升降平台下限位
00005c  200a              MOVS     r0,#0xa
00005e  f7fffffe          BL       GPIO_ReadInput_24V
000062  7368              STRB     r0,[r5,#0xd]
000064  72e0              STRB     r0,[r4,#0xb]
;;;384    //	agv.Public.i_Shift1_LimitLeft			= MasterInput.IN15 = X15;//左极限
;;;385    //	agv.Public.i_Shift1_LimitRight 		= MasterInput.IN18 = X18;//右极限
;;;386    //	agv.Public.i_Shift1ArrivalLeft 		= MasterInput.IN16 = X16;//左到位对射
;;;387    //	agv.Public.i_Shift1ArrivalRight 	= MasterInput.IN17 = !X17;//右到位对射
;;;388    	if(agv.Public.i_StartSig)//第一次上电按启动复位举升
000066  7862              LDRB     r2,[r4,#1]  ; agv
000068  3c60              SUBS     r4,r4,#0x60
00006a  2101              MOVS     r1,#1                 ;368
00006c  b10a              CBZ      r2,|L3.114|
;;;389    	{
;;;390    		agv.Public.i_StartFirst = true;
00006e  f8841084          STRB     r1,[r4,#0x84]
                  |L3.114|
;;;391    	}
;;;392    	if(agv.Public.i_Lift1DownLimit)//下限位触发  位置清零
000072  b120              CBZ      r0,|L3.126|
;;;393    	{
;;;394    		Lifter1.encodePos = 0;
000074  480e              LDR      r0,|L3.176|
000076  ed9f0a0d          VLDR     s0,|L3.172|
00007a  ed800a03          VSTR     s0,[r0,#0xc]
                  |L3.126|
;;;395    	}
;;;396    	if(agv.Public.i_StopSig)
00007e  f8942062          LDRB     r2,[r4,#0x62]  ; agv
;;;397    	{
;;;398    		if(agv.Public.SystemTime - lastTime >= 3000)
000082  480c              LDR      r0,|L3.180|
000084  b14a              CBZ      r2,|L3.154|
000086  f8d420bc          LDR      r2,[r4,#0xbc]  ; agv
00008a  6b43              LDR      r3,[r0,#0x34]  ; lastTime
00008c  1ad2              SUBS     r2,r2,r3
00008e  f64033b8          MOV      r3,#0xbb8
000092  429a              CMP      r2,r3
000094  d300              BCC      |L3.152|
;;;399    		{
;;;400    			resetLiftFlag = 1;
000096  7001              STRB     r1,[r0,#0]
                  |L3.152|
;;;401    		}
;;;402    	}
;;;403    	else
;;;404    		lastTime = agv.Public.SystemTime;
;;;405    }
000098  bd70              POP      {r4-r6,pc}
                  |L3.154|
00009a  f8d410bc          LDR      r1,[r4,#0xbc]         ;404  ; agv
00009e  6341              STR      r1,[r0,#0x34]         ;404  ; lastTime
0000a0  bd70              POP      {r4-r6,pc}
;;;406
                          ENDP

0000a2  0000              DCW      0x0000
                  |L3.164|
                          DCD      MasterInput
                  |L3.168|
                          DCD      agv+0x60
                  |L3.172|
0000ac  00000000          DCFS     0x00000000 ; 0
                  |L3.176|
                          DCD      Lifter1
                  |L3.180|
                          DCD      ||.data||

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

                  checkMaterialState PROC
;;;334
;;;335    void checkMaterialState() //物料检测
000000  4a0e              LDR      r2,|L4.60|
;;;336    {
;;;337        //下层物料检测
;;;338        if (agv.Public.i_Shift1ArrivalLeft && agv.Public.i_Shift1ArrivalRight)
000002  4610              MOV      r0,r2
000004  f8921071          LDRB     r1,[r2,#0x71]  ; agv
000008  f8900072          LDRB     r0,[r0,#0x72]
00000c  2900              CMP      r1,#0
00000e  d004              BEQ      |L4.26|
000010  b118              CBZ      r0,|L4.26|
;;;339        {
;;;340            agv.Public.i_MaterialFlagFirst = 1;
000012  2001              MOVS     r0,#1
000014  f8820089          STRB     r0,[r2,#0x89]
;;;341        }
;;;342        else if (!agv.Public.i_Shift1ArrivalLeft && !agv.Public.i_Shift1ArrivalRight && !agv.Public.i_Shift1_LimitLeft && !agv.Public.i_Shift1_LimitRight)
;;;343        {
;;;344            agv.Public.i_MaterialFlagFirst = 0;
;;;345        }
;;;346        else
;;;347            agv.Public.i_MaterialFlagFirst = 2;
;;;348    }
000018  4770              BX       lr
                  |L4.26|
00001a  b931              CBNZ     r1,|L4.42|
00001c  b928              CBNZ     r0,|L4.42|
00001e  f892006e          LDRB     r0,[r2,#0x6e]         ;342  ; agv
000022  b910              CBNZ     r0,|L4.42|
000024  f892006f          LDRB     r0,[r2,#0x6f]         ;342  ; agv
000028  b118              CBZ      r0,|L4.50|
                  |L4.42|
00002a  2002              MOVS     r0,#2                 ;347
00002c  f8820089          STRB     r0,[r2,#0x89]         ;347
000030  4770              BX       lr
                  |L4.50|
000032  2000              MOVS     r0,#0                 ;344
000034  f8820089          STRB     r0,[r2,#0x89]         ;344
000038  4770              BX       lr
;;;349
                          ENDP

00003a  0000              DCW      0x0000
                  |L4.60|
                          DCD      agv

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

                  commandActionAnalysis PROC
;;;521    /*SETVMOT动作指令解析*/
;;;522    void commandActionAnalysis(int dataOne, int dataTwo)
000000  b570              PUSH     {r4-r6,lr}
;;;523    {
000002  4606              MOV      r6,r0
000004  ed2d8b02          VPUSH    {d8}
000008  460c              MOV      r4,r1
;;;524        switch (dataOne)
;;;525        {
;;;526        case 1: //举升1 下降2
;;;527            Lifter1.runState = dataTwo;
;;;528            break;
;;;529
;;;530        case 2: //左旋转
;;;531    				Rotate1.runState = dataOne;
00000a  482f              LDR      r0,|L5.200|
;;;532            agv.Command.SetPlatformTurnAngle = dataTwo;
00000c  4d2f              LDR      r5,|L5.204|
;;;533            break;
;;;534
;;;535        case 3: //右旋转
;;;536    				Rotate1.runState = dataOne;
;;;537            agv.Command.SetPlatformTurnAngle = dataTwo;
;;;538            break;
;;;539
;;;540        case 4:              //打开充电状态
;;;541            switch (dataTwo) //小车运动方式设置       //
;;;542            {
;;;543            case 0:
;;;544                Y10(1); //关闭充电器触点
;;;545                break;
;;;546
;;;547            case 1:    //自动充电站停靠传感器选型   充电预停车指令
;;;548                Y10(0); //打开充电器触点
;;;549                break;
;;;550            }
;;;551
;;;552        case 5: //AGV原地旋转左转
;;;553    				agv.Command.HandMotorState = dataOne;
;;;554            agv.Command.SetChassisTurnAngle = (float)dataTwo/57.3;
00000e  ed9f8b30          VLDR     d8,|L5.208|
000012  2e07              CMP      r6,#7                 ;524
000014  d207              BCS      |L5.38|
000016  e8dff006          TBB      [pc,r6]               ;524
00001a  0604              DCB      0x06,0x04
00001c  09121b28          DCB      0x09,0x12,0x1b,0x28
000020  3f00              DCB      0x3f,0x00
000022  482d              LDR      r0,|L5.216|
000024  7404              STRB     r4,[r0,#0x10]         ;527
                  |L5.38|
;;;555    				agv.Command.SetPlatformTurnAngle = dataTwo;
;;;556    				break;
;;;557    		case 6: //AGV原地旋转左转
;;;558    				agv.Command.HandMotorState = dataOne;
;;;559            agv.Command.SetChassisTurnAngle = (float)dataTwo/57.3;
;;;560    				agv.Command.SetPlatformTurnAngle = dataTwo;
;;;561    				break;
;;;562        }
;;;563    }
000026  ecbd8b02          VPOP     {d8}
00002a  bd70              POP      {r4-r6,pc}
00002c  2102              MOVS     r1,#2                 ;531
00002e  7581              STRB     r1,[r0,#0x16]         ;531
000030  ee004a10          VMOV     s0,r4                 ;532
000034  eeb80ac0          VCVT.F32.S32 s0,s0                 ;532
000038  ed850a08          VSTR     s0,[r5,#0x20]         ;532
00003c  e7f3              B        |L5.38|
00003e  2103              MOVS     r1,#3                 ;536
000040  7581              STRB     r1,[r0,#0x16]         ;536
000042  ee004a10          VMOV     s0,r4                 ;537
000046  eeb80ac0          VCVT.F32.S32 s0,s0                 ;537
00004a  ed850a08          VSTR     s0,[r5,#0x20]         ;537
00004e  e7ea              B        |L5.38|
000050  b114              CBZ      r4,|L5.88|
000052  2c01              CMP      r4,#1                 ;541
000054  d109              BNE      |L5.106|
000056  e004              B        |L5.98|
                  |L5.88|
000058  2101              MOVS     r1,#1                 ;544
00005a  2009              MOVS     r0,#9                 ;544
00005c  f7fffffe          BL       GPIO_SetOutput_24V
000060  e003              B        |L5.106|
                  |L5.98|
000062  2100              MOVS     r1,#0                 ;548
000064  2009              MOVS     r0,#9                 ;548
000066  f7fffffe          BL       GPIO_SetOutput_24V
                  |L5.106|
00006a  716e              STRB     r6,[r5,#5]            ;553
00006c  ee004a10          VMOV     s0,r4                 ;554
000070  eeb80ac0          VCVT.F32.S32 s0,s0                 ;554
000074  ee100a10          VMOV     r0,s0                 ;554
000078  f7fffffe          BL       __aeabi_f2d
00007c  ec532b18          VMOV     r2,r3,d8              ;554
000080  f7fffffe          BL       __aeabi_ddiv
000084  f7fffffe          BL       __aeabi_d2f
000088  6268              STR      r0,[r5,#0x24]         ;554  ; agv
00008a  ee004a10          VMOV     s0,r4                 ;555
00008e  eeb80ac0          VCVT.F32.S32 s0,s0                 ;555
000092  ed850a08          VSTR     s0,[r5,#0x20]         ;555
000096  e7c6              B        |L5.38|
000098  2006              MOVS     r0,#6                 ;558
00009a  7168              STRB     r0,[r5,#5]            ;558
00009c  ee004a10          VMOV     s0,r4                 ;559
0000a0  eeb80ac0          VCVT.F32.S32 s0,s0                 ;559
0000a4  ee100a10          VMOV     r0,s0                 ;559
0000a8  f7fffffe          BL       __aeabi_f2d
0000ac  ec532b18          VMOV     r2,r3,d8              ;559
0000b0  f7fffffe          BL       __aeabi_ddiv
0000b4  f7fffffe          BL       __aeabi_d2f
0000b8  6268              STR      r0,[r5,#0x24]         ;559  ; agv
0000ba  ee004a10          VMOV     s0,r4                 ;560
0000be  eeb80ac0          VCVT.F32.S32 s0,s0                 ;560
0000c2  ed850a08          VSTR     s0,[r5,#0x20]         ;560
0000c6  e7ae              B        |L5.38|
;;;564    #endif
                          ENDP

                  |L5.200|
                          DCD      Rotate1
                  |L5.204|
                          DCD      agv
                  |L5.208|
0000d0  66666666          DCFD     0x404ca66666666666 ; 57.299999999999997
0000d4  404ca666
                  |L5.216|
                          DCD      Lifter1

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

                  initPlaformParam PROC
;;;3      #if P_SETUP_PLATFORM_TYPE == 5
;;;4      void initPlaformParam(void) //参数初始化
000000  4770              BX       lr
;;;5      {
;;;6      //    Uart_Printf(COM1, "参数初始化\r\n");
;;;7      //    Y9(0); // 关闭agv自动充电触点
;;;8
;;;9      //    agv.Command.standSiteID = 1;
;;;10     //    agv.Command.Au_Hand = 0;
;;;11     //		agv.Public.i_StartFirst = false;
;;;12     //    agv.Public.i_UpdatePathSig = 1;
;;;13
;;;14     //    DriverMotor1.Command.speedSlope = 5;
;;;15     //    DriverMotor2.Command.speedSlope = 5;
;;;16     //    DriverLifter1.Command.speedSlope = 50;
;;;17     //    DriverShifter1.Command.speedSlope = 50;
;;;18
;;;19     //		#if P_SETUP_SINGLE_PLAYER == 0 //0不开启单机测试  1开启
;;;20     //		SetAlarm(0x0040);
;;;21     //		#endif
;;;22     }
;;;23
                          ENDP


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

                  lifterRunAuto PROC
;;;85     u8 resetLiftFlag = 0;
;;;86     void lifterRunAuto() //举升自动运行,运行状态0不动  1上升  2下降
000000  b570              PUSH     {r4-r6,lr}
;;;87     {
;;;88         static u8 FirstToZero1 = 1;
;;;89
;;;90     		switch (Lifter1.runState)
000002  4c34              LDR      r4,|L7.212|
;;;91     		{
;;;92     			case 0:
;;;93     				Lifter1.setSpeed = 0;
;;;94     				break;
;;;95     			case 1:
;;;96     				Lifter1.setSpeed = 500000;
;;;97     				break;
;;;98     			case 2:
;;;99     				Lifter1.setSpeed = -500000;
000004  ed9f0a34          VLDR     s0,|L7.216|
000008  eddf0a34          VLDR     s1,|L7.220|
00000c  7c20              LDRB     r0,[r4,#0x10]         ;90  ; Lifter1
00000e  2803              CMP      r0,#3                 ;90
000010  d20d              BCS      |L7.46|
000012  e8dff000          TBB      [pc,r0]               ;90
000016  0205              DCB      0x02,0x05
000018  0a00              DCB      0x0a,0x00
00001a  edc40a01          VSTR     s1,[r4,#4]            ;93
00001e  e006              B        |L7.46|
000020  ed9f1a2f          VLDR     s2,|L7.224|
000024  ed841a01          VSTR     s2,[r4,#4]            ;96
000028  e001              B        |L7.46|
00002a  ed840a01          VSTR     s0,[r4,#4]
                  |L7.46|
;;;100    				break;
;;;101    			case 3:
;;;102    				break;
;;;103    			case 4:
;;;104    				break;
;;;105    			case 5:
;;;106    				break;
;;;107    			case 6:
;;;108    				break;
;;;109    			case 7:
;;;110    				break;
;;;111    			case 8:
;;;112    				break;
;;;113    			default:
;;;114    				break;
;;;115
;;;116    		}
;;;117    		if(FirstToZero1)
00002e  492d              LDR      r1,|L7.228|
000030  7848              LDRB     r0,[r1,#1]  ; FirstToZero1
000032  b140              CBZ      r0,|L7.70|
;;;118    		{
;;;119    			if(Lifter1.setSpeed > 0)
000034  ed941a01          VLDR     s2,[r4,#4]
000038  eeb51ac0          VCMPE.F32 s2,#0.0
00003c  eef1fa10          VMRS     APSR_nzcv,FPSCR
000040  dd01              BLE      |L7.70|
;;;120    			{
;;;121    				Lifter1.setSpeed = 0;
000042  edc40a01          VSTR     s1,[r4,#4]
                  |L7.70|
;;;122    			}
;;;123    		}
;;;124        if ((FirstToZero1 && agv.Public.i_StartFirst)||resetLiftFlag) //下层第一次上电归零
000046  4a28              LDR      r2,|L7.232|
000048  b110              CBZ      r0,|L7.80|
00004a  f8920084          LDRB     r0,[r2,#0x84]  ; agv
00004e  b908              CBNZ     r0,|L7.84|
                  |L7.80|
000050  7808              LDRB     r0,[r1,#0]  ; resetLiftFlag
000052  b108              CBZ      r0,|L7.88|
                  |L7.84|
;;;125        {
;;;126            Lifter1.setSpeed = -500000;
000054  ed840a01          VSTR     s0,[r4,#4]
                  |L7.88|
;;;127        }
;;;128        //升降限制
;;;129        if ((agv.Public.i_Lift1DownLimit && Lifter1.setSpeed < 0)||(Lifter1.encodePos > 43 && Lifter1.setSpeed > 0))
000058  f892006b          LDRB     r0,[r2,#0x6b]  ; agv
00005c  4a1d              LDR      r2,|L7.212|
00005e  2500              MOVS     r5,#0
000060  ed920a03          VLDR     s0,[r2,#0xc]
000064  b130              CBZ      r0,|L7.116|
000066  ed941a01          VLDR     s2,[r4,#4]
00006a  eeb51ac0          VCMPE.F32 s2,#0.0
00006e  eef1fa10          VMRS     APSR_nzcv,FPSCR
000072  d30b              BCC      |L7.140|
                  |L7.116|
000074  ee102a10          VMOV     r2,s0
000078  4b1c              LDR      r3,|L7.236|
00007a  429a              CMP      r2,r3
00007c  dd0b              BLE      |L7.150|
00007e  ed941a01          VLDR     s2,[r4,#4]
000082  eeb51ac0          VCMPE.F32 s2,#0.0
000086  eef1fa10          VMRS     APSR_nzcv,FPSCR
00008a  dd04              BLE      |L7.150|
                  |L7.140|
;;;130        {
;;;131    			resetLiftFlag = 0;
00008c  700d              STRB     r5,[r1,#0]
;;;132            FirstToZero1 = 0;
00008e  704d              STRB     r5,[r1,#1]
;;;133            Lifter1.setSpeed = 0;
000090  edc40a01          VSTR     s1,[r4,#4]
;;;134    			Lifter1.runState = 0;
000094  7425              STRB     r5,[r4,#0x10]
                  |L7.150|
;;;135        }
;;;136    		//举升状态
;;;137    		if(agv.Public.i_Lift1DownLimit)
000096  b118              CBZ      r0,|L7.160|
;;;138    		{
;;;139    			Lifter1.liftState = -1;
000098  f04f30ff          MOV      r0,#0xffffffff
00009c  6160              STR      r0,[r4,#0x14]  ; Lifter1
;;;140    		}
;;;141    		else if(fabs(Lifter1.encodePos - 30) < 1)//小于1MM认为是到达目标点
;;;142    		{
;;;143    			Lifter1.liftState = 0;
;;;144    		}
;;;145    		else
;;;146    			Lifter1.liftState = 1;
;;;147    }
00009e  bd70              POP      {r4-r6,pc}
                  |L7.160|
0000a0  eef30a0e          VMOV.F32 s1,#30.00000000       ;141
0000a4  ee300a60          VSUB.F32 s0,s0,s1              ;141
0000a8  ee100a10          VMOV     r0,s0                 ;141
0000ac  f7fffffe          BL       __aeabi_f2d
0000b0  ec410b10          VMOV     d0,r0,r1              ;141
0000b4  f7fffffe          BL       __hardfp_fabs
0000b8  ed9f1b0d          VLDR     d1,|L7.240|
0000bc  ec510b10          VMOV     r0,r1,d0              ;141
0000c0  ec532b11          VMOV     r2,r3,d1              ;141
0000c4  f7fffffe          BL       __aeabi_cdcmple
0000c8  d201              BCS      |L7.206|
0000ca  6165              STR      r5,[r4,#0x14]         ;143  ; Lifter1
0000cc  bd70              POP      {r4-r6,pc}
                  |L7.206|
0000ce  2001              MOVS     r0,#1                 ;146
0000d0  6160              STR      r0,[r4,#0x14]         ;146  ; Lifter1
0000d2  bd70              POP      {r4-r6,pc}
;;;148
                          ENDP

                  |L7.212|
                          DCD      Lifter1
                  |L7.216|
0000d8  c8f42400          DCFS     0xc8f42400 ; -500000
                  |L7.220|
0000dc  00000000          DCFS     0x00000000 ; 0
                  |L7.224|
0000e0  48f42400          DCFS     0x48f42400 ; 500000
                  |L7.228|
                          DCD      ||.data||
                  |L7.232|
                          DCD      agv
                  |L7.236|
                          DCD      0x422c0000
                  |L7.240|
0000f0  00000000          DCFD     0x3ff0000000000000 ; 1
0000f4  3ff00000

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

                  platformControlAuto PROC
;;;357
;;;358    void platformControlAuto() //平台自动对接
000000  b510              PUSH     {r4,lr}
;;;359    {
;;;360        lifterRunAuto();
000002  f7fffffe          BL       lifterRunAuto
;;;361        RotateRunAuto();
000006  e8bd4010          POP      {r4,lr}
00000a  f7ffbffe          B.W      RotateRunAuto
;;;362    }
;;;363
                          ENDP


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

                  platformControlManual PROC
;;;349
;;;350    void platformControlManual() //平台手动运行
000000  b510              PUSH     {r4,lr}
;;;351    {
;;;352    //    lifterRunManu(&DriverLifter1);
;;;353    //    shifterRunManu(&DriverShifter1);
;;;354    		lifterRunAuto();
000002  f7fffffe          BL       lifterRunAuto
;;;355        RotateRunAuto();
000006  e8bd4010          POP      {r4,lr}
00000a  f7ffbffe          B.W      RotateRunAuto
;;;356    }
;;;357
                          ENDP


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

                  platformDataProcess PROC
;;;406
;;;407    void platformDataProcess(void)
000000  b570              PUSH     {r4-r6,lr}
;;;408    {
;;;409    	X_Input();//IO输入信号
000002  f7fffffe          BL       X_Input
;;;410    	//自动模式且开启红外，雷达避障信号
;;;411    	if(agv.Command.Au_Hand&&agv.Command.Barrier_ONOFF)
000006  4c5d              LDR      r4,|L10.380|
000008  2500              MOVS     r5,#0
00000a  7920              LDRB     r0,[r4,#4]  ; agv
00000c  b308              CBZ      r0,|L10.82|
00000e  7aa0              LDRB     r0,[r4,#0xa]  ; agv
000010  b1f8              CBZ      r0,|L10.82|
;;;412    	{
;;;413    		if(agv.Command.CurDirection||agv.Command.HandMotorState)//前进方向
000012  7f20              LDRB     r0,[r4,#0x1c]  ; agv
000014  b908              CBNZ     r0,|L10.26|
000016  7960              LDRB     r0,[r4,#5]  ; agv
000018  b310              CBZ      r0,|L10.96|
                  |L10.26|
;;;414    		{
;;;415    			if(agv.Public.i_RadarSigFront_3)//最里层雷达避障
00001a  f8940066          LDRB     r0,[r4,#0x66]  ; agv
00001e  b118              CBZ      r0,|L10.40|
;;;416    			{
;;;417    				SetAlarm(0x0001);//停车
000020  2001              MOVS     r0,#1
000022  f7fffffe          BL       SetAlarm
000026  e002              B        |L10.46|
                  |L10.40|
;;;418    			}
;;;419    			else
;;;420    			{
;;;421    				Reset_Alarm(0x0001);//取消停车避障
000028  2001              MOVS     r0,#1
00002a  f7fffffe          BL       Reset_Alarm
                  |L10.46|
;;;422    			}
;;;423    			if(agv.Public.i_RadarSigFront_2)//二级减速
00002e  f8941065          LDRB     r1,[r4,#0x65]  ; agv
000032  2001              MOVS     r0,#1                 ;417
000034  b111              CBZ      r1,|L10.60|
;;;424    			{
;;;425    				agv.Public.i_DoubleReduction = 1;//开启二级减速
000036  f884007f          STRB     r0,[r4,#0x7f]
00003a  e001              B        |L10.64|
                  |L10.60|
;;;426    			}
;;;427    			else
;;;428    			{
;;;429    				agv.Public.i_DoubleReduction = 0;//取消二级减速
00003c  f884507f          STRB     r5,[r4,#0x7f]
                  |L10.64|
;;;430    			}
;;;431    			if(agv.Public.i_RadarSigFront_1)//一级减速
000040  f8941064          LDRB     r1,[r4,#0x64]  ; agv
000044  b111              CBZ      r1,|L10.76|
;;;432    			{
;;;433    				agv.Public.i_Reduction = 1;//开启一级减速
000046  f884007e          STRB     r0,[r4,#0x7e]
00004a  e009              B        |L10.96|
                  |L10.76|
;;;434    			}
;;;435    			else
;;;436    			{
;;;437    				agv.Public.i_Reduction = 0;//取消一级减速
00004c  f884507e          STRB     r5,[r4,#0x7e]
000050  e006              B        |L10.96|
                  |L10.82|
;;;438    			}
;;;439    		}
;;;440
;;;441    	}
;;;442    	else//不开启避障
;;;443    	{
;;;444    		Reset_Alarm(0x0001);//取消停车避障
000052  2001              MOVS     r0,#1
000054  f7fffffe          BL       Reset_Alarm
;;;445
;;;446    		agv.Public.i_Reduction = 0;//取消一级减速
000058  f884507e          STRB     r5,[r4,#0x7e]
;;;447
;;;448    		agv.Public.i_DoubleReduction = 0;//取消二级减速
00005c  f884507f          STRB     r5,[r4,#0x7f]
                  |L10.96|
;;;449    	}
;;;450    	//防撞条
;;;451    	if(agv.Public.i_BumperSig)
000060  f8940063          LDRB     r0,[r4,#0x63]  ; agv
000064  b118              CBZ      r0,|L10.110|
;;;452    	{
;;;453    		SetAlarm(0x0006);//停车，防撞条报警和暂停触发
000066  2006              MOVS     r0,#6
000068  f7fffffe          BL       SetAlarm
00006c  e002              B        |L10.116|
                  |L10.110|
;;;454    	}
;;;455    	else
;;;456    		Reset_Alarm(0x0002);//取消防撞条报警
00006e  2002              MOVS     r0,#2
000070  f7fffffe          BL       Reset_Alarm
                  |L10.116|
;;;457
;;;458    	//急停触发
;;;459    	if (agv.Public.i_QuickStopSig)
000074  f8940060          LDRB     r0,[r4,#0x60]  ; agv
000078  b120              CBZ      r0,|L10.132|
;;;460    	{
;;;461    		SetAlarm(0x0104);//急停和暂停触发，防止再启动
00007a  f44f7082          MOV      r0,#0x104
00007e  f7fffffe          BL       SetAlarm
000082  e003              B        |L10.140|
                  |L10.132|
;;;462    	}
;;;463    	else
;;;464    	{
;;;465    		Reset_Alarm(0x0100);//取消急停报警
000084  f44f7080          MOV      r0,#0x100
000088  f7fffffe          BL       Reset_Alarm
                  |L10.140|
;;;466    	}
;;;467    	if(agv.Public.i_StartSig)//启动
00008c  f8940061          LDRB     r0,[r4,#0x61]  ; agv
000090  b110              CBZ      r0,|L10.152|
;;;468    	{
;;;469    		Reset_Alarm(0x0004);
000092  2004              MOVS     r0,#4
000094  f7fffffe          BL       Reset_Alarm
                  |L10.152|
;;;470    	}
;;;471    	if(agv.Public.i_StopSig)
000098  f8940062          LDRB     r0,[r4,#0x62]  ; agv
00009c  b110              CBZ      r0,|L10.164|
;;;472    	{
;;;473    		SetAlarm(0x0004);//暂停
00009e  2004              MOVS     r0,#4
0000a0  f7fffffe          BL       SetAlarm
                  |L10.164|
;;;474    	}
;;;475    	if(agv.Public.i_Lift1DownLimit)//拉线编码器清零
0000a4  f894106b          LDRB     r1,[r4,#0x6b]  ; agv
;;;476    	{
;;;477    		TIM_SetCounter(TIM1, 0);
0000a8  4d35              LDR      r5,|L10.384|
0000aa  b119              CBZ      r1,|L10.180|
0000ac  2100              MOVS     r1,#0
0000ae  4628              MOV      r0,r5
0000b0  f7fffffe          BL       TIM_SetCounter
                  |L10.180|
;;;478    	}
;;;479
;;;480    	//启动和暂停指示灯
;;;481    	if(agv.Command.Au_Hand && agv.Public.Error_Flag==0)//自动模式无异常显示启动灯
0000b4  7921              LDRB     r1,[r4,#4]  ; agv
0000b6  b111              CBZ      r1,|L10.190|
0000b8  f8d41080          LDR      r1,[r4,#0x80]  ; agv
0000bc  b3c9              CBZ      r1,|L10.306|
                  |L10.190|
;;;482    	{
;;;483    		Y8(1);
;;;484    		Y9(0);
;;;485    	}
;;;486    	else
;;;487    	{
;;;488    		Y8(0);
0000be  2100              MOVS     r1,#0
0000c0  2007              MOVS     r0,#7
0000c2  f7fffffe          BL       GPIO_SetOutput_24V
;;;489    		Y9(1);
0000c6  2101              MOVS     r1,#1
0000c8  2008              MOVS     r0,#8
0000ca  f7fffffe          BL       GPIO_SetOutput_24V
                  |L10.206|
;;;490    	}
;;;491
;;;492    	agv.Public.i_quadEncoder1 = TIM_GetCounter(TIM1);
0000ce  4628              MOV      r0,r5
0000d0  f7fffffe          BL       TIM_GetCounter
0000d4  3454              ADDS     r4,r4,#0x54
0000d6  86e0              STRH     r0,[r4,#0x36]
;;;493    	agv.Public.i_quadEncoder2 = TIM_GetCounter(TIM2);
0000d8  f04f4080          MOV      r0,#0x40000000
0000dc  f7fffffe          BL       TIM_GetCounter
0000e0  8720              STRH     r0,[r4,#0x38]
;;;494    	agv.Public.i_quadEncoder3 = TIM_GetCounter(TIM3);
0000e2  4828              LDR      r0,|L10.388|
0000e4  f7fffffe          BL       TIM_GetCounter
0000e8  8760              STRH     r0,[r4,#0x3a]
;;;495    	agv.Public.i_quadEncoder4 = TIM_GetCounter(TIM4);
0000ea  4827              LDR      r0,|L10.392|
0000ec  f7fffffe          BL       TIM_GetCounter
0000f0  87a0              STRH     r0,[r4,#0x3c]
;;;496    	agv.Public.i_LiftHeightFirst  = agv.Public.i_quadEncoder1/4;
0000f2  f9b40036          LDRSH    r0,[r4,#0x36]  ; agv
0000f6  17c1              ASRS     r1,r0,#31
0000f8  eb007091          ADD      r0,r0,r1,LSR #30
0000fc  1080              ASRS     r0,r0,#2
0000fe  6420              STR      r0,[r4,#0x40]  ; agv
;;;497
;;;498      checkMaterialState(); //物料检测
000100  f7fffffe          BL       checkMaterialState
;;;499
;;;500
;;;501    	//自动模式脱轨判断
;;;502    	if(agv.Command.Au_Hand)
000104  f8140c50          LDRB     r0,[r4,#-0x50]  ; agv
000108  3c54              SUBS     r4,r4,#0x54
00010a  2800              CMP      r0,#0
00010c  d035              BEQ      |L10.378|
;;;503    	{
;;;504    		//前进后退，弧线左右转做脱轨判断
;;;505    		if(agv.Command.CurDirection == 1||agv.Command.CurDirection == 2||agv.Command.CurDirection == 7
00010e  7f20              LDRB     r0,[r4,#0x1c]  ; agv
000110  2801              CMP      r0,#1
000112  d017              BEQ      |L10.324|
000114  2802              CMP      r0,#2
000116  d015              BEQ      |L10.324|
000118  2807              CMP      r0,#7
00011a  d013              BEQ      |L10.324|
;;;506    			||agv.Command.CurDirection == 8||agv.Command.CurDirection == 9||agv.Command.CurDirection == 10)
00011c  2808              CMP      r0,#8
00011e  d011              BEQ      |L10.324|
000120  2809              CMP      r0,#9
000122  d00f              BEQ      |L10.324|
000124  280a              CMP      r0,#0xa
000126  d00d              BEQ      |L10.324|
;;;507    		{
;;;508    			if(fabs(navi.Public.CenterOffset) > 100)//位置偏差大于10CM，脱轨
;;;509    			{
;;;510    				SetAlarm(0x0008);//脱轨报警
;;;511    			}
;;;512    			else
;;;513    				Reset_Alarm(0x0008);//取消脱轨报警
;;;514    		}
;;;515    		else
;;;516    			Reset_Alarm(0x0008);//取消脱轨报警
000128  e8bd4070          POP      {r4-r6,lr}
00012c  2008              MOVS     r0,#8
00012e  f7ffbffe          B.W      Reset_Alarm
                  |L10.306|
000132  2101              MOVS     r1,#1                 ;483
000134  2007              MOVS     r0,#7                 ;483
000136  f7fffffe          BL       GPIO_SetOutput_24V
00013a  2100              MOVS     r1,#0                 ;484
00013c  2008              MOVS     r0,#8                 ;484
00013e  f7fffffe          BL       GPIO_SetOutput_24V
000142  e7c4              B        |L10.206|
                  |L10.324|
000144  4811              LDR      r0,|L10.396|
000146  6840              LDR      r0,[r0,#4]            ;508  ; navi
000148  f7fffffe          BL       __aeabi_f2d
00014c  ec410b10          VMOV     d0,r0,r1              ;508
000150  f7fffffe          BL       __hardfp_fabs
000154  ed9f1b0e          VLDR     d1,|L10.400|
000158  ec510b10          VMOV     r0,r1,d0              ;508
00015c  ec532b11          VMOV     r2,r3,d1              ;508
000160  f7fffffe          BL       __aeabi_cdrcmple
000164  d204              BCS      |L10.368|
000166  e8bd4070          POP      {r4-r6,lr}            ;510
00016a  2008              MOVS     r0,#8                 ;510
00016c  f7ffbffe          B.W      SetAlarm
                  |L10.368|
000170  e8bd4070          POP      {r4-r6,lr}            ;513
000174  2008              MOVS     r0,#8                 ;513
000176  f7ffbffe          B.W      Reset_Alarm
                  |L10.378|
;;;517    	}
;;;518    }
00017a  bd70              POP      {r4-r6,pc}
;;;519
                          ENDP

                  |L10.380|
                          DCD      agv
                  |L10.384|
                          DCD      0x40010000
                  |L10.388|
                          DCD      0x40000400
                  |L10.392|
                          DCD      0x40000800
                  |L10.396|
                          DCD      navi
                  |L10.400|
000190  00000000          DCFD     0x4059000000000000 ; 100
000194  40590000

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

                  resetLiftFlag
000000  00                DCB      0x00
                  FirstToZero1
000001  01                DCB      0x01
                  TurnFlag
000002  0000              DCB      0x00,0x00
                  turnCount
                          DCD      0x00000000
                  palstanceOffset
000008  3d3851ec          DCFS     0x3d3851ec ; 0.045000001788139343
                  palstanceMin
00000c  3db851ec          DCFS     0x3db851ec ; 0.090000003576278687
                  setArriveAngle
000010  3f800000          DCFS     0x3f800000 ; 1
                  setPalstance
000014  00000000          DCFS     0x00000000 ; 0
                  angleOffset
000018  00000000          DCFS     0x00000000 ; 0
                  allowOffset
00001c  41a00000          DCFS     0x41a00000 ; 20
                  lastAngle
000020  00000000          DCFS     0x00000000 ; 0
                  diffAngle
000024  00000000          DCFS     0x00000000 ; 0
                  alreadyTurnAngle
000028  00000000          DCFS     0x00000000 ; 0
                  lastPalstance
00002c  00000000          DCFS     0x00000000 ; 0
                  countNum
                          DCD      0x000003e8
                  lastTime
                          DCD      0x00000000

;*** Start embedded assembler ***

#line 1 "..\\..\\User\\PLATFORM\\KIVA.c"
	AREA ||.rev16_text||, CODE
	THUMB
	EXPORT |__asm___6_KIVA_c_edacf173____REV16|
#line 129 "..\\..\\Libraries\\CMSIS\\Include\\core_cmInstr.h"
|__asm___6_KIVA_c_edacf173____REV16| PROC
#line 130

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

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

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***