AGV / agv_shengyiPPxiaoche · Files

; 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\ppc.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\ppc.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\ppc.crf ..\..\User\NAVAGATION\PPC.c]
                          THUMB

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

                  ArriveJugement PROC
;;;15     //到达判断
;;;16     unsigned char ArriveJugement()
000000  e92d41f0          PUSH     {r4-r8,lr}
;;;17     {
;;;18     	static u8 lastStopFlagL = 0,lastStopFlagR = 0;
;;;19     	if(navi.Public.VerticalDistanceAgvToTARGET < LIGHT_STOP_SIGNAL_DIS)
000004  492a              LDR      r1,|L1.176|
000006  4a2b              LDR      r2,|L1.180|
000008  ed910a06          VLDR     s0,[r1,#0x18]
00000c  ee100a10          VMOV     r0,s0
000010  4290              CMP      r0,r2
000012  da4a              BGE      |L1.170|
;;;20     	{
;;;21     		if(agv.Command.LightStopSig == 1)//左光电停靠
000014  4d28              LDR      r5,|L1.184|
;;;22     		{
;;;23     			if(agv.Public.i_LightStopFlagL == 1&&lastStopFlagL == 0)
000016  4b29              LDR      r3,|L1.188|
000018  4628              MOV      r0,r5                 ;21
00001a  7dea              LDRB     r2,[r5,#0x17]         ;21  ; agv
00001c  f890007c          LDRB     r0,[r0,#0x7c]
000020  2a01              CMP      r2,#1                 ;21
000022  d018              BEQ      |L1.86|
;;;24     			{
;;;25     				LightArriveFlagL = 1;
;;;26     			}
;;;27     			if(LightArriveFlagL == 1)
;;;28     			{
;;;29     				Uart_Printf(COM1,"左光电到达  %d\r\n",agv.Command.LightStopSig);
;;;30     				return 1;
;;;31     			}
;;;32     			if(agv.Public.i_LightStopFlagL == 0 && navi.Public.VerticalDistanceAgvToTARGET < REFLECTIVE_RANGE)
;;;33     			{
;;;34     				navi.Private.FirstZeroPoint = 1;
;;;35     			}
;;;36     			if(navi.Private.FirstZeroPoint && navi.Public.VerticalDistanceAgvToTARGET > REFLECTIVE_RANGE*2)
;;;37     			{
;;;38     				Uart_Printf(COM1,"未扫到光电到达  %d\r\n",agv.Command.LightStopSig);
;;;39     				return 1;
;;;40     			}
;;;41     		}
;;;42     		else if(agv.Command.LightStopSig == 1)//右光电停靠
;;;43     		{
;;;44     			if(agv.Public.i_LightStopFlagR == 1&&lastStopFlagR == 0)
;;;45     			{
;;;46     				LightArriveFlagR = 1;
;;;47     			}
;;;48     			if(LightArriveFlagR == 1)
;;;49     			{
;;;50     				Uart_Printf(COM1,"右光电到达  %d\r\n",agv.Command.LightStopSig);
;;;51     				return 1;
;;;52     			}
;;;53     		}
;;;54     		else
;;;55     		{
;;;56     			if ((navi.Public.VerticalDistanceAgvToSTART >= (navi.Public.DistanceSTARTtoTARGET - POSARRIVE_RANGE)) && (navi.Public.VerticalDistanceAgvToTARGET < navi.Public.VerticalDistanceAgvToSTART))
000024  ed911a05          VLDR     s2,[r1,#0x14]
000028  eef11a04          VMOV.F32 s3,#5.00000000
00002c  edd10a07          VLDR     s1,[r1,#0x1c]
000030  ee311a61          VSUB.F32 s2,s2,s3
000034  eef40ac1          VCMPE.F32 s1,s2
000038  eef1fa10          VMRS     APSR_nzcv,FPSCR
00003c  db31              BLT      |L1.162|
00003e  eeb40ae0          VCMPE.F32 s0,s1
000042  eef1fa10          VMRS     APSR_nzcv,FPSCR
000046  d22c              BCS      |L1.162|
;;;57     			{
;;;58     				Uart_Printf(COM1,"正常到达  %d\r\n",agv.Command.LightStopSig);
000048  a11d              ADR      r1,|L1.192|
00004a  2001              MOVS     r0,#1
00004c  f7fffffe          BL       Uart_Printf
;;;59     				return 1;
000050  2001              MOVS     r0,#1
                  |L1.82|
;;;60     			}
;;;61     		}
;;;62     		lastStopFlagL = agv.Public.i_LightStopFlagL;
;;;63     	
;;;64     		lastStopFlagR = agv.Public.i_LightStopFlagR;
;;;65     	}
;;;66     	return 0;
;;;67     }
000052  e8bd81f0          POP      {r4-r8,pc}
                  |L1.86|
000056  2401              MOVS     r4,#1                 ;21
000058  2801              CMP      r0,#1                 ;23
00005a  d102              BNE      |L1.98|
00005c  781e              LDRB     r6,[r3,#0]            ;23  ; lastStopFlagL
00005e  b906              CBNZ     r6,|L1.98|
000060  605c              STR      r4,[r3,#4]            ;25  ; LightArriveFlagL
                  |L1.98|
000062  685e              LDR      r6,[r3,#4]            ;27  ; LightArriveFlagL
000064  2e01              CMP      r6,#1                 ;27
000066  d001              BEQ      |L1.108|
000068  b130              CBZ      r0,|L1.120|
00006a  e00c              B        |L1.134|
                  |L1.108|
00006c  a118              ADR      r1,|L1.208|
00006e  2001              MOVS     r0,#1                 ;29
000070  f7fffffe          BL       Uart_Printf
000074  2001              MOVS     r0,#1                 ;30
000076  e7ec              B        |L1.82|
                  |L1.120|
000078  ee106a10          VMOV     r6,s0                 ;32
00007c  4f19              LDR      r7,|L1.228|
00007e  42be              CMP      r6,r7                 ;32
000080  da01              BGE      |L1.134|
000082  f881402c          STRB     r4,[r1,#0x2c]         ;34
                  |L1.134|
000086  f891102c          LDRB     r1,[r1,#0x2c]         ;36  ; navi
00008a  b151              CBZ      r1,|L1.162|
00008c  ee101a10          VMOV     r1,s0                 ;36
000090  4c15              LDR      r4,|L1.232|
000092  42a1              CMP      r1,r4                 ;36
000094  dd05              BLE      |L1.162|
000096  a115              ADR      r1,|L1.236|
000098  2001              MOVS     r0,#1                 ;38
00009a  f7fffffe          BL       Uart_Printf
00009e  2001              MOVS     r0,#1                 ;39
0000a0  e7d7              B        |L1.82|
                  |L1.162|
0000a2  7018              STRB     r0,[r3,#0]            ;62
0000a4  f895007d          LDRB     r0,[r5,#0x7d]         ;64  ; agv
0000a8  7058              STRB     r0,[r3,#1]            ;64
                  |L1.170|
0000aa  2000              MOVS     r0,#0                 ;66
0000ac  e7d1              B        |L1.82|
;;;68     //更新执行站点
                          ENDP

0000ae  0000              DCW      0x0000
                  |L1.176|
                          DCD      navi
                  |L1.180|
                          DCD      0x41f00000
                  |L1.184|
                          DCD      agv
                  |L1.188|
                          DCD      ||.data||
                  |L1.192|
0000c0  d5fdb3a3          DCB      213,253,179,163,181,189,180,239,"  %d\r\n",0
0000c4  b5bdb4ef
0000c8  20202564
0000cc  0d0a00  
0000cf  00                DCB      0
                  |L1.208|
0000d0  d7f3b9e2          DCB      215,243,185,226,181,231,181,189,180,239,"  %d\r\n",0
0000d4  b5e7b5bd
0000d8  b4ef2020
0000dc  25640d0a
0000e0  00      
0000e1  00                DCB      0
0000e2  00                DCB      0
0000e3  00                DCB      0
                  |L1.228|
                          DCD      0x42480000
                  |L1.232|
                          DCD      0x42c80000
                  |L1.236|
0000ec  ceb4c9a8          DCB      206,180,201,168,181,189,185,226,181,231,181,189,180,239," "
0000f0  b5bdb9e2
0000f4  b5e7b5bd
0000f8  b4ef20  
0000fb  2025640d          DCB      " %d\r\n",0
0000ff  0a00    
000101  00                DCB      0
000102  00                DCB      0
000103  00                DCB      0

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

                  calculateOffsetValue PROC
;;;191    //计算偏移量 设置不同运动方式的pid值
;;;192    void calculateOffsetValue(unsigned char Direction)
000000  b510              PUSH     {r4,lr}
;;;193    {
;;;194        if (Direction == 7 || Direction == 8 || Direction == 9 || Direction == 10) //转弯需要计算圆心坐标
000002  2807              CMP      r0,#7
000004  d005              BEQ      |L2.18|
000006  2808              CMP      r0,#8
000008  d003              BEQ      |L2.18|
00000a  2809              CMP      r0,#9
00000c  d001              BEQ      |L2.18|
00000e  280a              CMP      r0,#0xa
000010  d12f              BNE      |L2.114|
                  |L2.18|
;;;195        {
;;;196            CircleCenterPoint = GetCircleCenterPoint(CurrentCenterPoint,PointThree,PointFour);                       //计算圆心坐标
000012  4818              LDR      r0,|L2.116|
000014  ed904a02          VLDR     s8,[r0,#8]
000018  edd03a01          VLDR     s7,[r0,#4]
00001c  ed903a00          VLDR     s6,[r0,#0]
000020  4815              LDR      r0,|L2.120|
000022  edd02a02          VLDR     s5,[r0,#8]
000026  ed902a01          VLDR     s4,[r0,#4]
00002a  edd01a00          VLDR     s3,[r0,#0]
00002e  4813              LDR      r0,|L2.124|
000030  ed901a02          VLDR     s2,[r0,#8]
000034  edd00a01          VLDR     s1,[r0,#4]
000038  ed900a00          VLDR     s0,[r0,#0]
00003c  f7fffffe          BL       GetCircleCenterPoint
000040  480f              LDR      r0,|L2.128|
000042  ed800a00          VSTR     s0,[r0,#0]
000046  edc00a01          VSTR     s1,[r0,#4]
00004a  ed801a02          VSTR     s2,[r0,#8]
;;;197            navi.Private.SetCalculationRadius = TwoPointDistance(PointTwo, CircleCenterPoint); //入弯点到圆心的距离
00004e  480d              LDR      r0,|L2.132|
000050  eef02a41          VMOV.F32 s5,s2
000054  eeb02a60          VMOV.F32 s4,s1
000058  eef01a40          VMOV.F32 s3,s0
00005c  ed901a02          VLDR     s2,[r0,#8]
000060  edd00a01          VLDR     s1,[r0,#4]
000064  ed900a00          VLDR     s0,[r0,#0]
000068  f7fffffe          BL       TwoPointDistance
00006c  4806              LDR      r0,|L2.136|
00006e  ed800a08          VSTR     s0,[r0,#0x20]
                  |L2.114|
;;;198        }
;;;199    
;;;200    }
000072  bd10              POP      {r4,pc}
;;;201    
                          ENDP

                  |L2.116|
                          DCD      PointFour
                  |L2.120|
                          DCD      PointThree
                  |L2.124|
                          DCD      CurrentCenterPoint
                  |L2.128|
                          DCD      CircleCenterPoint
                  |L2.132|
                          DCD      PointTwo
                  |L2.136|
                          DCD      navi

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

                  getControlFrontDistance PROC
;;;172    //1,获取前视距离,根据剩余距离调整前视距离
;;;173    float getControlFrontDistance(float *distance)
000000  6800              LDR      r0,[r0,#0]
;;;174    {
;;;175    	float valueDis = 0;
;;;176    	
;;;177    	if(*distance > 1000)
000002  4904              LDR      r1,|L3.20|
000004  4288              CMP      r0,r1
000006  dd02              BLE      |L3.14|
;;;178    	{
;;;179    		valueDis = 1000;
000008  ed9f0a03          VLDR     s0,|L3.24|
;;;180    	}
;;;181    	else
;;;182    		valueDis = 200;
;;;183    	
;;;184    	return valueDis;
;;;185    }
00000c  4770              BX       lr
                  |L3.14|
00000e  ed9f0a03          VLDR     s0,|L3.28|
000012  4770              BX       lr
;;;186    
                          ENDP

                  |L3.20|
                          DCD      0x447a0000
                  |L3.24|
000018  447a0000          DCFS     0x447a0000 ; 1000
                  |L3.28|
00001c  43480000          DCFS     0x43480000 ; 200

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

                  laserDataUpdate PROC
;;;4      //获取激光数据slam,此补偿不是将雷达位置补偿到车体中心而是工位坐标特殊补偿，每个工位的补偿值不一样，龙旗项目使用中
;;;5      void laserDataUpdate() //
000000  480d              LDR      r0,|L4.56|
;;;6      {
;;;7          CurrentCenterPoint.TarX = agv.Public.Coordinate_X + agv.Command.X_compensation; //X补偿
000002  490e              LDR      r1,|L4.60|
000004  ed900a2c          VLDR     s0,[r0,#0xb0]
000008  edd00a0a          VLDR     s1,[r0,#0x28]
00000c  ee300a20          VADD.F32 s0,s0,s1
000010  ed810a00          VSTR     s0,[r1,#0]
;;;8      
;;;9          CurrentCenterPoint.TarY = agv.Public.Coordinate_Y + agv.Command.Y_compensation; //Y补偿
000014  ed900a2d          VLDR     s0,[r0,#0xb4]
000018  edd00a0b          VLDR     s1,[r0,#0x2c]
00001c  ee300a20          VADD.F32 s0,s0,s1
000020  ed810a01          VSTR     s0,[r1,#4]
;;;10     
;;;11         CurrentCenterPoint.CurAngle = agv.Public.Coordinate_W + agv.Command.W_compensation; //W补偿
000024  ed900a2e          VLDR     s0,[r0,#0xb8]
000028  edd00a0c          VLDR     s1,[r0,#0x30]
00002c  ee300a20          VADD.F32 s0,s0,s1
000030  ed810a02          VSTR     s0,[r1,#8]
;;;12     }
000034  4770              BX       lr
;;;13     
                          ENDP

000036  0000              DCW      0x0000
                  |L4.56|
                          DCD      agv
                  |L4.60|
                          DCD      CurrentCenterPoint

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

                  offsetCompensationOutput PROC
;;;202    //根据P值，I值，D值，位置偏差权重，角度偏差权重，输出偏移值
;;;203    void offsetCompensationOutput(navigationPID *Pid)
000000  e92d4ff0          PUSH     {r4-r11,lr}
;;;204    {
000004  ed2d8b02          VPUSH    {d8}
000008  b09b              SUB      sp,sp,#0x6c
;;;205    	static int i = 0;
;;;206    	static float LastCenterOffset = 0, LastAngleOffset = 0;
;;;207    	static float Error = 0, DcalError = 0, LastError = 0, SumError = 0;
;;;208    	//当位置和角度偏差变化重新计算
;;;209    	if (LastCenterOffset != navi.Public.CenterOffset || LastAngleOffset != navi.Public.AngleDifference)
00000a  4a64              LDR      r2,|L5.412|
00000c  4964              LDR      r1,|L5.416|
00000e  4613              MOV      r3,r2
000010  edd28a01          VLDR     s17,[r2,#4]
000014  ed910a05          VLDR     s0,[r1,#0x14]
000018  ed938a00          VLDR     s16,[r3,#0]
00001c  eef48a40          VCMP.F32 s17,s0
000020  eef1fa10          VMRS     APSR_nzcv,FPSCR
000024  d106              BNE      |L5.52|
000026  ed910a06          VLDR     s0,[r1,#0x18]
00002a  eeb48a40          VCMP.F32 s16,s0
00002e  eef1fa10          VMRS     APSR_nzcv,FPSCR
000032  d01d              BEQ      |L5.112|
                  |L5.52|
;;;210    	{
;;;211    			LastCenterOffset = navi.Public.CenterOffset;
000034  edc18a05          VSTR     s17,[r1,#0x14]
;;;212    
;;;213    			LastAngleOffset = navi.Public.AngleDifference;
000038  ed818a06          VSTR     s16,[r1,#0x18]
;;;214    			//取角度和位置偏差做为偏差补偿量
;;;215    			Error = navi.Public.CenterOffset * Pid->PosCofficient + navi.Public.AngleDifference * 180 / PI * Pid->AngleCofficient;
00003c  ed9f0a59          VLDR     s0,|L5.420|
000040  ee680a00          VMUL.F32 s1,s16,s0
000044  ed9f1a58          VLDR     s2,|L5.424|
000048  ee800a81          VDIV.F32 s0,s1,s2
00004c  edd00a04          VLDR     s1,[r0,#0x10]
000050  ee200a20          VMUL.F32 s0,s0,s1
000054  edd00a03          VLDR     s1,[r0,#0xc]
000058  ee080aa0          VMLA.F32 s0,s17,s1
00005c  ed810a07          VSTR     s0,[r1,#0x1c]
;;;216    
;;;217    			DcalError = Error - LastError; //误差变化量
000060  edd10a09          VLDR     s1,[r1,#0x24]
000064  ee700a60          VSUB.F32 s1,s0,s1
000068  edc10a08          VSTR     s1,[r1,#0x20]
;;;218    
;;;219    			LastError = Error; //记录上次偏差
00006c  ed810a09          VSTR     s0,[r1,#0x24]
                  |L5.112|
;;;220    	}
;;;221    	
;;;222    	navi.Private.OutputOffset = Error * Pid->Kp + SumError * Pid->Ki + DcalError * Pid->Kd;
000070  edd00a00          VLDR     s1,[r0,#0]
000074  ed910a07          VLDR     s0,[r1,#0x1c]
000078  ed911a0a          VLDR     s2,[r1,#0x28]
00007c  ee200a80          VMUL.F32 s0,s1,s0
000080  edd00a01          VLDR     s1,[r0,#4]
000084  ee010a20          VMLA.F32 s0,s2,s1
000088  edd00a02          VLDR     s1,[r0,#8]
00008c  ed911a08          VLDR     s2,[r1,#0x20]
000090  ee010a20          VMLA.F32 s0,s2,s1
000094  ed820a0a          VSTR     s0,[r2,#0x28]
;;;223    
;;;224    	if (navi.Private.OutputOffset >= Pid->MaxLimit)
000098  edd00a05          VLDR     s1,[r0,#0x14]
00009c  eeb40ae0          VCMPE.F32 s0,s1
0000a0  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000a4  db02              BLT      |L5.172|
;;;225    			navi.Private.OutputOffset = Pid->MaxLimit;
0000a6  edc20a0a          VSTR     s1,[r2,#0x28]
0000aa  e008              B        |L5.190|
                  |L5.172|
;;;226    	else if (navi.Private.OutputOffset <= -Pid->MaxLimit)
0000ac  eef10a60          VNEG.F32 s1,s1
0000b0  eeb40ae0          VCMPE.F32 s0,s1
0000b4  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000b8  d801              BHI      |L5.190|
;;;227    			navi.Private.OutputOffset = -Pid->MaxLimit;
0000ba  edc20a0a          VSTR     s1,[r2,#0x28]
                  |L5.190|
;;;228    	
;;;229    	#if PRINT_TYPE_SELECT == 1
;;;230    	if (i++ > PRINT_FREQUENCY)
0000be  6908              LDR      r0,[r1,#0x10]  ; i
0000c0  1c43              ADDS     r3,r0,#1
0000c2  610b              STR      r3,[r1,#0x10]  ; i
0000c4  280a              CMP      r0,#0xa
0000c6  dd63              BLE      |L5.400|
;;;231    	{
;;;232    			i = 0;
0000c8  2000              MOVS     r0,#0
0000ca  6108              STR      r0,[r1,#0x10]  ; i
;;;233    			//		Uart_Printf(COM1,"当前方向 = %d,角度偏差 = %.2f,位置偏差 = %.2f,输出补偿值 = %.2f\r\n",
;;;234    			//		agv.Command.CurDirection,navi.Public.AngleDifference*57.3,navi.Public.CenterOffset,navi.Private.OutputOffset);
;;;235    			Uart_Printf(COM1, "X = %.2f Y = %.2f 当前方向 = %d,发送速度 = %.2f %.2f 反馈速度 = %.2f %.2f 角度偏差 = %.2f,位置偏差 = %.2f,输出补偿值 = %.2f\r\n", agv.Public.Coordinate_X, agv.Public.Coordinate_Y,
0000cc  6a90              LDR      r0,[r2,#0x28]  ; navi
0000ce  f7fffffe          BL       __aeabi_f2d
0000d2  4604              MOV      r4,r0
0000d4  460d              MOV      r5,r1
0000d6  ee180a90          VMOV     r0,s17
0000da  f7fffffe          BL       __aeabi_f2d
0000de  4606              MOV      r6,r0
0000e0  460f              MOV      r7,r1
0000e2  ee180a10          VMOV     r0,s16
0000e6  f7fffffe          BL       __aeabi_f2d
0000ea  ed9f1b30          VLDR     d1,|L5.428|
0000ee  ec532b11          VMOV     r2,r3,d1
0000f2  f7fffffe          BL       __aeabi_dmul
0000f6  f8df80bc          LDR      r8,|L5.436|
0000fa  4682              MOV      r10,r0
0000fc  468b              MOV      r11,r1
0000fe  f8d80084          LDR      r0,[r8,#0x84]  ; DriverMotor2
000102  f7fffffe          BL       __aeabi_f2d
000106  f8df90b0          LDR      r9,|L5.440|
00010a  e9cd0118          STRD     r0,r1,[sp,#0x60]
00010e  f8d90084          LDR      r0,[r9,#0x84]  ; DriverMotor1
000112  f7fffffe          BL       __aeabi_f2d
000116  e9cd0116          STRD     r0,r1,[sp,#0x58]
00011a  f8d80008          LDR      r0,[r8,#8]  ; DriverMotor2
00011e  f7fffffe          BL       __aeabi_f2d
000122  e9cd0114          STRD     r0,r1,[sp,#0x50]
000126  f8d90008          LDR      r0,[r9,#8]  ; DriverMotor1
00012a  f7fffffe          BL       __aeabi_f2d
00012e  e9cd0112          STRD     r0,r1,[sp,#0x48]
000132  4822              LDR      r0,|L5.444|
000134  f8d000b4          LDR      r0,[r0,#0xb4]  ; agv
000138  f7fffffe          BL       __aeabi_f2d
00013c  4680              MOV      r8,r0
00013e  481f              LDR      r0,|L5.444|
000140  4689              MOV      r9,r1
000142  f8d000b0          LDR      r0,[r0,#0xb0]  ; agv
000146  f7fffffe          BL       __aeabi_f2d
00014a  ec410b10          VMOV     d0,r0,r1
00014e  e9cd670e          STRD     r6,r7,[sp,#0x38]
000152  e9dd0118          LDRD     r0,r1,[sp,#0x60]
000156  e9cdab0c          STRD     r10,r11,[sp,#0x30]
00015a  e9cd4510          STRD     r4,r5,[sp,#0x40]
00015e  e9cd010a          STRD     r0,r1,[sp,#0x28]
000162  e9dd0116          LDRD     r0,r1,[sp,#0x58]
000166  e9cd0108          STRD     r0,r1,[sp,#0x20]
00016a  e9dd0114          LDRD     r0,r1,[sp,#0x50]
00016e  e9cd0106          STRD     r0,r1,[sp,#0x18]
000172  e9dd0112          LDRD     r0,r1,[sp,#0x48]
000176  e9cd0104          STRD     r0,r1,[sp,#0x10]
00017a  4810              LDR      r0,|L5.444|
00017c  ec532b10          VMOV     r2,r3,d0
000180  490f              LDR      r1,|L5.448|
000182  7f00              LDRB     r0,[r0,#0x1c]  ; agv
000184  9002              STR      r0,[sp,#8]
000186  e9cd8900          STRD     r8,r9,[sp,#0]
00018a  2001              MOVS     r0,#1
00018c  f7fffffe          BL       Uart_Printf
                  |L5.400|
;;;236    									agv.Command.CurDirection, DriverMotor1.Command.speed, DriverMotor2.Command.speed, DriverMotor1.Public.encoderSpeed, DriverMotor2.Public.encoderSpeed, navi.Public.AngleDifference * 57.3, navi.Public.CenterOffset, navi.Private.OutputOffset);
;;;237    	}
;;;238    	#endif
;;;239    }
000190  b01b              ADD      sp,sp,#0x6c
000192  ecbd8b02          VPOP     {d8}
000196  e8bd8ff0          POP      {r4-r11,pc}
;;;240    
                          ENDP

00019a  0000              DCW      0x0000
                  |L5.412|
                          DCD      navi
                  |L5.416|
                          DCD      ||.data||
                  |L5.420|
0001a4  43340000          DCFS     0x43340000 ; 180
                  |L5.424|
0001a8  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L5.428|
0001ac  66666666          DCFD     0x404ca66666666666 ; 57.299999999999997
0001b0  404ca666
                  |L5.436|
                          DCD      DriverMotor2
                  |L5.440|
                          DCD      DriverMotor1
                  |L5.444|
                          DCD      agv
                  |L5.448|
                          DCD      ||.conststring||

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

                  pathUpdate PROC
;;;68     //更新执行站点
;;;69     void pathUpdate(u8 *CurrentID)
000000  e92d43f8          PUSH     {r3-r9,lr}
;;;70     {
000004  4604              MOV      r4,r0
;;;71         static int lastTime = 0;
;;;72     		static float lastAnglediff = 0;
;;;73         /*****************到达站点判断以及切换站点**********************/
;;;74         if (agv.Command.CurDirection == 1 || agv.Command.CurDirection == 2) //直行判断
000006  4e8f              LDR      r6,|L6.580|
;;;75         {
;;;76             //到达站点判断
;;;77             if (ArriveJugement()==1)
;;;78             {
;;;79     					navi.Private.FirstZeroPoint = 0;
000008  4f8f              LDR      r7,|L6.584|
;;;80                 *CurrentID += 1;
;;;81     /****单机测试***/
;;;82     #if P_SETUP_SINGLE_PLAYER == 1
;;;83                 if (*CurrentID >= traffic_land_marks.size)
;;;84                 {
;;;85                     *CurrentID = 1;
;;;86     
;;;87                     traffic_land_marks.land_marks[0].Direction = 4;
;;;88     
;;;89                     traffic_land_marks.land_marks[3].Direction = 3;
;;;90                 }
;;;91     #elif P_SETUP_SINGLE_PLAYER == 0
;;;92                 if (*CurrentID >= traffic_land_marks.size)
;;;93                 {
;;;94                    if((agv.Command.LightStopSig == 1 && LightArriveFlagL == 1)||(agv.Command.LightStopSig == 2 && LightArriveFlagR == 1))
00000a  4d90              LDR      r5,|L6.588|
00000c  7f30              LDRB     r0,[r6,#0x1c]         ;74  ; agv
00000e  f8df8240          LDR      r8,|L6.592|
000012  f04f0900          MOV      r9,#0                 ;74
000016  2801              CMP      r0,#1                 ;74
000018  d00a              BEQ      |L6.48|
00001a  2802              CMP      r0,#2                 ;74
00001c  d008              BEQ      |L6.48|
;;;95     							 {
;;;96     								 SetAlarm(0x200); 
;;;97     							 }
;;;98     							 else
;;;99     							 {
;;;100    								 SetAlarm(0x40);
;;;101    							 }
;;;102    							 LightArriveFlagL = 0;
;;;103    						
;;;104    							 LightArriveFlagR = 0;
;;;105    						
;;;106    							 agv.Public.i_UpdatePathSig = 1;
;;;107                }
;;;108    #endif
;;;109                Uart_Printf(COM1, "站点更新，当前站点 = %d 当前方向 = %d Error_Flag = %x\r\n", agv.Command.standSiteID, agv.Command.CurDirection, agv.Public.Error_Flag);
;;;110            }
;;;111    				lastAnglediff = 0;
;;;112        }
;;;113        else if (agv.Command.CurDirection == 7 || agv.Command.CurDirection == 8 || agv.Command.CurDirection == 9 || agv.Command.CurDirection == 10) //弧线转弯
00001e  2807              CMP      r0,#7
000020  d03c              BEQ      |L6.156|
000022  2808              CMP      r0,#8
000024  d03a              BEQ      |L6.156|
000026  2809              CMP      r0,#9
000028  d038              BEQ      |L6.156|
00002a  280a              CMP      r0,#0xa
00002c  d036              BEQ      |L6.156|
00002e  e057              B        |L6.224|
                  |L6.48|
000030  f7fffffe          BL       ArriveJugement
000034  2801              CMP      r0,#1                 ;77
000036  d127              BNE      |L6.136|
000038  f887902c          STRB     r9,[r7,#0x2c]         ;79
00003c  7820              LDRB     r0,[r4,#0]            ;80
00003e  1c40              ADDS     r0,r0,#1              ;80
000040  b2c0              UXTB     r0,r0                 ;80
000042  7020              STRB     r0,[r4,#0]            ;80
000044  f8d81004          LDR      r1,[r8,#4]            ;92  ; traffic_land_marks
000048  4288              CMP      r0,r1                 ;92
00004a  db14              BLT      |L6.118|
00004c  7df0              LDRB     r0,[r6,#0x17]         ;94  ; agv
00004e  2801              CMP      r0,#1                 ;94
000050  d102              BNE      |L6.88|
000052  6869              LDR      r1,[r5,#4]            ;94  ; LightArriveFlagL
000054  2901              CMP      r1,#1                 ;94
000056  d01c              BEQ      |L6.146|
                  |L6.88|
000058  2802              CMP      r0,#2                 ;94
00005a  d102              BNE      |L6.98|
00005c  68a8              LDR      r0,[r5,#8]            ;94  ; LightArriveFlagR
00005e  2801              CMP      r0,#1                 ;94
000060  d017              BEQ      |L6.146|
                  |L6.98|
000062  2040              MOVS     r0,#0x40              ;100
000064  f7fffffe          BL       SetAlarm
                  |L6.104|
000068  f8c59004          STR      r9,[r5,#4]            ;102  ; LightArriveFlagL
00006c  f8c59008          STR      r9,[r5,#8]            ;104  ; LightArriveFlagR
000070  2001              MOVS     r0,#1                 ;106
000072  f8860093          STRB     r0,[r6,#0x93]         ;106
                  |L6.118|
000076  f8d60080          LDR      r0,[r6,#0x80]         ;109  ; agv
00007a  9000              STR      r0,[sp,#0]            ;109
00007c  7f33              LDRB     r3,[r6,#0x1c]         ;109  ; agv
00007e  7db2              LDRB     r2,[r6,#0x16]         ;109  ; agv
000080  a174              ADR      r1,|L6.596|
000082  2001              MOVS     r0,#1                 ;109
000084  f7fffffe          BL       Uart_Printf
                  |L6.136|
000088  ed9f0a80          VLDR     s0,|L6.652|
00008c  ed850a03          VSTR     s0,[r5,#0xc]          ;111
000090  e026              B        |L6.224|
                  |L6.146|
000092  f44f7000          MOV      r0,#0x200             ;96
000096  f7fffffe          BL       SetAlarm
00009a  e7e5              B        |L6.104|
                  |L6.156|
;;;114        {
;;;115            //站点更新
;;;116            if ((fabs(navi.Public.AngleDifference) <= TURNINGOFF_ANGLE)||(navi.Public.AngleDifference*lastAnglediff<0))
00009c  6838              LDR      r0,[r7,#0]  ; navi
00009e  f7fffffe          BL       __aeabi_f2d
0000a2  ec410b10          VMOV     d0,r0,r1
0000a6  f7fffffe          BL       __hardfp_fabs
0000aa  ed9f1b79          VLDR     d1,|L6.656|
0000ae  ec510b10          VMOV     r0,r1,d0
0000b2  ec532b11          VMOV     r2,r3,d1
0000b6  f7fffffe          BL       __aeabi_cdcmple
0000ba  d90a              BLS      |L6.210|
0000bc  edd70a00          VLDR     s1,[r7,#0]
0000c0  ed950a03          VLDR     s0,[r5,#0xc]
0000c4  ee200a80          VMUL.F32 s0,s1,s0
0000c8  eeb50ac0          VCMPE.F32 s0,#0.0
0000cc  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000d0  d202              BCS      |L6.216|
                  |L6.210|
;;;117            {
;;;118                *CurrentID += 1;
0000d2  7820              LDRB     r0,[r4,#0]
0000d4  1c40              ADDS     r0,r0,#1
0000d6  7020              STRB     r0,[r4,#0]
                  |L6.216|
;;;119            }
;;;120    				lastAnglediff = navi.Public.AngleDifference;
0000d8  ed970a00          VLDR     s0,[r7,#0]
0000dc  ed850a03          VSTR     s0,[r5,#0xc]
                  |L6.224|
;;;121        }
;;;122        agv.Command.DispatchSpeed = traffic_land_marks.land_marks[*CurrentID - 1].DispatchSpeed; //系统下发速度
0000e0  7820              LDRB     r0,[r4,#0]
0000e2  1e40              SUBS     r0,r0,#1
0000e4  eb000040          ADD      r0,r0,r0,LSL #1
0000e8  eb0800c0          ADD      r0,r8,r0,LSL #3
0000ec  8b80              LDRH     r0,[r0,#0x1c]
0000ee  8330              STRH     r0,[r6,#0x18]
;;;123    
;;;124        agv.Command.CurDirection = traffic_land_marks.land_marks[*CurrentID - 1].Direction; //当前站点运行方向
0000f0  7820              LDRB     r0,[r4,#0]
0000f2  1e40              SUBS     r0,r0,#1
0000f4  eb000040          ADD      r0,r0,r0,LSL #1
0000f8  eb0800c0          ADD      r0,r8,r0,LSL #3
0000fc  7e80              LDRB     r0,[r0,#0x1a]
0000fe  7730              STRB     r0,[r6,#0x1c]
;;;125    
;;;126        if (*CurrentID < traffic_land_marks.size) //当前站点不是最后站点，保存下一个站点方向和光电停车信号
000100  7820              LDRB     r0,[r4,#0]
000102  f8d81004          LDR      r1,[r8,#4]  ; traffic_land_marks
000106  4288              CMP      r0,r1
000108  da15              BGE      |L6.310|
;;;127        {
;;;128            agv.Command.LightStopSig = traffic_land_marks.land_marks[*CurrentID].LightStopSig; //目标站点广电信号
00010a  eb000040          ADD      r0,r0,r0,LSL #1
00010e  eb0800c0          ADD      r0,r8,r0,LSL #3
000112  f8900020          LDRB     r0,[r0,#0x20]
000116  75f0              STRB     r0,[r6,#0x17]
;;;129    
;;;130            agv.Command.NextDirection = traffic_land_marks.land_marks[*CurrentID].Direction; //下个站点运行方向
000118  7820              LDRB     r0,[r4,#0]
00011a  eb000040          ADD      r0,r0,r0,LSL #1
00011e  eb0800c0          ADD      r0,r8,r0,LSL #3
000122  7e80              LDRB     r0,[r0,#0x1a]
000124  7770              STRB     r0,[r6,#0x1d]
;;;131    
;;;132            agv.Command.NextDispatchSpeed = traffic_land_marks.land_marks[*CurrentID].DispatchSpeed;
000126  7820              LDRB     r0,[r4,#0]
000128  eb000040          ADD      r0,r0,r0,LSL #1
00012c  eb0800c0          ADD      r0,r8,r0,LSL #3
000130  8b80              LDRH     r0,[r0,#0x1c]
000132  8370              STRH     r0,[r6,#0x1a]
000134  e005              B        |L6.322|
                  |L6.310|
;;;133        }
;;;134        else //已经是最后一个站点，没有下一个站点清零
;;;135        {
;;;136            agv.Command.LightStopSig = 0;
000136  f8869017          STRB     r9,[r6,#0x17]
;;;137    
;;;138            agv.Command.NextDirection = 0;
00013a  f886901d          STRB     r9,[r6,#0x1d]
;;;139    
;;;140            agv.Command.NextDispatchSpeed = 0;
00013e  f8a6901a          STRH     r9,[r6,#0x1a]
                  |L6.322|
;;;141        }
;;;142    
;;;143        StartPoint.TarX = traffic_land_marks.land_marks[*CurrentID - 1].pose[0]; //当前点（起点）
000142  7820              LDRB     r0,[r4,#0]
000144  4a54              LDR      r2,|L6.664|
000146  1e40              SUBS     r0,r0,#1
000148  eb000040          ADD      r0,r0,r0,LSL #1
00014c  eb0800c0          ADD      r0,r8,r0,LSL #3
000150  ed900a03          VLDR     s0,[r0,#0xc]
000154  ed820a00          VSTR     s0,[r2,#0]
;;;144    
;;;145        StartPoint.TarY = traffic_land_marks.land_marks[*CurrentID - 1].pose[1];
000158  7820              LDRB     r0,[r4,#0]
00015a  1e40              SUBS     r0,r0,#1
00015c  eb000040          ADD      r0,r0,r0,LSL #1
000160  eb0800c0          ADD      r0,r8,r0,LSL #3
000164  ed900a04          VLDR     s0,[r0,#0x10]
000168  ed820a01          VSTR     s0,[r2,#4]
;;;146    
;;;147        TargetPoint.TarX = traffic_land_marks.land_marks[*CurrentID].pose[0]; //目标点（终点）
00016c  7820              LDRB     r0,[r4,#0]
00016e  4a4b              LDR      r2,|L6.668|
000170  eb000040          ADD      r0,r0,r0,LSL #1
000174  eb0800c0          ADD      r0,r8,r0,LSL #3
000178  ed900a03          VLDR     s0,[r0,#0xc]
00017c  ed820a00          VSTR     s0,[r2,#0]
;;;148    
;;;149        TargetPoint.TarY = traffic_land_marks.land_marks[*CurrentID].pose[1];
000180  7820              LDRB     r0,[r4,#0]
000182  eb000040          ADD      r0,r0,r0,LSL #1
000186  eb0800c0          ADD      r0,r8,r0,LSL #3
00018a  ed900a04          VLDR     s0,[r0,#0x10]
00018e  ed820a01          VSTR     s0,[r2,#4]
;;;150    
;;;151        if (*CurrentID >= 2 && *CurrentID < traffic_land_marks.size - 1) //实现弧线转弯需要有四个点位计算
000192  7820              LDRB     r0,[r4,#0]
000194  2802              CMP      r0,#2
000196  d353              BCC      |L6.576|
000198  1e49              SUBS     r1,r1,#1
00019a  4288              CMP      r0,r1
00019c  da50              BGE      |L6.576|
;;;152        {
;;;153            PointOne.TarX = traffic_land_marks.land_marks[*CurrentID - 2].pose[0];
00019e  1e80              SUBS     r0,r0,#2
0001a0  eb000040          ADD      r0,r0,r0,LSL #1
0001a4  eb0800c0          ADD      r0,r8,r0,LSL #3
0001a8  493d              LDR      r1,|L6.672|
0001aa  ed900a03          VLDR     s0,[r0,#0xc]
0001ae  ed810a00          VSTR     s0,[r1,#0]
;;;154    
;;;155            PointOne.TarY = traffic_land_marks.land_marks[*CurrentID - 2].pose[1];
0001b2  7820              LDRB     r0,[r4,#0]
0001b4  1e80              SUBS     r0,r0,#2
0001b6  eb000040          ADD      r0,r0,r0,LSL #1
0001ba  eb0800c0          ADD      r0,r8,r0,LSL #3
0001be  ed900a04          VLDR     s0,[r0,#0x10]
0001c2  ed810a01          VSTR     s0,[r1,#4]
;;;156    
;;;157            PointTwo.TarX = traffic_land_marks.land_marks[*CurrentID - 1].pose[0];
0001c6  7820              LDRB     r0,[r4,#0]
0001c8  4936              LDR      r1,|L6.676|
0001ca  1e40              SUBS     r0,r0,#1
0001cc  eb000040          ADD      r0,r0,r0,LSL #1
0001d0  eb0800c0          ADD      r0,r8,r0,LSL #3
0001d4  ed900a03          VLDR     s0,[r0,#0xc]
0001d8  ed810a00          VSTR     s0,[r1,#0]
;;;158    
;;;159            PointTwo.TarY = traffic_land_marks.land_marks[*CurrentID - 1].pose[1];
0001dc  7820              LDRB     r0,[r4,#0]
0001de  1e40              SUBS     r0,r0,#1
0001e0  eb000040          ADD      r0,r0,r0,LSL #1
0001e4  eb0800c0          ADD      r0,r8,r0,LSL #3
0001e8  ed900a04          VLDR     s0,[r0,#0x10]
0001ec  ed810a01          VSTR     s0,[r1,#4]
;;;160    
;;;161            PointThree.TarX = traffic_land_marks.land_marks[*CurrentID].pose[0];
0001f0  7820              LDRB     r0,[r4,#0]
0001f2  492d              LDR      r1,|L6.680|
0001f4  eb000040          ADD      r0,r0,r0,LSL #1
0001f8  eb0800c0          ADD      r0,r8,r0,LSL #3
0001fc  ed900a03          VLDR     s0,[r0,#0xc]
000200  ed810a00          VSTR     s0,[r1,#0]
;;;162    
;;;163            PointThree.TarY = traffic_land_marks.land_marks[*CurrentID].pose[1];
000204  7820              LDRB     r0,[r4,#0]
000206  eb000040          ADD      r0,r0,r0,LSL #1
00020a  eb0800c0          ADD      r0,r8,r0,LSL #3
00020e  ed900a04          VLDR     s0,[r0,#0x10]
000212  ed810a01          VSTR     s0,[r1,#4]
;;;164    
;;;165            PointFour.TarX = traffic_land_marks.land_marks[*CurrentID + 1].pose[0];
000216  7820              LDRB     r0,[r4,#0]
000218  4924              LDR      r1,|L6.684|
00021a  1c40              ADDS     r0,r0,#1
00021c  eb000040          ADD      r0,r0,r0,LSL #1
000220  eb0800c0          ADD      r0,r8,r0,LSL #3
000224  ed900a03          VLDR     s0,[r0,#0xc]
000228  ed810a00          VSTR     s0,[r1,#0]
;;;166    
;;;167            PointFour.TarY = traffic_land_marks.land_marks[*CurrentID + 1].pose[1];
00022c  7820              LDRB     r0,[r4,#0]
00022e  1c40              ADDS     r0,r0,#1
000230  eb000040          ADD      r0,r0,r0,LSL #1
000234  eb0800c0          ADD      r0,r8,r0,LSL #3
000238  ed900a04          VLDR     s0,[r0,#0x10]
00023c  ed810a01          VSTR     s0,[r1,#4]
                  |L6.576|
;;;168        }
;;;169    }
000240  e8bd83f8          POP      {r3-r9,pc}
;;;170    
                          ENDP

                  |L6.580|
                          DCD      agv
                  |L6.584|
                          DCD      navi
                  |L6.588|
                          DCD      ||.data||
                  |L6.592|
                          DCD      traffic_land_marks
                  |L6.596|
000254  d5beb5e3          DCB      213,190,181,227,184,252,208,194,163,172,181,177,199,176,213
000258  b8fcd0c2
00025c  a3acb5b1
000260  c7b0d5  
000263  beb5e320          DCB      190,181,227," = %d ",181,177,199,176,183,189,207,242," ="
000267  3d202564
00026b  20b5b1c7
00026f  b0b7bdcf
000273  f2203d  
000276  20256420          DCB      " %d Error_Flag = %x\r\n",0
00027a  4572726f
00027e  725f466c
000282  6167203d
000286  2025780d
00028a  0a00    
                  |L6.652|
00028c  00000000          DCFS     0x00000000 ; 0
                  |L6.656|
000290  e0000000          DCFD     0x3f9eb851e0000000 ; 0.029999999329447746
000294  3f9eb851
                  |L6.664|
                          DCD      StartPoint
                  |L6.668|
                          DCD      TargetPoint
                  |L6.672|
                          DCD      PointOne
                  |L6.676|
                          DCD      PointTwo
                  |L6.680|
                          DCD      PointThree
                  |L6.684|
                          DCD      PointFour

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

                  slamNavigation PROC
;;;240    
;;;241    void slamNavigation()
000000  b5f0              PUSH     {r4-r7,lr}
;;;242    {
000002  ed2d8b02          VPUSH    {d8}
000006  b099              SUB      sp,sp,#0x64
;;;243    	laserDataUpdate(); 										//获取激光数据，以及特殊工位定点补偿**************
000008  f7fffffe          BL       laserDataUpdate
;;;244    
;;;245    	pathUpdate(&agv.Command.standSiteID); //执行路径更新 和站点++判断。
00000c  484d              LDR      r0,|L7.324|
00000e  f7fffffe          BL       pathUpdate
;;;246    
;;;247    	CalculateDistance(); 									//计算起始点、目标点、agv之前的距离
000012  f7fffffe          BL       CalculateDistance
;;;248    	
;;;249    	if(agv.Command.CurDirection == 1 || agv.Command.CurDirection == 2)
000016  484b              LDR      r0,|L7.324|
000018  3816              SUBS     r0,r0,#0x16
00001a  7f00              LDRB     r0,[r0,#0x1c]  ; agv
00001c  2801              CMP      r0,#1
00001e  d001              BEQ      |L7.36|
000020  2802              CMP      r0,#2
000022  d17d              BNE      |L7.288|
                  |L7.36|
;;;250    	{
;;;251    		Line Lpath;
;;;252    		
;;;253    		static float angleDiff = 0,valueDis = 0;
;;;254    		
;;;255    		Lpath = getLine2(StartPoint,TargetPoint);
000024  4848              LDR      r0,|L7.328|
000026  edd02a02          VLDR     s5,[r0,#8]
00002a  ed902a01          VLDR     s4,[r0,#4]
00002e  edd01a00          VLDR     s3,[r0,#0]
000032  4846              LDR      r0,|L7.332|
000034  ed901a02          VLDR     s2,[r0,#8]
000038  edd00a01          VLDR     s1,[r0,#4]
00003c  ed900a00          VLDR     s0,[r0,#0]
000040  4668              MOV      r0,sp
000042  f7fffffe          BL       getLine2
000046  2230              MOVS     r2,#0x30
000048  4669              MOV      r1,sp
00004a  a80c              ADD      r0,sp,#0x30
00004c  f7fffffe          BL       __aeabi_memcpy4
;;;256    		
;;;257    		navi.Private.FrontViewDistance = getControlFrontDistance(&navi.Public.DistanceAgvToTARGET);//获取并调节前视距离
000050  483f              LDR      r0,|L7.336|
000052  f7fffffe          BL       getControlFrontDistance
000056  4d3e              LDR      r5,|L7.336|
000058  eeb08a40          VMOV.F32 s16,s0
00005c  3d10              SUBS     r5,r5,#0x10
00005e  ed850a0c          VSTR     s0,[r5,#0x30]
;;;258    		
;;;259    		FrontViewPoint = getControlTargetPoint(CurrentCenterPoint,Lpath,navi.Private.FrontViewDistance);//求预瞄点
000062  2220              MOVS     r2,#0x20
000064  a910              ADD      r1,sp,#0x40
000066  4668              MOV      r0,sp
000068  f7fffffe          BL       __aeabi_memcpy4
00006c  4c39              LDR      r4,|L7.340|
00006e  a80c              ADD      r0,sp,#0x30
000070  eef01a48          VMOV.F32 s3,s16
000074  c80f              LDM      r0,{r0-r3}
000076  ed941a02          VLDR     s2,[r4,#8]
00007a  edd40a01          VLDR     s1,[r4,#4]
00007e  ed940a00          VLDR     s0,[r4,#0]
000082  f7fffffe          BL       getControlTargetPoint
000086  4e34              LDR      r6,|L7.344|
000088  ed860a00          VSTR     s0,[r6,#0]
00008c  edc60a01          VSTR     s1,[r6,#4]
000090  ed861a02          VSTR     s2,[r6,#8]
;;;260    		
;;;261    		navi.Private.FrontViewAngle = CalculatingDirectionAngle(CurrentCenterPoint,FrontViewPoint);//求当前点与预瞄点角度
000094  eef02a41          VMOV.F32 s5,s2
000098  eeb02a60          VMOV.F32 s4,s1
00009c  eef01a40          VMOV.F32 s3,s0
0000a0  ed941a02          VLDR     s2,[r4,#8]
0000a4  edd40a01          VLDR     s1,[r4,#4]
0000a8  ed940a00          VLDR     s0,[r4,#0]
0000ac  f7fffffe          BL       CalculatingDirectionAngle
0000b0  ed850a0d          VSTR     s0,[r5,#0x34]
0000b4  eef00a40          VMOV.F32 s1,s0
;;;262    		
;;;263    		angleDiff = CalculatingCurrentAndTargetAngle(CurrentCenterPoint.CurAngle,navi.Private.FrontViewAngle);//求车体与预瞄方向夹角
0000b8  ed940a02          VLDR     s0,[r4,#8]
0000bc  f7fffffe          BL       CalculatingCurrentAndTargetAngle
0000c0  4f26              LDR      r7,|L7.348|
0000c2  ed870a0b          VSTR     s0,[r7,#0x2c]
;;;264    		
;;;265    		valueDis = TwoPointDistance(CurrentCenterPoint,FrontViewPoint);
0000c6  edd62a02          VLDR     s5,[r6,#8]
0000ca  ed962a01          VLDR     s4,[r6,#4]
0000ce  edd61a00          VLDR     s3,[r6,#0]
0000d2  ed941a02          VLDR     s2,[r4,#8]
0000d6  edd40a01          VLDR     s1,[r4,#4]
0000da  ed940a00          VLDR     s0,[r4,#0]
0000de  f7fffffe          BL       TwoPointDistance
0000e2  ed870a0c          VSTR     s0,[r7,#0x30]
;;;266    		
;;;267    		if(angleDiff != 0)
0000e6  ed970a0b          VLDR     s0,[r7,#0x2c]
0000ea  eeb50ac0          VCMPE.F32 s0,#0.0
0000ee  eef1fa10          VMRS     APSR_nzcv,FPSCR
0000f2  d021              BEQ      |L7.312|
;;;268    		{
;;;269    			navi.Private.SetCalculationRadius = valueDis/sin(angleDiff)/2;
0000f4  ee100a10          VMOV     r0,s0
0000f8  f7fffffe          BL       __aeabi_f2d
0000fc  ec410b10          VMOV     d0,r0,r1
000100  f7fffffe          BL       __hardfp_sin
000104  eeb08a40          VMOV.F32 s16,s0
000108  eef08a60          VMOV.F32 s17,s1
00010c  6b38              LDR      r0,[r7,#0x30]  ; valueDis
00010e  f7fffffe          BL       __aeabi_f2d
000112  ec532b18          VMOV     r2,r3,d8
000116  f7fffffe          BL       __aeabi_ddiv
00011a  ed9f1b11          VLDR     d1,|L7.352|
00011e  e000              B        |L7.290|
                  |L7.288|
000120  e006              B        |L7.304|
                  |L7.290|
000122  ec532b11          VMOV     r2,r3,d1
000126  f7fffffe          BL       __aeabi_dmul
00012a  f7fffffe          BL       __aeabi_d2f
00012e  6228              STR      r0,[r5,#0x20]  ; navi
                  |L7.304|
;;;270    		}
;;;271    		else
;;;272    			navi.Private.SetCalculationRadius = 0;
;;;273    		
;;;274    	}
;;;275    	else
;;;276    	{
;;;277    		//	calculateOffsetValue(agv.Command.CurDirection); //计算偏移量和设置不同运动方式的pid值
;;;278    
;;;279    		//	offsetCompensationOutput(&pid); 			//计算处理得到补偿值***********
;;;280    	}
;;;281    	
;;;282    	
;;;283    
;;;284    
;;;285    
;;;286        
;;;287    }
000130  b019              ADD      sp,sp,#0x64
000132  ecbd8b02          VPOP     {d8}
000136  bdf0              POP      {r4-r7,pc}
                  |L7.312|
000138  ed9f0a0b          VLDR     s0,|L7.360|
00013c  ed850a08          VSTR     s0,[r5,#0x20]         ;272
000140  e7f6              B        |L7.304|
;;;288    #endif
                          ENDP

000142  0000              DCW      0x0000
                  |L7.324|
                          DCD      agv+0x16
                  |L7.328|
                          DCD      TargetPoint
                  |L7.332|
                          DCD      StartPoint
                  |L7.336|
                          DCD      navi+0x10
                  |L7.340|
                          DCD      CurrentCenterPoint
                  |L7.344|
                          DCD      FrontViewPoint
                  |L7.348|
                          DCD      ||.data||
                  |L7.352|
000160  00000000          DCFD     0x3fe0000000000000 ; 0.5
000164  3fe00000
                  |L7.360|
000168  00000000          DCFS     0x00000000 ; 0

                          AREA ||.conststring||, DATA, READONLY, MERGE=1, STRINGS, ALIGN=2

000000  58203d20          DCB      "X = %.2f Y = %.2f ",181,177,199,176,183,189,207,242," ="
000004  252e3266
000008  2059203d
00000c  20252e32
000010  6620b5b1
000014  c7b0b7bd
000018  cff2203d
00001c  2025642c          DCB      " %d,",183,162,203,205,203,217,182,200," = %.2f %.2f ",183
000020  b7a2cbcd
000024  cbd9b6c8
000028  203d2025
00002c  2e326620
000030  252e3266
000034  20b7    
000036  b4c0a1cb          DCB      180,192,161,203,217,182,200," = %.2f %.2f ",189,199,182,200
00003a  d9b6c820
00003e  3d20252e
000042  32662025
000046  2e326620
00004a  bdc7b6c8
00004e  c6abb2ee          DCB      198,171,178,238," = %.2f,",206,187,214,195,198,171,178,238
000052  203d2025
000056  2e32662c
00005a  cebbd6c3
00005e  c6abb2ee
000062  203d2025          DCB      " = %.2f,",202,228,179,246,178,185,179,165,214,181," = %"
000066  2e32662c
00006a  cae4b3f6
00006e  b2b9b3a5
000072  d6b5203d
000076  2025    
000078  2e32660d          DCB      ".2f\r\n",0
00007c  0a00    

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

                  lastStopFlagL
000000  00                DCB      0x00
                  lastStopFlagR
000001  000000            DCB      0x00,0x00,0x00
                  LightArriveFlagL
                          DCD      0x00000000
                  LightArriveFlagR
                          DCD      0x00000000
                  lastAnglediff
00000c  00000000          DCFS     0x00000000 ; 0
                  i
                          DCD      0x00000000
                  LastCenterOffset
000014  00000000          DCFS     0x00000000 ; 0
                  LastAngleOffset
000018  00000000          DCFS     0x00000000 ; 0
                  Error
00001c  00000000          DCFS     0x00000000 ; 0
                  DcalError
000020  00000000          DCFS     0x00000000 ; 0
                  LastError
000024  00000000          DCFS     0x00000000 ; 0
                  SumError
000028  00000000          DCFS     0x00000000 ; 0
                  angleDiff
00002c  00000000          DCFS     0x00000000 ; 0
                  valueDis
000030  00000000          DCFS     0x00000000 ; 0

;*** Start embedded assembler ***

#line 1 "..\\..\\User\\NAVAGATION\\PPC.c"
	AREA ||.rev16_text||, CODE
	THUMB
	EXPORT |__asm___5_PPC_c_27921a34____REV16|
#line 129 "..\\..\\Libraries\\CMSIS\\Include\\core_cmInstr.h"
|__asm___5_PPC_c_27921a34____REV16| PROC
#line 130

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

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

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***