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

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

                  GetQRcodeinformation PROC
;;;107
;;;108    void GetQRcodeinformation()
000000  b570              PUSH     {r4-r6,lr}
;;;109    {
;;;110    	static int nn=0;
;;;111    	if(nn++ == 1000)
000002  4c0e              LDR      r4,|L1.60|
000004  f24035e9          MOV      r5,#0x3e9
000008  6920              LDR      r0,[r4,#0x10]  ; nn
00000a  1c40              ADDS     r0,r0,#1
00000c  6120              STR      r0,[r4,#0x10]  ; nn
00000e  42a8              CMP      r0,r5
000010  d104              BNE      |L1.28|
;;;112    	{
;;;113    		WriteUart(COM6,selectpostag,2);
000012  2202              MOVS     r2,#2
000014  1ca1              ADDS     r1,r4,#2
000016  2006              MOVS     r0,#6
000018  f7fffffe          BL       WriteUart
                  |L1.28|
;;;114    	}
;;;115    	if(nn > 1000)nn=1001;
00001c  6920              LDR      r0,[r4,#0x10]  ; nn
00001e  f5b07f7a          CMP      r0,#0x3e8
000022  dd00              BLE      |L1.38|
000024  6125              STR      r5,[r4,#0x10]  ; nn
                  |L1.38|
;;;116    	UpdataCamerParameter();           //更新PGV读头x,y,angle数据
000026  f7fffffe          BL       UpdataCamerParameter
;;;117    	ProcessDataFormUartQR();
00002a  f7fffffe          BL       ProcessDataFormUartQR
;;;118
;;;119    	PgvSendCmd(readposandstatedata,2);
00002e  e8bd4070          POP      {r4-r6,lr}
000032  2102              MOVS     r1,#2
000034  4801              LDR      r0,|L1.60|
000036  f7ffbffe          B.W      PgvSendCmd
;;;120    }
;;;121
                          ENDP

00003a  0000              DCW      0x0000
                  |L1.60|
                          DCD      ||.data||

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

                  ParsingPGVTagData PROC
;;;70
;;;71     void ParsingPGVTagData(unsigned char buff[],int* x,int* y,u32* angle,u32* num)     //解析tag标签
000000  b5f0              PUSH     {r4-r7,lr}
;;;72     {
000002  9d05              LDR      r5,[sp,#0x14]
;;;73     	int Byte3_1,data_x,data_y;
;;;74     	Byte3_1 = buff[2] & 0x07;
000004  7884              LDRB     r4,[r0,#2]
000006  f0040407          AND      r4,r4,#7
;;;75
;;;76     	data_x = Byte3_1 * 0x80 * 0x4000 + buff[3] * 0x4000 + buff[4] * 0x80 + buff[5];
00000a  7946              LDRB     r6,[r0,#5]
00000c  7907              LDRB     r7,[r0,#4]
00000e  eb065444          ADD      r4,r6,r4,LSL #21
000012  78c6              LDRB     r6,[r0,#3]
000014  03b6              LSLS     r6,r6,#14
000016  eb0616c7          ADD      r6,r6,r7,LSL #7
00001a  4434              ADD      r4,r4,r6
;;;77     	if(data_x > 0x800000) data_x = -(0x1000000 - data_x);
00001c  f5b40f00          CMP      r4,#0x800000
000020  dd01              BLE      |L2.38|
000022  f1a47480          SUB      r4,r4,#0x1000000
                  |L2.38|
;;;78
;;;79     	*x = data_x;
000026  600c              STR      r4,[r1,#0]
;;;80
;;;81     	data_y = buff[6] * 0x80 + buff[7];
000028  79c1              LDRB     r1,[r0,#7]
00002a  7984              LDRB     r4,[r0,#6]
00002c  eb0111c4          ADD      r1,r1,r4,LSL #7
;;;82     	if(data_y > 0x2000) data_y = -(0x4000 - data_y);
000030  f5b15f00          CMP      r1,#0x2000
000034  dd01              BLE      |L2.58|
000036  f5a14180          SUB      r1,r1,#0x4000
                  |L2.58|
;;;83
;;;84     	*y = data_y;
00003a  6011              STR      r1,[r2,#0]
;;;85
;;;86     	*angle = (buff[10] * 0x80 + buff[11]);
00003c  7ac1              LDRB     r1,[r0,#0xb]
00003e  7a82              LDRB     r2,[r0,#0xa]
000040  eb0111c2          ADD      r1,r1,r2,LSL #7
000044  6019              STR      r1,[r3,#0]
;;;87
;;;88     	*num = buff[14] * 0x80 * 0x4000 + buff[15] * 0x4000 + buff[16] * 0x80 + buff[17];
000046  7c41              LDRB     r1,[r0,#0x11]
000048  7b82              LDRB     r2,[r0,#0xe]
00004a  eb015142          ADD      r1,r1,r2,LSL #21
00004e  7bc2              LDRB     r2,[r0,#0xf]
000050  7c00              LDRB     r0,[r0,#0x10]
000052  0392              LSLS     r2,r2,#14
000054  eb0210c0          ADD      r0,r2,r0,LSL #7
000058  4408              ADD      r0,r0,r1
00005a  6028              STR      r0,[r5,#0]
;;;89     }
00005c  bdf0              POP      {r4-r7,pc}
;;;90
                          ENDP


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

                  PgvSendCmd PROC
;;;16
;;;17     void PgvSendCmd(unsigned char *buff,unsigned int len)
000000  b510              PUSH     {r4,lr}
;;;18     {
000002  460a              MOV      r2,r1
;;;19     	static int i=0;
;;;20     	if(i++ > 10)
000004  4c06              LDR      r4,|L3.32|
000006  6861              LDR      r1,[r4,#4]  ; i
000008  1c4b              ADDS     r3,r1,#1
00000a  6063              STR      r3,[r4,#4]  ; i
00000c  290a              CMP      r1,#0xa
00000e  dd05              BLE      |L3.28|
;;;21     	{
;;;22     		WriteUart(COM6,buff,len);
000010  4601              MOV      r1,r0
000012  2006              MOVS     r0,#6
000014  f7fffffe          BL       WriteUart
;;;23     		i = 0;
000018  2000              MOVS     r0,#0
00001a  6060              STR      r0,[r4,#4]  ; i
                  |L3.28|
;;;24     	}
;;;25     }
00001c  bd10              POP      {r4,pc}
;;;26
                          ENDP

00001e  0000              DCW      0x0000
                  |L3.32|
                          DCD      ||.data||

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

                  ProcessDataFormUartQR PROC
;;;90
;;;91     void ProcessDataFormUartQR()
000000  b500              PUSH     {lr}
;;;92     {
000002  b09b              SUB      sp,sp,#0x6c
;;;93     	unsigned char buff[99];
;;;94     	unsigned int len;
;;;95     	len = ReadUart(COM6,buff,99);
000004  2263              MOVS     r2,#0x63
000006  a902              ADD      r1,sp,#8
000008  2006              MOVS     r0,#6
00000a  f7fffffe          BL       ReadUart
;;;96
;;;97     	if(len == 21)
00000e  2815              CMP      r0,#0x15
000010  d107              BNE      |L4.34|
;;;98     	{
;;;99     		ParsingPGVTagData(buff,&(Tagdata.x),&(Tagdata.y),&(Tagdata.angle),&(Tagdata.tagnumber));
000012  4805              LDR      r0,|L4.40|
000014  9000              STR      r0,[sp,#0]
000016  1f03              SUBS     r3,r0,#4
000018  1f1a              SUBS     r2,r3,#4
00001a  1f11              SUBS     r1,r2,#4
00001c  a802              ADD      r0,sp,#8
00001e  f7fffffe          BL       ParsingPGVTagData
                  |L4.34|
;;;100    	}
;;;101    }
000022  b01b              ADD      sp,sp,#0x6c
000024  bd00              POP      {pc}
;;;102
                          ENDP

000026  0000              DCW      0x0000
                  |L4.40|
                          DCD      ||.bss||+0xc

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

                  QecodeControlFunction PROC
;;;275
;;;276    void QecodeControlFunction(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  e92d41f0          PUSH     {r4-r8,lr}
;;;277    {
000004  460c              MOV      r4,r1
000006  ed2d8b06          VPUSH    {d8-d10}
00000a  4617              MOV      r7,r2
;;;278    	static float F_Angle_compensation_factor = 80;
;;;279    	static float F_Offset_compensation_factor = 0.2f;
;;;280    	float dis,Offset_Straight = 0;
00000c  eddf9aed          VLDR     s19,|L5.964|
;;;281    	static float AngleMigration = 0.0f,OutPutData = 0,AdjustmentCoefficient = 0;
;;;282    	static float Xbias = 0.0f,Ybias = 0.0f,Theata = 0.0f;
;;;283
;;;284    	agvIntegratedLaserAndIntegralData(&Xbias,&Ybias,&Theata,ControlInputValue);
000010  4aed              LDR      r2,|L5.968|
000012  4623              MOV      r3,r4
000014  1f11              SUBS     r1,r2,#4
000016  1f08              SUBS     r0,r1,#4
000018  f7fffffe          BL       agvIntegratedLaserAndIntegralData
;;;285
;;;286    	if((ControlInputValue->agvControlStatus == 0) || (ControlInputValue->OnlineLogo == 0) || (ControlInputValue->OfflineLogo == 1))
00001c  f89400b4          LDRB     r0,[r4,#0xb4]
;;;287    	{
;;;288    //		Uart_Printf(COM1,"Online=%d	Offline=%d	shouzidong=%d	x=%f	y=%f	w=%f	dir=%d\r\n",
;;;289    //		ControlInputValue->OnlineLogo,ControlInputValue->OfflineLogo,ControlInputValue->agvControlStatus,
;;;290    //		Xbias*1000,Ybias*1000,Theata,camerparameter.TwoDCdirection);
;;;291    //		Xbias = 0.0f,Ybias = 0.0f,Theata = 0.0f;
;;;292    		camerparameter.BaseAngle = 0;
000020  4eea              LDR      r6,|L5.972|
000022  b1d8              CBZ      r0,|L5.92|
000024  f89400b8          LDRB     r0,[r4,#0xb8]         ;286
000028  b1c0              CBZ      r0,|L5.92|
00002a  f89400bd          LDRB     r0,[r4,#0xbd]         ;286
00002e  2801              CMP      r0,#1                 ;286
000030  d014              BEQ      |L5.92|
;;;293    		camerparameter.TwoDCdirection = 0;
;;;294    		return;
;;;295    	}
;;;296
;;;297    	AngleMigration = CalculatingCurrentAndTargetAngle(Theata,camerparameter.BaseAngle);
000032  4de5              LDR      r5,|L5.968|
000034  edd60a04          VLDR     s1,[r6,#0x10]
000038  3d4c              SUBS     r5,r5,#0x4c
00003a  ed950a13          VLDR     s0,[r5,#0x4c]
00003e  f7fffffe          BL       CalculatingCurrentAndTargetAngle
000042  ed850a0f          VSTR     s0,[r5,#0x3c]
000046  eef08a40          VMOV.F32 s17,s0
;;;298    	if(ControlInputValue->CurDirection == 1)
00004a  f9b400d0          LDRSH    r0,[r4,#0xd0]
;;;299    	{
;;;300    		switch(camerparameter.TwoDCdirection)
;;;301    		{
;;;302    			case 1:
;;;303    				dis = -(Ybias*1000.0f - ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
00004e  ed9f9ae0          VLDR     s18,|L5.976|
000052  2801              CMP      r0,#1                 ;298
000054  d00a              BEQ      |L5.108|
;;;304    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;305
;;;306    				camerparameter.DistanceCalibrationValue = Xbias*1000.0f;
;;;307
;;;308    				break;
;;;309    			case 2:
;;;310    				dis = (Xbias*1000.0f + ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;311    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;312
;;;313    				camerparameter.DistanceCalibrationValue = Ybias*1000.0f;
;;;314    				break;
;;;315    			case 3:
;;;316    				dis = (Ybias*1000.0f + ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;317    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;318
;;;319    				camerparameter.DistanceCalibrationValue = Xbias*1000.0f;
;;;320    				break;
;;;321    			case 4:
;;;322    				dis = -(Xbias*1000.0f - ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;323    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;324
;;;325    				camerparameter.DistanceCalibrationValue = Ybias*1000.0f;
;;;326    				break;
;;;327    			default:
;;;328    				dis = 0;
;;;329    				break;
;;;330    		}
;;;331    	}
;;;332    	else if(ControlInputValue->CurDirection == 2)
000056  2802              CMP      r0,#2
000058  d07e              BEQ      |L5.344|
00005a  e0d1              B        |L5.512|
                  |L5.92|
00005c  edc69a04          VSTR     s19,[r6,#0x10]        ;292
000060  2000              MOVS     r0,#0                 ;293
000062  7030              STRB     r0,[r6,#0]            ;293
                  |L5.100|
;;;333    	{
;;;334    		switch(camerparameter.TwoDCdirection)
;;;335    		{
;;;336    			case 1:
;;;337    				dis = (Ybias*1000.0f + ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;338    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;339
;;;340    				camerparameter.DistanceCalibrationValue = Xbias*1000.0f;
;;;341    				break;
;;;342    			case 2:
;;;343    				dis = -(Xbias*1000.0f - ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;344    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;345    				camerparameter.DistanceCalibrationValue = Ybias*1000.0f;
;;;346
;;;347    				break;
;;;348    			case 3:
;;;349    				dis = -(Ybias*1000.0f - ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;350    				//OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;351    				camerparameter.DistanceCalibrationValue = Xbias*1000.0f;
;;;352
;;;353    				break;
;;;354    			case 4:
;;;355    				dis = (Xbias*1000.0f + ControlInputValue->LaserDistanceExtensionOne * sin(AngleMigration));
;;;356    				////OutPutData = dis * ControlInputValue->LowSp_F_offset + AngleMigration *F_Angle_compensation_factor;
;;;357
;;;358    				camerparameter.DistanceCalibrationValue = Ybias*1000.0f;
;;;359    				break;
;;;360    			default:
;;;361    				dis = 0;
;;;362    				break;
;;;363    		}
;;;364    	}
;;;365    	Offset_Straight = dis;// + HandlePlcData.RecvPlcData.QRoffset;
;;;366
;;;367    	OutPutData = ControlInputValue->LowSp_F_offset * Offset_Straight;
;;;368    	if(OutPutData >= 30) OutPutData = 30;
;;;369    	if(OutPutData <= -30) OutPutData = -30;
;;;370    	ControlOutputValue->AgvSettingFirstSpeed = ControlOutputValue->SetMoveSpeed +  OutPutData;
;;;371    	ControlOutputValue->AgvSettingSecondSpeed = ControlOutputValue->SetMoveSpeed - OutPutData;
;;;372    }
000064  ecbd8b06          VPOP     {d8-d10}
000068  e8bd81f0          POP      {r4-r8,pc}
                  |L5.108|
00006c  7830              LDRB     r0,[r6,#0]            ;300  ; camerparameter
00006e  2801              CMP      r0,#1                 ;300
000070  d008              BEQ      |L5.132|
000072  2802              CMP      r0,#2                 ;300
000074  d032              BEQ      |L5.220|
000076  2803              CMP      r0,#3                 ;300
000078  d05c              BEQ      |L5.308|
00007a  2804              CMP      r0,#4                 ;300
00007c  d06d              BEQ      |L5.346|
00007e  eeb08a69          VMOV.F32 s16,s19               ;328
000082  e0bd              B        |L5.512|
                  |L5.132|
000084  ed950a12          VLDR     s0,[r5,#0x48]         ;303
000088  ee200a09          VMUL.F32 s0,s0,s18             ;303
00008c  ee100a10          VMOV     r0,s0                 ;303
000090  f7fffffe          BL       __aeabi_f2d
000094  ec410b1a          VMOV     d10,r0,r1             ;303
000098  ee180a90          VMOV     r0,s17                ;303
00009c  f7fffffe          BL       __aeabi_f2d
0000a0  ec410b10          VMOV     d0,r0,r1              ;303
0000a4  f7fffffe          BL       __hardfp_sin
0000a8  eeb08a40          VMOV.F32 s16,s0                ;303
0000ac  eef08a60          VMOV.F32 s17,s1                ;303
0000b0  6ea0              LDR      r0,[r4,#0x68]         ;303
0000b2  f7fffffe          BL       __aeabi_f2d
0000b6  ec532b18          VMOV     r2,r3,d8              ;303
0000ba  f7fffffe          BL       __aeabi_dmul
0000be  ec532b1a          VMOV     r2,r3,d10             ;303
0000c2  f7fffffe          BL       __aeabi_dsub
0000c6  f7fffffe          BL       __aeabi_d2f
0000ca  ee080a10          VMOV     s16,r0                ;303
0000ce  ed950a11          VLDR     s0,[r5,#0x44]         ;306
0000d2  ee200a09          VMUL.F32 s0,s0,s18             ;306
0000d6  ed860a07          VSTR     s0,[r6,#0x1c]         ;306
0000da  e091              B        |L5.512|
                  |L5.220|
0000dc  ee180a90          VMOV     r0,s17                ;310
0000e0  f7fffffe          BL       __aeabi_f2d
0000e4  ec410b10          VMOV     d0,r0,r1              ;310
0000e8  f7fffffe          BL       __hardfp_sin
0000ec  eeb08a40          VMOV.F32 s16,s0                ;310
0000f0  eef08a60          VMOV.F32 s17,s1                ;310
0000f4  6ea0              LDR      r0,[r4,#0x68]         ;310
0000f6  f7fffffe          BL       __aeabi_f2d
0000fa  ec532b18          VMOV     r2,r3,d8              ;310
0000fe  f7fffffe          BL       __aeabi_dmul
000102  ed950a11          VLDR     s0,[r5,#0x44]         ;310
000106  ec410b18          VMOV     d8,r0,r1              ;310
00010a  ee200a09          VMUL.F32 s0,s0,s18             ;310
00010e  ee100a10          VMOV     r0,s0                 ;310
000112  f7fffffe          BL       __aeabi_f2d
000116  ec532b18          VMOV     r2,r3,d8              ;310
00011a  f7fffffe          BL       __aeabi_dadd
00011e  f7fffffe          BL       __aeabi_d2f
000122  ee080a10          VMOV     s16,r0                ;310
000126  ed950a12          VLDR     s0,[r5,#0x48]         ;313
00012a  ee200a09          VMUL.F32 s0,s0,s18             ;313
00012e  ed860a07          VSTR     s0,[r6,#0x1c]         ;313
000132  e065              B        |L5.512|
                  |L5.308|
000134  ee180a90          VMOV     r0,s17                ;316
000138  f7fffffe          BL       __aeabi_f2d
00013c  ec410b10          VMOV     d0,r0,r1              ;316
000140  f7fffffe          BL       __hardfp_sin
000144  eeb08a40          VMOV.F32 s16,s0                ;316
000148  eef08a60          VMOV.F32 s17,s1                ;316
00014c  6ea0              LDR      r0,[r4,#0x68]         ;316
00014e  f7fffffe          BL       __aeabi_f2d
000152  ec532b18          VMOV     r2,r3,d8              ;316
000156  e001              B        |L5.348|
                  |L5.344|
000158  e047              B        |L5.490|
                  |L5.346|
00015a  e01a              B        |L5.402|
                  |L5.348|
00015c  f7fffffe          BL       __aeabi_dmul
000160  ed950a12          VLDR     s0,[r5,#0x48]         ;316
000164  ec410b18          VMOV     d8,r0,r1              ;316
000168  ee200a09          VMUL.F32 s0,s0,s18             ;316
00016c  ee100a10          VMOV     r0,s0                 ;316
000170  f7fffffe          BL       __aeabi_f2d
000174  ec532b18          VMOV     r2,r3,d8              ;316
000178  f7fffffe          BL       __aeabi_dadd
00017c  f7fffffe          BL       __aeabi_d2f
000180  ee080a10          VMOV     s16,r0                ;316
000184  ed950a11          VLDR     s0,[r5,#0x44]         ;319
000188  ee200a09          VMUL.F32 s0,s0,s18             ;319
00018c  ed860a07          VSTR     s0,[r6,#0x1c]         ;319
000190  e036              B        |L5.512|
                  |L5.402|
000192  ed950a11          VLDR     s0,[r5,#0x44]         ;322
000196  ee200a09          VMUL.F32 s0,s0,s18             ;322
00019a  ee100a10          VMOV     r0,s0                 ;322
00019e  f7fffffe          BL       __aeabi_f2d
0001a2  ec410b1a          VMOV     d10,r0,r1             ;322
0001a6  ee180a90          VMOV     r0,s17                ;322
0001aa  f7fffffe          BL       __aeabi_f2d
0001ae  ec410b10          VMOV     d0,r0,r1              ;322
0001b2  f7fffffe          BL       __hardfp_sin
0001b6  eeb08a40          VMOV.F32 s16,s0                ;322
0001ba  eef08a60          VMOV.F32 s17,s1                ;322
0001be  6ea0              LDR      r0,[r4,#0x68]         ;322
0001c0  f7fffffe          BL       __aeabi_f2d
0001c4  ec532b18          VMOV     r2,r3,d8              ;322
0001c8  f7fffffe          BL       __aeabi_dmul
0001cc  ec532b1a          VMOV     r2,r3,d10             ;322
0001d0  f7fffffe          BL       __aeabi_dsub
0001d4  f7fffffe          BL       __aeabi_d2f
0001d8  ee080a10          VMOV     s16,r0                ;322
0001dc  ed950a12          VLDR     s0,[r5,#0x48]         ;325
0001e0  ee200a09          VMUL.F32 s0,s0,s18             ;325
0001e4  ed860a07          VSTR     s0,[r6,#0x1c]         ;325
0001e8  e00a              B        |L5.512|
                  |L5.490|
0001ea  7830              LDRB     r0,[r6,#0]            ;334  ; camerparameter
0001ec  2801              CMP      r0,#1                 ;334
0001ee  d035              BEQ      |L5.604|
0001f0  2802              CMP      r0,#2                 ;334
0001f2  d05f              BEQ      |L5.692|
0001f4  2803              CMP      r0,#3                 ;334
0001f6  d07d              BEQ      |L5.756|
0001f8  2804              CMP      r0,#4                 ;334
0001fa  d07c              BEQ      |L5.758|
0001fc  eeb08a69          VMOV.F32 s16,s19               ;361
                  |L5.512|
000200  ed940a27          VLDR     s0,[r4,#0x9c]         ;367
000204  ee200a08          VMUL.F32 s0,s0,s16             ;367
000208  ed850a10          VSTR     s0,[r5,#0x40]         ;367
00020c  ee100a10          VMOV     r0,s0                 ;368
000210  4970              LDR      r1,|L5.980|
000212  4288              CMP      r0,r1                 ;368
000214  db03              BLT      |L5.542|
000216  eeb30a0e          VMOV.F32 s0,#30.00000000       ;368
00021a  ed850a10          VSTR     s0,[r5,#0x40]         ;368
                  |L5.542|
00021e  496e              LDR      r1,|L5.984|
000220  6c28              LDR      r0,[r5,#0x40]         ;369  ; OutPutData
000222  4288              CMP      r0,r1                 ;369
000224  d303              BCC      |L5.558|
000226  eebb0a0e          VMOV.F32 s0,#-30.00000000      ;369
00022a  ed850a10          VSTR     s0,[r5,#0x40]         ;369
                  |L5.558|
00022e  ed970a00          VLDR     s0,[r7,#0]            ;370
000232  edd50a10          VLDR     s1,[r5,#0x40]         ;370
000236  eeb81ac0          VCVT.F32.S32 s2,s0                 ;370
00023a  ee311a20          VADD.F32 s2,s2,s1              ;370
00023e  eebd1ac1          VCVT.S32.F32 s2,s2                 ;370
000242  ee110a10          VMOV     r0,s2                 ;370
000246  81b8              STRH     r0,[r7,#0xc]          ;370
000248  eeb80ac0          VCVT.F32.S32 s0,s0                 ;371
00024c  ee300a60          VSUB.F32 s0,s0,s1              ;371
000250  eebd0ac0          VCVT.S32.F32 s0,s0                 ;371
000254  ee100a10          VMOV     r0,s0                 ;371
000258  81f8              STRH     r0,[r7,#0xe]          ;371
00025a  e703              B        |L5.100|
                  |L5.604|
00025c  ee180a90          VMOV     r0,s17                ;337
000260  f7fffffe          BL       __aeabi_f2d
000264  ec410b10          VMOV     d0,r0,r1              ;337
000268  f7fffffe          BL       __hardfp_sin
00026c  eeb08a40          VMOV.F32 s16,s0                ;337
000270  eef08a60          VMOV.F32 s17,s1                ;337
000274  6ea0              LDR      r0,[r4,#0x68]         ;337
000276  f7fffffe          BL       __aeabi_f2d
00027a  ec532b18          VMOV     r2,r3,d8              ;337
00027e  f7fffffe          BL       __aeabi_dmul
000282  ed950a12          VLDR     s0,[r5,#0x48]         ;337
000286  ec410b18          VMOV     d8,r0,r1              ;337
00028a  ee200a09          VMUL.F32 s0,s0,s18             ;337
00028e  ee100a10          VMOV     r0,s0                 ;337
000292  f7fffffe          BL       __aeabi_f2d
000296  ec532b18          VMOV     r2,r3,d8              ;337
00029a  f7fffffe          BL       __aeabi_dadd
00029e  f7fffffe          BL       __aeabi_d2f
0002a2  ee080a10          VMOV     s16,r0                ;337
0002a6  ed950a11          VLDR     s0,[r5,#0x44]         ;340
0002aa  ee200a09          VMUL.F32 s0,s0,s18             ;340
0002ae  ed860a07          VSTR     s0,[r6,#0x1c]         ;340
0002b2  e7a5              B        |L5.512|
                  |L5.692|
0002b4  ed950a11          VLDR     s0,[r5,#0x44]         ;343
0002b8  ee200a09          VMUL.F32 s0,s0,s18             ;343
0002bc  ee100a10          VMOV     r0,s0                 ;343
0002c0  f7fffffe          BL       __aeabi_f2d
0002c4  ec410b1a          VMOV     d10,r0,r1             ;343
0002c8  ee180a90          VMOV     r0,s17                ;343
0002cc  f7fffffe          BL       __aeabi_f2d
0002d0  ec410b10          VMOV     d0,r0,r1              ;343
0002d4  f7fffffe          BL       __hardfp_sin
0002d8  eeb08a40          VMOV.F32 s16,s0                ;343
0002dc  eef08a60          VMOV.F32 s17,s1                ;343
0002e0  6ea0              LDR      r0,[r4,#0x68]         ;343
0002e2  f7fffffe          BL       __aeabi_f2d
0002e6  ec532b18          VMOV     r2,r3,d8              ;343
0002ea  f7fffffe          BL       __aeabi_dmul
0002ee  ec532b1a          VMOV     r2,r3,d10             ;343
0002f2  e001              B        |L5.760|
                  |L5.756|
0002f4  e00d              B        |L5.786|
                  |L5.758|
0002f6  e038              B        |L5.874|
                  |L5.760|
0002f8  f7fffffe          BL       __aeabi_dsub
0002fc  f7fffffe          BL       __aeabi_d2f
000300  ee080a10          VMOV     s16,r0                ;343
000304  ed950a12          VLDR     s0,[r5,#0x48]         ;345
000308  ee200a09          VMUL.F32 s0,s0,s18             ;345
00030c  ed860a07          VSTR     s0,[r6,#0x1c]         ;345
000310  e776              B        |L5.512|
                  |L5.786|
000312  ed950a12          VLDR     s0,[r5,#0x48]         ;349
000316  ee200a09          VMUL.F32 s0,s0,s18             ;349
00031a  ee100a10          VMOV     r0,s0                 ;349
00031e  f7fffffe          BL       __aeabi_f2d
000322  ec410b1a          VMOV     d10,r0,r1             ;349
000326  ee180a90          VMOV     r0,s17                ;349
00032a  f7fffffe          BL       __aeabi_f2d
00032e  ec410b10          VMOV     d0,r0,r1              ;349
000332  f7fffffe          BL       __hardfp_sin
000336  eeb08a40          VMOV.F32 s16,s0                ;349
00033a  eef08a60          VMOV.F32 s17,s1                ;349
00033e  6ea0              LDR      r0,[r4,#0x68]         ;349
000340  f7fffffe          BL       __aeabi_f2d
000344  ec532b18          VMOV     r2,r3,d8              ;349
000348  f7fffffe          BL       __aeabi_dmul
00034c  ec532b1a          VMOV     r2,r3,d10             ;349
000350  f7fffffe          BL       __aeabi_dsub
000354  f7fffffe          BL       __aeabi_d2f
000358  ee080a10          VMOV     s16,r0                ;349
00035c  ed950a11          VLDR     s0,[r5,#0x44]         ;351
000360  ee200a09          VMUL.F32 s0,s0,s18             ;351
000364  ed860a07          VSTR     s0,[r6,#0x1c]         ;351
000368  e74a              B        |L5.512|
                  |L5.874|
00036a  ee180a90          VMOV     r0,s17                ;355
00036e  f7fffffe          BL       __aeabi_f2d
000372  ec410b10          VMOV     d0,r0,r1              ;355
000376  f7fffffe          BL       __hardfp_sin
00037a  eeb08a40          VMOV.F32 s16,s0                ;355
00037e  eef08a60          VMOV.F32 s17,s1                ;355
000382  6ea0              LDR      r0,[r4,#0x68]         ;355
000384  f7fffffe          BL       __aeabi_f2d
000388  ec532b18          VMOV     r2,r3,d8              ;355
00038c  f7fffffe          BL       __aeabi_dmul
000390  ed950a11          VLDR     s0,[r5,#0x44]         ;355
000394  ec410b18          VMOV     d8,r0,r1              ;355
000398  ee200a09          VMUL.F32 s0,s0,s18             ;355
00039c  ee100a10          VMOV     r0,s0                 ;355
0003a0  f7fffffe          BL       __aeabi_f2d
0003a4  ec532b18          VMOV     r2,r3,d8              ;355
0003a8  f7fffffe          BL       __aeabi_dadd
0003ac  f7fffffe          BL       __aeabi_d2f
0003b0  ee080a10          VMOV     s16,r0                ;355
0003b4  ed950a12          VLDR     s0,[r5,#0x48]         ;358
0003b8  ee200a09          VMUL.F32 s0,s0,s18             ;358
0003bc  ed860a07          VSTR     s0,[r6,#0x1c]         ;358
0003c0  e71e              B        |L5.512|
;;;373
                          ENDP

0003c2  0000              DCW      0x0000
                  |L5.964|
0003c4  00000000          DCFS     0x00000000 ; 0
                  |L5.968|
                          DCD      ||.data||+0x4c
                  |L5.972|
                          DCD      ||.bss||+0x10
                  |L5.976|
0003d0  447a0000          DCFS     0x447a0000 ; 1000
                  |L5.980|
                          DCD      0x41f00000
                  |L5.984|
                          DCD      0xc1f00000

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

                  QecodeSiteChange PROC
;;;437
;;;438    void QecodeSiteChange(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  b570              PUSH     {r4-r6,lr}
;;;439    {
000002  460c              MOV      r4,r1
;;;440    	ControlOutputValue->SetMoveSpeed = 20;
000004  2014              MOVS     r0,#0x14
000006  6010              STR      r0,[r2,#0]
;;;441
;;;442    	Uart_Printf(COM1,"camerparameter.DistanceCalibrationValue=%.1f\r\n",camerparameter.DistanceCalibrationValue);
000008  4d15              LDR      r5,|L6.96|
00000a  69e8              LDR      r0,[r5,#0x1c]  ; camerparameter
00000c  f7fffffe          BL       __aeabi_f2d
000010  ec410b10          VMOV     d0,r0,r1
000014  a113              ADR      r1,|L6.100|
000016  ec532b10          VMOV     r2,r3,d0
00001a  2001              MOVS     r0,#1
00001c  f7fffffe          BL       Uart_Printf
;;;443    	if(fabs(camerparameter.DistanceCalibrationValue) <= 10.0f)
000020  69e8              LDR      r0,[r5,#0x1c]  ; camerparameter
000022  f7fffffe          BL       __aeabi_f2d
000026  ec410b10          VMOV     d0,r0,r1
00002a  f7fffffe          BL       __hardfp_fabs
00002e  ed9f1b19          VLDR     d1,|L6.148|
000032  ec510b10          VMOV     r0,r1,d0
000036  ec532b11          VMOV     r2,r3,d1
00003a  f7fffffe          BL       __aeabi_cdcmple
00003e  d80d              BHI      |L6.92|
;;;444    	{
;;;445    		//
;;;446    		Uart_Printf(COM1,"到达二维码码心");
000040  a116              ADR      r1,|L6.156|
000042  2001              MOVS     r0,#1
000044  f7fffffe          BL       Uart_Printf
;;;447
;;;448    		//强制更新站点
;;;449    		agvSwitchUpdataSiteID();
000048  f7fffffe          BL       agvSwitchUpdataSiteID
;;;450    //		CalculationTurningSteering->BeforeTurningOffset = CalculationTurningSteering->TailOffset;
;;;451    //		CalculationTurningSteering->BeforeTurningAngleDifference = CalculationTurningSteering->AngleDifference;
;;;452    		LaserControlSignal.StraightHalfwayStatus = 0;
00004c  4817              LDR      r0,|L6.172|
00004e  2500              MOVS     r5,#0
000050  70c5              STRB     r5,[r0,#3]
;;;453    		Printf_LineToLine(ControlInputValue);
000052  4620              MOV      r0,r4
000054  f7fffffe          BL       Printf_LineToLine
;;;454    		ControlInputValue->QrcodeLocationingStatus = 0;
000058  f88450b9          STRB     r5,[r4,#0xb9]
                  |L6.92|
;;;455    	}
;;;456    }
00005c  bd70              POP      {r4-r6,pc}
;;;457
                          ENDP

00005e  0000              DCW      0x0000
                  |L6.96|
                          DCD      ||.bss||+0x10
                  |L6.100|
000064  63616d65          DCB      "camerparameter.DistanceCalibrationValue=%.1f\r\n",0
000068  72706172
00006c  616d6574
000070  65722e44
000074  69737461
000078  6e636543
00007c  616c6962
000080  72617469
000084  6f6e5661
000088  6c75653d
00008c  252e3166
000090  0d0a00
000093  00                DCB      0
                  |L6.148|
000094  00000000          DCFD     0x4024000000000000 ; 10
000098  40240000
                  |L6.156|
00009c  b5bdb4ef          DCB      181,189,180,239,182,254,206,172,194,235,194,235,208,196,0
0000a0  b6feceac
0000a4  c2ebc2eb
0000a8  d0c400
0000ab  00                DCB      0
                  |L6.172|
                          DCD      LaserControlSignal

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

                  SendInitQRCommend PROC
;;;102
;;;103    void SendInitQRCommend()
000000  2202              MOVS     r2,#2
;;;104    {
;;;105    	WriteUart(COM6,selectpostag,2);
000002  4902              LDR      r1,|L7.12|
000004  2006              MOVS     r0,#6
000006  f7ffbffe          B.W      WriteUart
;;;106    }
;;;107
                          ENDP

00000a  0000              DCW      0x0000
                  |L7.12|
                          DCD      ||.data||+0x2

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

                  UpdataCamerParameter PROC
;;;26
;;;27     void UpdataCamerParameter()
000000  e92d41f0          PUSH     {r4-r8,lr}
;;;28     {
000004  ed2d8b02          VPUSH    {d8}
;;;29     	static int QRcodeSpacing = 0;//二维码间距
;;;30     	static int CurDis = 0,LastDis = 0,CurFlag = 0,LastFlag = 0;
;;;31
;;;32     	if(Tagdata.tagnumber != 0)
000008  4e38              LDR      r6,|L8.236|
;;;33     	{
;;;34     		CurDis = ControlInputValue.AgvActualFirstDis;
00000a  4f3a              LDR      r7,|L8.244|
00000c  f8df80e0          LDR      r8,|L8.240|
000010  68f0              LDR      r0,[r6,#0xc]  ; Tagdata
000012  697c              LDR      r4,[r7,#0x14]         ;32
;;;35     		camerparameter.UpdataFlag = 1;
000014  f1060510          ADD      r5,r6,#0x10
000018  2800              CMP      r0,#0                 ;32
00001a  d005              BEQ      |L8.40|
00001c  f8c84008          STR      r4,[r8,#8]            ;34  ; CurDis
000020  2101              MOVS     r1,#1
000022  7629              STRB     r1,[r5,#0x18]
;;;36     		camerparameter.id = Tagdata.tagnumber;
000024  8068              STRH     r0,[r5,#2]
000026  e001              B        |L8.44|
                  |L8.40|
;;;37     	}
;;;38     	else camerparameter.UpdataFlag = 0;
000028  2000              MOVS     r0,#0
00002a  7628              STRB     r0,[r5,#0x18]
                  |L8.44|
;;;39
;;;40     	camerparameter.X_pixel = Tagdata.x/10000.0;          //毫米转换成米
00002c  6830              LDR      r0,[r6,#0]  ; Tagdata
00002e  f7fffffe          BL       __aeabi_i2d
000032  ed9f8b31          VLDR     d8,|L8.248|
000036  ec532b18          VMOV     r2,r3,d8
00003a  f7fffffe          BL       __aeabi_ddiv
00003e  f7fffffe          BL       __aeabi_d2f
000042  6068              STR      r0,[r5,#4]  ; camerparameter
;;;41     	camerparameter.Y_pixel = Tagdata.y/10000.0;
000044  6870              LDR      r0,[r6,#4]  ; Tagdata
000046  f7fffffe          BL       __aeabi_i2d
00004a  ec532b18          VMOV     r2,r3,d8
00004e  f7fffffe          BL       __aeabi_ddiv
000052  f7fffffe          BL       __aeabi_d2f
000056  60a8              STR      r0,[r5,#8]  ; camerparameter
;;;42
;;;43     	if(Tagdata.angle) camerparameter.angle = 360.0 - Tagdata.angle/10.0;    //转换为一般坐标系对应的角度
000058  68b0              LDR      r0,[r6,#8]  ; Tagdata
00005a  b188              CBZ      r0,|L8.128|
00005c  f7fffffe          BL       __aeabi_ui2d
000060  ed9f1b27          VLDR     d1,|L8.256|
000064  ec532b11          VMOV     r2,r3,d1
000068  f7fffffe          BL       __aeabi_ddiv
00006c  ed9f1b26          VLDR     d1,|L8.264|
000070  ec532b11          VMOV     r2,r3,d1
000074  f7fffffe          BL       __aeabi_drsub
000078  f7fffffe          BL       __aeabi_d2f
00007c  60e8              STR      r0,[r5,#0xc]  ; camerparameter
00007e  e003              B        |L8.136|
                  |L8.128|
;;;44     	else camerparameter.angle = 0.0;
000080  ed9f0a23          VLDR     s0,|L8.272|
000084  ed850a03          VSTR     s0,[r5,#0xc]
                  |L8.136|
;;;45
;;;46     	if(camerparameter.angle >= 180.0)
000088  4922              LDR      r1,|L8.276|
00008a  68e8              LDR      r0,[r5,#0xc]  ; camerparameter
;;;47     	{
;;;48     		camerparameter.angle = camerparameter.angle - 180.0;
00008c  ed9f8b22          VLDR     d8,|L8.280|
000090  4288              CMP      r0,r1                 ;46
000092  db09              BLT      |L8.168|
000094  f7fffffe          BL       __aeabi_f2d
000098  ec532b18          VMOV     r2,r3,d8
00009c  f7fffffe          BL       __aeabi_dsub
0000a0  f7fffffe          BL       __aeabi_d2f
0000a4  60e8              STR      r0,[r5,#0xc]  ; camerparameter
0000a6  e008              B        |L8.186|
                  |L8.168|
;;;49     	}
;;;50     	else
;;;51     	{
;;;52     		camerparameter.angle = camerparameter.angle + 180.0;
0000a8  f7fffffe          BL       __aeabi_f2d
0000ac  ec532b18          VMOV     r2,r3,d8
0000b0  f7fffffe          BL       __aeabi_dadd
0000b4  f7fffffe          BL       __aeabi_d2f
0000b8  60e8              STR      r0,[r5,#0xc]  ; camerparameter
                  |L8.186|
;;;53     	}
;;;54
;;;55     	LastDis = ControlInputValue.AgvActualFirstDis;
0000ba  f8c8400c          STR      r4,[r8,#0xc]  ; LastDis
;;;56
;;;57     	ControlInputValue.QRcodeSpacing = abs(LastDis - CurDis);
0000be  f8d80008          LDR      r0,[r8,#8]  ; CurDis
0000c2  1a20              SUBS     r0,r4,r0
0000c4  d500              BPL      |L8.200|
0000c6  4240              RSBS     r0,r0,#0
                  |L8.200|
0000c8  67b8              STR      r0,[r7,#0x78]  ; ControlInputValue
;;;58
;;;59     	if(camerparameter.UpdataFlag)//二维码导航
0000ca  7e29              LDRB     r1,[r5,#0x18]  ; camerparameter
0000cc  b111              CBZ      r1,|L8.212|
;;;60     	{
;;;61     		ControlInputValue.NavcitionMode = 3;
0000ce  2103              MOVS     r1,#3
0000d0  f887103d          STRB     r1,[r7,#0x3d]
                  |L8.212|
;;;62     	}
;;;63     	if(ControlInputValue.QRcodeSpacing >= 1500)//激光导航
0000d4  f24051dc          MOV      r1,#0x5dc
0000d8  4288              CMP      r0,r1
0000da  db02              BLT      |L8.226|
;;;64     	{
;;;65     		ControlInputValue.NavcitionMode = 2;
0000dc  2002              MOVS     r0,#2
0000de  f887003d          STRB     r0,[r7,#0x3d]
                  |L8.226|
;;;66     	}
;;;67
;;;68     //	Uart_Printf(COM1,"X=%.1f	y=%.1f	w=%f\r\n",camerparameter.X_pixel*1000.0f,camerparameter.Y_pixel*1000.0f,camerparameter.angle);
;;;69     }
0000e2  ecbd8b02          VPOP     {d8}
0000e6  e8bd81f0          POP      {r4-r8,pc}
;;;70
                          ENDP

0000ea  0000              DCW      0x0000
                  |L8.236|
                          DCD      ||.bss||
                  |L8.240|
                          DCD      ||.data||
                  |L8.244|
                          DCD      ControlInputValue
                  |L8.248|
0000f8  00000000          DCFD     0x40c3880000000000 ; 10000
0000fc  40c38800
                  |L8.256|
000100  00000000          DCFD     0x4024000000000000 ; 10
000104  40240000
                  |L8.264|
000108  00000000          DCFD     0x4076800000000000 ; 360
00010c  40768000
                  |L8.272|
000110  00000000          DCFS     0x00000000 ; 0
                  |L8.276|
                          DCD      0x43340000
                  |L8.280|
000118  00000000          DCFD     0x4066800000000000 ; 180
00011c  40668000

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

                  agvIntegratedLaserAndIntegralData PROC
;;;121
;;;122    void agvIntegratedLaserAndIntegralData(float *XBias,float *YBias,float *Theta,agvControlInputValue *ControlInputValue)
000000  e92d4ff0          PUSH     {r4-r11,lr}
;;;123    {
000004  4605              MOV      r5,r0
000006  ed2d8b0a          VPUSH    {d8-d12}
00000a  b0a1              SUB      sp,sp,#0x84
00000c  460e              MOV      r6,r1
00000e  4690              MOV      r8,r2
000010  461c              MOV      r4,r3
;;;124    	static float CenterPx = 0,CenterPy = 0,QRx = 0,QRy = 0,IntegralX = 0,IntegralY = 0,AngleDifference = 0,RectificationX = 0,RectificationY = 150;
;;;125
;;;126    	if(camerparameter.UpdataFlag == 1)//扫到二维码
000012  f8dfb3e8          LDR      r11,|L9.1020|
000016  f89b0018          LDRB     r0,[r11,#0x18]  ; camerparameter
;;;127    	{
;;;128    		*Theta = camerparameter.angle/180.0f * PI;
;;;129    		/**************AGV相对于二维码的方向****************/
;;;130    		if((camerparameter.angle <= 25.0f) || (camerparameter.angle >= 335.0f))
;;;131    		{
;;;132    			camerparameter.TwoDCdirection = 1;
;;;133    			camerparameter.BaseAngle = 0.0;
;;;134    		}
;;;135    		else if((camerparameter.angle <= 115.0f) && (camerparameter.angle >= 65.0f))
;;;136    		{
;;;137    			camerparameter.TwoDCdirection = 2;
;;;138    			camerparameter.BaseAngle = PI/2.0f;
00001a  eddf1af9          VLDR     s3,|L9.1024|
;;;139    		}
;;;140    		else if((camerparameter.angle <= 205.0f) && (camerparameter.angle >= 155.0f))
;;;141    		{
;;;142    			camerparameter.TwoDCdirection = 3;
;;;143    			camerparameter.BaseAngle = PI;
;;;144    		}
;;;145    		else if((camerparameter.angle <= 295.0f) && (camerparameter.angle >= 245.0f))
;;;146    		{
;;;147    			camerparameter.TwoDCdirection = 4;
;;;148    			camerparameter.BaseAngle = 3.0f*PI/2.0f;
00001e  ed9f2af9          VLDR     s4,|L9.1028|
000022  ed9faaf9          VLDR     s20,|L9.1032|
000026  eddf8af9          VLDR     s17,|L9.1036|
;;;149    		}
;;;150    		else
;;;151    		{
;;;152    			camerparameter.TwoDCdirection = 0;
;;;153    		}
;;;154
;;;155    		*XBias = (float)camerparameter.X_pixel;
;;;156    		*YBias = (float)camerparameter.Y_pixel;
;;;157
;;;158    		AngleDifference = CalculatingCurrentAndTargetAngle(*Theta,camerparameter.BaseAngle);
00002a  4ff9              LDR      r7,|L9.1040|
;;;159
;;;160    		if(ControlInputValue->Direction == 1)//前进
;;;161    		{
;;;162    			//转换到积分点
;;;163    			CenterPx = *XBias + sin(AngleDifference)*QR_X_OFFSET;
;;;164    			CenterPy = *YBias + cos(AngleDifference)*QR_X_OFFSET;
;;;165
;;;166    			IntegralX = CenterPx - cos(AngleDifference) * (ControlInputValue->Wheelbase / 2.0f);
;;;167    			IntegralY = CenterPy - sin(AngleDifference) * (ControlInputValue->Wheelbase / 2.0f);
;;;168
;;;169    			RectificationX = IntegralX + cos(AngleDifference) * ControlInputValue->Wheelbase;
;;;170    			RectificationY = IntegralY + sin(AngleDifference) * ControlInputValue->Wheelbase;
;;;171    		}
;;;172    		else if(ControlInputValue->Direction == 2)	//后退
;;;173    		{
;;;174    			//转换到积分点
;;;175    			CenterPx = *XBias + sin(AngleDifference)*QR_X_OFFSET;
;;;176    			CenterPy = *YBias + cos(AngleDifference)*QR_X_OFFSET;
;;;177
;;;178    			IntegralX = CenterPx + cos(AngleDifference) * (ControlInputValue->Wheelbase / 2.0f);
;;;179    			IntegralY = CenterPy + sin(AngleDifference) * (ControlInputValue->Wheelbase / 2.0f);
;;;180
;;;181    			RectificationX = IntegralX - cos(AngleDifference) * ControlInputValue->Wheelbase;
;;;182    			RectificationY = IntegralY - sin(AngleDifference) * ControlInputValue->Wheelbase;
;;;183    		}
;;;184    		else if(ControlInputValue->Direction == 3)//平移前进
;;;185    		{
;;;186    			IntegralX = *XBias + sin(AngleDifference) * PINGYI_WHEEL_BASE_BACK;
;;;187    			IntegralY = *YBias - cos(AngleDifference) * PINGYI_WHEEL_BASE_BACK;
;;;188
;;;189    			RectificationX = *XBias - sin(AngleDifference) * PINGYI_WHEEL_BASE_FRONT;
;;;190    			RectificationY = *YBias + cos(AngleDifference) * PINGYI_WHEEL_BASE_FRONT;
;;;191    		}
;;;192    		else if(ControlInputValue->Direction == 4)//平移后退
;;;193    		{
;;;194    			IntegralX = *XBias - sin(AngleDifference) * PINGYI_WHEEL_BASE_FRONT;
;;;195    			IntegralY = *YBias + cos(AngleDifference) * PINGYI_WHEEL_BASE_FRONT;
;;;196
;;;197    			RectificationX = *XBias + sin(AngleDifference) * PINGYI_WHEEL_BASE_BACK;
;;;198    			RectificationY = *YBias - cos(AngleDifference) * PINGYI_WHEEL_BASE_BACK;
;;;199    		}
;;;200
;;;201    		Uart_Printf(COM1,"二维码数据=%.1f	y=%.1f	w=%f	夹角=%f	车头x=%.1f	y=%.1f	积分点x=%.1f	y=%.1f	offset=%.1f	D%d\r\n",
00002c  ed9f8af9          VLDR     s16,|L9.1044|
000030  2801              CMP      r0,#1                 ;126
000032  d00f              BEQ      |L9.84|
;;;202    		*XBias*1000.0f,*YBias*1000.0f,camerparameter.angle,AngleDifference*180.0f/PI,
;;;203    		RectificationX*1000,RectificationY*1000,IntegralX*1000,IntegralY*1000,ControlOutputValue.AgvOffset,ControlInputValue->Direction);
;;;204
;;;205    		TIM_SetCounter(TIM3,0);//清空计时器
;;;206    	}
;;;207    	else
;;;208    	{
;;;209    		static float RequstAngle = 0,RequstSpeed = 0,Angle = 0;
;;;210
;;;211    		if(ControlInputValue->Direction == 1)//前进
000034  f89400ba          LDRB     r0,[r4,#0xba]
000038  2801              CMP      r0,#1
00003a  d07c              BEQ      |L9.310|
;;;212    		{
;;;213    			RequstAngle = ControlInputValue->AgvFirstRudderWheelAngle;
;;;214    			RequstSpeed = (float)ControlInputValue->AgvActualFirstSpeed / 1000.0f;
;;;215
;;;216    			agvEncoderIntegrate(ControlInputValue->Wheelbase,&IntegralX,&IntegralY,&AngleDifference,RequstSpeed,RequstAngle);
;;;217
;;;218    			RectificationX = IntegralX + cos(AngleDifference) * ControlInputValue->Wheelbase;
;;;219    			RectificationY = IntegralY + sin(AngleDifference) * ControlInputValue->Wheelbase;
;;;220    		}
;;;221    		else if(ControlInputValue->Direction == 2)	//后退
00003c  2802              CMP      r0,#2
00003e  d07b              BEQ      |L9.312|
;;;222    		{
;;;223    			RequstAngle = ControlInputValue->AgvSecondRudderWheelAngle;
;;;224    			RequstSpeed = (float)ControlInputValue->AgvActualSecondSpeed / 1000.0f;
;;;225
;;;226    			agvEncoderIntegrate(ControlInputValue->Wheelbase,&IntegralX,&IntegralY,&AngleDifference,RequstSpeed,RequstAngle);
;;;227
;;;228    			RectificationX = IntegralX - cos(AngleDifference) * ControlInputValue->Wheelbase;
;;;229    			RectificationY = IntegralY - sin(AngleDifference) * ControlInputValue->Wheelbase;
;;;230    		}
;;;231    		else if(ControlInputValue->Direction == 3)//平移前进
;;;232    		{
;;;233    			RequstAngle = ControlInputValue->AgvThirdRudderWheelAngle;
;;;234    			RequstSpeed = (float)ControlInputValue->AgvActualThirdSpeed / 1000.0f;
;;;235
;;;236    			Angle = AngleDifference + PI / 2.0f;
;;;237    			agvEncoderIntegrate(PINGYI_WHEEL_BASE_FRONT,&IntegralX,&IntegralY,&Angle,RequstSpeed,RequstAngle);
;;;238
;;;239    			RectificationX = IntegralX - sin(AngleDifference) * PINGYI_WHEEL_BASE;
000040  ed9fbbf5          VLDR     d11,|L9.1048|
000044  4639              MOV      r1,r7                 ;158
000046  2803              CMP      r0,#3                 ;231
000048  ed910a09          VLDR     s0,[r1,#0x24]         ;158
00004c  d075              BEQ      |L9.314|
;;;240    			RectificationY = IntegralY + cos(AngleDifference) * PINGYI_WHEEL_BASE;
;;;241    		}
;;;242    		else if(ControlInputValue->Direction == 4)//平移后退
00004e  2804              CMP      r0,#4
000050  d074              BEQ      |L9.316|
000052  e399              B        |L9.1928|
                  |L9.84|
000054  eddb0a03          VLDR     s1,[r11,#0xc]         ;128
000058  ee800a8a          VDIV.F32 s0,s1,s20             ;128
00005c  ee200a28          VMUL.F32 s0,s0,s17             ;128
000060  ed880a00          VSTR     s0,[r8,#0]            ;128
000064  49ee              LDR      r1,|L9.1056|
000066  f8db000c          LDR      r0,[r11,#0xc]         ;130  ; camerparameter
00006a  4288              CMP      r0,r1                 ;130
00006c  dd02              BLE      |L9.116|
00006e  49ed              LDR      r1,|L9.1060|
000070  4288              CMP      r0,r1                 ;130
000072  db07              BLT      |L9.132|
                  |L9.116|
000074  2001              MOVS     r0,#1                 ;132
000076  f88b0000          STRB     r0,[r11,#0]           ;132
00007a  ed9f0aeb          VLDR     s0,|L9.1064|
00007e  ed8b0a04          VSTR     s0,[r11,#0x10]        ;133
000082  e023              B        |L9.204|
                  |L9.132|
000084  49e9              LDR      r1,|L9.1068|
000086  4401              ADD      r1,r1,r0              ;135
000088  f5b10fc8          CMP      r1,#0x640000          ;135
00008c  d805              BHI      |L9.154|
00008e  2002              MOVS     r0,#2                 ;137
000090  f88b0000          STRB     r0,[r11,#0]           ;137
000094  edcb1a04          VSTR     s3,[r11,#0x10]        ;138
000098  e018              B        |L9.204|
                  |L9.154|
00009a  49e5              LDR      r1,|L9.1072|
00009c  4401              ADD      r1,r1,r0              ;140
00009e  f5b11f48          CMP      r1,#0x320000          ;140
0000a2  d805              BHI      |L9.176|
0000a4  2003              MOVS     r0,#3                 ;142
0000a6  f88b0000          STRB     r0,[r11,#0]           ;142
0000aa  edcb8a04          VSTR     s17,[r11,#0x10]       ;143
0000ae  e00d              B        |L9.204|
                  |L9.176|
0000b0  49e0              LDR      r1,|L9.1076|
0000b2  4408              ADD      r0,r0,r1              ;145
0000b4  f5b01ff4          CMP      r0,#0x1e8000          ;145
0000b8  d805              BHI      |L9.198|
0000ba  2004              MOVS     r0,#4                 ;147
0000bc  f88b0000          STRB     r0,[r11,#0]           ;147
0000c0  ed8b2a04          VSTR     s4,[r11,#0x10]        ;148
0000c4  e002              B        |L9.204|
                  |L9.198|
0000c6  2000              MOVS     r0,#0                 ;152
0000c8  f88b0000          STRB     r0,[r11,#0]           ;152
                  |L9.204|
0000cc  ed9b0a01          VLDR     s0,[r11,#4]           ;155
0000d0  ed850a00          VSTR     s0,[r5,#0]            ;155
0000d4  ed9b0a02          VLDR     s0,[r11,#8]           ;156
0000d8  ed860a00          VSTR     s0,[r6,#0]            ;156
0000dc  eddb0a04          VLDR     s1,[r11,#0x10]        ;158
0000e0  ed980a00          VLDR     s0,[r8,#0]            ;158
0000e4  f7fffffe          BL       CalculatingCurrentAndTargetAngle
0000e8  ed870a09          VSTR     s0,[r7,#0x24]         ;158
0000ec  f89400ba          LDRB     r0,[r4,#0xba]         ;160
0000f0  ed9f9bd1          VLDR     d9,|L9.1080|
0000f4  eef6aa00          VMOV.F32 s21,#0.50000000       ;166
0000f8  2801              CMP      r0,#1                 ;160
0000fa  d00a              BEQ      |L9.274|
0000fc  2802              CMP      r0,#2                 ;172
0000fe  d07d              BEQ      |L9.508|
000100  ed9f9bcf          VLDR     d9,|L9.1088|
000104  ed9fbbd0          VLDR     d11,|L9.1096|
000108  2803              CMP      r0,#3                 ;184
00010a  d078              BEQ      |L9.510|
00010c  2804              CMP      r0,#4                 ;192
00010e  d077              BEQ      |L9.512|
000110  e269              B        |L9.1510|
                  |L9.274|
000112  ee100a10          VMOV     r0,s0                 ;163
000116  f7fffffe          BL       __aeabi_f2d
00011a  ec410b10          VMOV     d0,r0,r1              ;163
00011e  f7fffffe          BL       __hardfp_sin
000122  ec532b19          VMOV     r2,r3,d9              ;163
000126  ec510b10          VMOV     r0,r1,d0              ;163
00012a  f7fffffe          BL       __aeabi_dmul
00012e  ec410b1b          VMOV     d11,r0,r1             ;163
000132  6828              LDR      r0,[r5,#0]            ;163
000134  e003              B        |L9.318|
                  |L9.310|
000136  e2d6              B        |L9.1766|
                  |L9.312|
000138  e327              B        |L9.1930|
                  |L9.314|
00013a  e378              B        |L9.2094|
                  |L9.316|
00013c  e3c5              B        |L9.2250|
                  |L9.318|
00013e  f7fffffe          BL       __aeabi_f2d
000142  ec532b1b          VMOV     r2,r3,d11             ;163
000146  f7fffffe          BL       __aeabi_dadd
00014a  f7fffffe          BL       __aeabi_d2f
00014e  6178              STR      r0,[r7,#0x14]         ;163  ; CenterPx
000150  6a78              LDR      r0,[r7,#0x24]         ;164  ; AngleDifference
000152  f7fffffe          BL       __aeabi_f2d
000156  ec410b10          VMOV     d0,r0,r1              ;164
00015a  f7fffffe          BL       __hardfp_cos
00015e  ec532b19          VMOV     r2,r3,d9              ;164
000162  ec510b10          VMOV     r0,r1,d0              ;164
000166  f7fffffe          BL       __aeabi_dmul
00016a  ec410b19          VMOV     d9,r0,r1              ;164
00016e  6830              LDR      r0,[r6,#0]            ;164
000170  f7fffffe          BL       __aeabi_f2d
000174  ec532b19          VMOV     r2,r3,d9              ;164
000178  f7fffffe          BL       __aeabi_dadd
00017c  f7fffffe          BL       __aeabi_d2f
000180  61b8              STR      r0,[r7,#0x18]         ;164  ; CenterPy
000182  ed940a10          VLDR     s0,[r4,#0x40]         ;166
000186  ee200a2a          VMUL.F32 s0,s0,s21             ;166
00018a  ee100a10          VMOV     r0,s0                 ;166
00018e  f7fffffe          BL       __aeabi_f2d
000192  ec410b19          VMOV     d9,r0,r1              ;166
000196  6a78              LDR      r0,[r7,#0x24]         ;166  ; AngleDifference
000198  f7fffffe          BL       __aeabi_f2d
00019c  ec410b10          VMOV     d0,r0,r1              ;166
0001a0  f7fffffe          BL       __hardfp_cos
0001a4  ec532b19          VMOV     r2,r3,d9              ;166
0001a8  ec510b10          VMOV     r0,r1,d0              ;166
0001ac  f7fffffe          BL       __aeabi_dmul
0001b0  ec410b19          VMOV     d9,r0,r1              ;166
0001b4  6978              LDR      r0,[r7,#0x14]         ;166  ; CenterPx
0001b6  f7fffffe          BL       __aeabi_f2d
0001ba  ec532b19          VMOV     r2,r3,d9              ;166
0001be  f7fffffe          BL       __aeabi_dsub
0001c2  f7fffffe          BL       __aeabi_d2f
0001c6  61f8              STR      r0,[r7,#0x1c]         ;166  ; IntegralX
0001c8  ed940a10          VLDR     s0,[r4,#0x40]         ;167
0001cc  ee200a2a          VMUL.F32 s0,s0,s21             ;167
0001d0  ee100a10          VMOV     r0,s0                 ;167
0001d4  f7fffffe          BL       __aeabi_f2d
0001d8  ec410b19          VMOV     d9,r0,r1              ;167
0001dc  6a78              LDR      r0,[r7,#0x24]         ;167  ; AngleDifference
0001de  f7fffffe          BL       __aeabi_f2d
0001e2  ec410b10          VMOV     d0,r0,r1              ;167
0001e6  f7fffffe          BL       __hardfp_sin
0001ea  ec532b19          VMOV     r2,r3,d9              ;167
0001ee  ec510b10          VMOV     r0,r1,d0              ;167
0001f2  f7fffffe          BL       __aeabi_dmul
0001f6  ec410b19          VMOV     d9,r0,r1              ;167
0001fa  e002              B        |L9.514|
                  |L9.508|
0001fc  e048              B        |L9.656|
                  |L9.510|
0001fe  e127              B        |L9.1104|
                  |L9.512|
000200  e18c              B        |L9.1308|
                  |L9.514|
000202  69b8              LDR      r0,[r7,#0x18]         ;167  ; CenterPy
000204  f7fffffe          BL       __aeabi_f2d
000208  ec532b19          VMOV     r2,r3,d9              ;167
00020c  f7fffffe          BL       __aeabi_dsub
000210  f7fffffe          BL       __aeabi_d2f
000214  6238              STR      r0,[r7,#0x20]         ;167  ; IntegralY
000216  6c20              LDR      r0,[r4,#0x40]         ;169
000218  f7fffffe          BL       __aeabi_f2d
00021c  ec410b19          VMOV     d9,r0,r1              ;169
000220  6a78              LDR      r0,[r7,#0x24]         ;169  ; AngleDifference
000222  f7fffffe          BL       __aeabi_f2d
000226  ec410b10          VMOV     d0,r0,r1              ;169
00022a  f7fffffe          BL       __hardfp_cos
00022e  ec532b19          VMOV     r2,r3,d9              ;169
000232  ec510b10          VMOV     r0,r1,d0              ;169
000236  f7fffffe          BL       __aeabi_dmul
00023a  ec410b19          VMOV     d9,r0,r1              ;169
00023e  69f8              LDR      r0,[r7,#0x1c]         ;169  ; IntegralX
000240  f7fffffe          BL       __aeabi_f2d
000244  ec532b19          VMOV     r2,r3,d9              ;169
000248  f7fffffe          BL       __aeabi_dadd
00024c  f7fffffe          BL       __aeabi_d2f
000250  62b8              STR      r0,[r7,#0x28]         ;169  ; RectificationX
000252  6c20              LDR      r0,[r4,#0x40]         ;170
000254  f7fffffe          BL       __aeabi_f2d
000258  ec410b19          VMOV     d9,r0,r1              ;170
00025c  6a78              LDR      r0,[r7,#0x24]         ;170  ; AngleDifference
00025e  f7fffffe          BL       __aeabi_f2d
000262  ec410b10          VMOV     d0,r0,r1              ;170
000266  f7fffffe          BL       __hardfp_sin
00026a  ec532b19          VMOV     r2,r3,d9              ;170
00026e  ec510b10          VMOV     r0,r1,d0              ;170
000272  f7fffffe          BL       __aeabi_dmul
000276  ec410b19          VMOV     d9,r0,r1              ;170
00027a  6a38              LDR      r0,[r7,#0x20]         ;170  ; IntegralY
00027c  f7fffffe          BL       __aeabi_f2d
000280  ec532b19          VMOV     r2,r3,d9              ;170
000284  f7fffffe          BL       __aeabi_dadd
000288  f7fffffe          BL       __aeabi_d2f
00028c  62f8              STR      r0,[r7,#0x2c]         ;170  ; RectificationY
00028e  e1aa              B        |L9.1510|
                  |L9.656|
000290  ee100a10          VMOV     r0,s0                 ;175
000294  f7fffffe          BL       __aeabi_f2d
000298  ec410b10          VMOV     d0,r0,r1              ;175
00029c  f7fffffe          BL       __hardfp_sin
0002a0  ec532b19          VMOV     r2,r3,d9              ;175
0002a4  ec510b10          VMOV     r0,r1,d0              ;175
0002a8  f7fffffe          BL       __aeabi_dmul
0002ac  ec410b1b          VMOV     d11,r0,r1             ;175
0002b0  6828              LDR      r0,[r5,#0]            ;175
0002b2  f7fffffe          BL       __aeabi_f2d
0002b6  ec532b1b          VMOV     r2,r3,d11             ;175
0002ba  f7fffffe          BL       __aeabi_dadd
0002be  f7fffffe          BL       __aeabi_d2f
0002c2  6178              STR      r0,[r7,#0x14]         ;175  ; CenterPx
0002c4  6a78              LDR      r0,[r7,#0x24]         ;176  ; AngleDifference
0002c6  f7fffffe          BL       __aeabi_f2d
0002ca  ec410b10          VMOV     d0,r0,r1              ;176
0002ce  f7fffffe          BL       __hardfp_cos
0002d2  ec532b19          VMOV     r2,r3,d9              ;176
0002d6  ec510b10          VMOV     r0,r1,d0              ;176
0002da  f7fffffe          BL       __aeabi_dmul
0002de  ec410b19          VMOV     d9,r0,r1              ;176
0002e2  6830              LDR      r0,[r6,#0]            ;176
0002e4  f7fffffe          BL       __aeabi_f2d
0002e8  ec532b19          VMOV     r2,r3,d9              ;176
0002ec  f7fffffe          BL       __aeabi_dadd
0002f0  f7fffffe          BL       __aeabi_d2f
0002f4  61b8              STR      r0,[r7,#0x18]         ;176  ; CenterPy
0002f6  ed940a10          VLDR     s0,[r4,#0x40]         ;178
0002fa  ee200a2a          VMUL.F32 s0,s0,s21             ;178
0002fe  ee100a10          VMOV     r0,s0                 ;178
000302  f7fffffe          BL       __aeabi_f2d
000306  ec410b19          VMOV     d9,r0,r1              ;178
00030a  6a78              LDR      r0,[r7,#0x24]         ;178  ; AngleDifference
00030c  f7fffffe          BL       __aeabi_f2d
000310  ec410b10          VMOV     d0,r0,r1              ;178
000314  f7fffffe          BL       __hardfp_cos
000318  ec532b19          VMOV     r2,r3,d9              ;178
00031c  ec510b10          VMOV     r0,r1,d0              ;178
000320  f7fffffe          BL       __aeabi_dmul
000324  ec410b19          VMOV     d9,r0,r1              ;178
000328  6978              LDR      r0,[r7,#0x14]         ;178  ; CenterPx
00032a  f7fffffe          BL       __aeabi_f2d
00032e  ec532b19          VMOV     r2,r3,d9              ;178
000332  f7fffffe          BL       __aeabi_dadd
000336  f7fffffe          BL       __aeabi_d2f
00033a  61f8              STR      r0,[r7,#0x1c]         ;178  ; IntegralX
00033c  ed940a10          VLDR     s0,[r4,#0x40]         ;179
000340  ee200a2a          VMUL.F32 s0,s0,s21             ;179
000344  ee100a10          VMOV     r0,s0                 ;179
000348  f7fffffe          BL       __aeabi_f2d
00034c  ec410b19          VMOV     d9,r0,r1              ;179
000350  6a78              LDR      r0,[r7,#0x24]         ;179  ; AngleDifference
000352  f7fffffe          BL       __aeabi_f2d
000356  ec410b10          VMOV     d0,r0,r1              ;179
00035a  f7fffffe          BL       __hardfp_sin
00035e  ec532b19          VMOV     r2,r3,d9              ;179
000362  ec510b10          VMOV     r0,r1,d0              ;179
000366  f7fffffe          BL       __aeabi_dmul
00036a  ec410b19          VMOV     d9,r0,r1              ;179
00036e  69b8              LDR      r0,[r7,#0x18]         ;179  ; CenterPy
000370  f7fffffe          BL       __aeabi_f2d
000374  ec532b19          VMOV     r2,r3,d9              ;179
000378  f7fffffe          BL       __aeabi_dadd
00037c  f7fffffe          BL       __aeabi_d2f
000380  6238              STR      r0,[r7,#0x20]         ;179  ; IntegralY
000382  6c20              LDR      r0,[r4,#0x40]         ;181
000384  f7fffffe          BL       __aeabi_f2d
000388  ec410b19          VMOV     d9,r0,r1              ;181
00038c  6a78              LDR      r0,[r7,#0x24]         ;181  ; AngleDifference
00038e  f7fffffe          BL       __aeabi_f2d
000392  ec410b10          VMOV     d0,r0,r1              ;181
000396  f7fffffe          BL       __hardfp_cos
00039a  ec532b19          VMOV     r2,r3,d9              ;181
00039e  ec510b10          VMOV     r0,r1,d0              ;181
0003a2  f7fffffe          BL       __aeabi_dmul
0003a6  ec410b19          VMOV     d9,r0,r1              ;181
0003aa  69f8              LDR      r0,[r7,#0x1c]         ;181  ; IntegralX
0003ac  f7fffffe          BL       __aeabi_f2d
0003b0  ec532b19          VMOV     r2,r3,d9              ;181
0003b4  f7fffffe          BL       __aeabi_dsub
0003b8  f7fffffe          BL       __aeabi_d2f
0003bc  62b8              STR      r0,[r7,#0x28]         ;181  ; RectificationX
0003be  6c20              LDR      r0,[r4,#0x40]         ;182
0003c0  f7fffffe          BL       __aeabi_f2d
0003c4  ec410b19          VMOV     d9,r0,r1              ;182
0003c8  6a78              LDR      r0,[r7,#0x24]         ;182  ; AngleDifference
0003ca  f7fffffe          BL       __aeabi_f2d
0003ce  ec410b10          VMOV     d0,r0,r1              ;182
0003d2  f7fffffe          BL       __hardfp_sin
0003d6  ec532b19          VMOV     r2,r3,d9              ;182
0003da  ec510b10          VMOV     r0,r1,d0              ;182
0003de  f7fffffe          BL       __aeabi_dmul
0003e2  ec410b19          VMOV     d9,r0,r1              ;182
0003e6  6a38              LDR      r0,[r7,#0x20]         ;182  ; IntegralY
0003e8  f7fffffe          BL       __aeabi_f2d
0003ec  ec532b19          VMOV     r2,r3,d9              ;182
0003f0  f7fffffe          BL       __aeabi_dsub
0003f4  f7fffffe          BL       __aeabi_d2f
0003f8  62f8              STR      r0,[r7,#0x2c]         ;182  ; RectificationY
0003fa  e0f4              B        |L9.1510|
                  |L9.1020|
                          DCD      ||.bss||+0x10
                  |L9.1024|
000400  3fc90fda          DCFS     0x3fc90fda ; 1.5707962512969971
                  |L9.1028|
000404  4096cbe4          DCFS     0x4096cbe4 ; 4.7123889923095703
                  |L9.1032|
000408  43340000          DCFS     0x43340000 ; 180
                  |L9.1036|
00040c  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L9.1040|
                          DCD      ||.data||
                  |L9.1044|
000414  447a0000          DCFS     0x447a0000 ; 1000
                  |L9.1048|
000418  20000000          DCFD     0x3ff147ae20000000 ; 1.0800000429153442
00041c  3ff147ae
                  |L9.1056|
                          DCD      0x41c80000
                  |L9.1060|
                          DCD      0x43a78000
                  |L9.1064|
000428  00000000          DCFS     0x00000000 ; 0
                  |L9.1068|
                          DCD      0xbd7e0000
                  |L9.1072|
                          DCD      0xbce50000
                  |L9.1076|
                          DCD      0xbc8b0000
                  |L9.1080|
000438  40000000          DCFD     0x3fc3333340000000 ; 0.15000000596046448
00043c  3fc33333
                  |L9.1088|
000440  80000000          DCFD     0x3fd8f5c280000000 ; 0.38999998569488525
000444  3fd8f5c2
                  |L9.1096|
000448  e0000000          DCFD     0x3fe6147ae0000000 ; 0.68999999761581421
00044c  3fe6147a
                  |L9.1104|
000450  ee100a10          VMOV     r0,s0                 ;186
000454  f7fffffe          BL       __aeabi_f2d
000458  ec410b10          VMOV     d0,r0,r1              ;186
00045c  f7fffffe          BL       __hardfp_sin
000460  ec532b19          VMOV     r2,r3,d9              ;186
000464  ec510b10          VMOV     r0,r1,d0              ;186
000468  f7fffffe          BL       __aeabi_dmul
00046c  ec410b1c          VMOV     d12,r0,r1             ;186
000470  6828              LDR      r0,[r5,#0]            ;186
000472  f7fffffe          BL       __aeabi_f2d
000476  ec532b1c          VMOV     r2,r3,d12             ;186
00047a  f7fffffe          BL       __aeabi_dadd
00047e  f7fffffe          BL       __aeabi_d2f
000482  61f8              STR      r0,[r7,#0x1c]         ;186  ; IntegralX
000484  6a78              LDR      r0,[r7,#0x24]         ;187  ; AngleDifference
000486  f7fffffe          BL       __aeabi_f2d
00048a  ec410b10          VMOV     d0,r0,r1              ;187
00048e  f7fffffe          BL       __hardfp_cos
000492  ec532b19          VMOV     r2,r3,d9              ;187
000496  ec510b10          VMOV     r0,r1,d0              ;187
00049a  f7fffffe          BL       __aeabi_dmul
00049e  ec410b19          VMOV     d9,r0,r1              ;187
0004a2  6830              LDR      r0,[r6,#0]            ;187
0004a4  f7fffffe          BL       __aeabi_f2d
0004a8  ec532b19          VMOV     r2,r3,d9              ;187
0004ac  f7fffffe          BL       __aeabi_dsub
0004b0  f7fffffe          BL       __aeabi_d2f
0004b4  6238              STR      r0,[r7,#0x20]         ;187  ; IntegralY
0004b6  6a78              LDR      r0,[r7,#0x24]         ;189  ; AngleDifference
0004b8  f7fffffe          BL       __aeabi_f2d
0004bc  ec410b10          VMOV     d0,r0,r1              ;189
0004c0  f7fffffe          BL       __hardfp_sin
0004c4  ec532b1b          VMOV     r2,r3,d11             ;189
0004c8  ec510b10          VMOV     r0,r1,d0              ;189
0004cc  f7fffffe          BL       __aeabi_dmul
0004d0  ec410b19          VMOV     d9,r0,r1              ;189
0004d4  6828              LDR      r0,[r5,#0]            ;189
0004d6  f7fffffe          BL       __aeabi_f2d
0004da  ec532b19          VMOV     r2,r3,d9              ;189
0004de  f7fffffe          BL       __aeabi_dsub
0004e2  f7fffffe          BL       __aeabi_d2f
0004e6  62b8              STR      r0,[r7,#0x28]         ;189  ; RectificationX
0004e8  6a78              LDR      r0,[r7,#0x24]         ;190  ; AngleDifference
0004ea  f7fffffe          BL       __aeabi_f2d
0004ee  ec410b10          VMOV     d0,r0,r1              ;190
0004f2  f7fffffe          BL       __hardfp_cos
0004f6  ec532b1b          VMOV     r2,r3,d11             ;190
0004fa  ec510b10          VMOV     r0,r1,d0              ;190
0004fe  f7fffffe          BL       __aeabi_dmul
000502  ec410b19          VMOV     d9,r0,r1              ;190
000506  6830              LDR      r0,[r6,#0]            ;190
000508  f7fffffe          BL       __aeabi_f2d
00050c  ec532b19          VMOV     r2,r3,d9              ;190
000510  f7fffffe          BL       __aeabi_dadd
000514  f7fffffe          BL       __aeabi_d2f
000518  62f8              STR      r0,[r7,#0x2c]         ;190  ; RectificationY
00051a  e064              B        |L9.1510|
                  |L9.1308|
00051c  ee100a10          VMOV     r0,s0                 ;194
000520  f7fffffe          BL       __aeabi_f2d
000524  ec410b10          VMOV     d0,r0,r1              ;194
000528  f7fffffe          BL       __hardfp_sin
00052c  ec532b1b          VMOV     r2,r3,d11             ;194
000530  ec510b10          VMOV     r0,r1,d0              ;194
000534  f7fffffe          BL       __aeabi_dmul
000538  ec410b1c          VMOV     d12,r0,r1             ;194
00053c  6828              LDR      r0,[r5,#0]            ;194
00053e  f7fffffe          BL       __aeabi_f2d
000542  ec532b1c          VMOV     r2,r3,d12             ;194
000546  f7fffffe          BL       __aeabi_dsub
00054a  f7fffffe          BL       __aeabi_d2f
00054e  61f8              STR      r0,[r7,#0x1c]         ;194  ; IntegralX
000550  6a78              LDR      r0,[r7,#0x24]         ;195  ; AngleDifference
000552  f7fffffe          BL       __aeabi_f2d
000556  ec410b10          VMOV     d0,r0,r1              ;195
00055a  f7fffffe          BL       __hardfp_cos
00055e  ec532b1b          VMOV     r2,r3,d11             ;195
000562  ec510b10          VMOV     r0,r1,d0              ;195
000566  f7fffffe          BL       __aeabi_dmul
00056a  ec410b1b          VMOV     d11,r0,r1             ;195
00056e  6830              LDR      r0,[r6,#0]            ;195
000570  f7fffffe          BL       __aeabi_f2d
000574  ec532b1b          VMOV     r2,r3,d11             ;195
000578  f7fffffe          BL       __aeabi_dadd
00057c  f7fffffe          BL       __aeabi_d2f
000580  6238              STR      r0,[r7,#0x20]         ;195  ; IntegralY
000582  6a78              LDR      r0,[r7,#0x24]         ;197  ; AngleDifference
000584  f7fffffe          BL       __aeabi_f2d
000588  ec410b10          VMOV     d0,r0,r1              ;197
00058c  f7fffffe          BL       __hardfp_sin
000590  ec532b19          VMOV     r2,r3,d9              ;197
000594  ec510b10          VMOV     r0,r1,d0              ;197
000598  f7fffffe          BL       __aeabi_dmul
00059c  ec410b1b          VMOV     d11,r0,r1             ;197
0005a0  6828              LDR      r0,[r5,#0]            ;197
0005a2  f7fffffe          BL       __aeabi_f2d
0005a6  ec532b1b          VMOV     r2,r3,d11             ;197
0005aa  f7fffffe          BL       __aeabi_dadd
0005ae  f7fffffe          BL       __aeabi_d2f
0005b2  62b8              STR      r0,[r7,#0x28]         ;197  ; RectificationX
0005b4  6a78              LDR      r0,[r7,#0x24]         ;198  ; AngleDifference
0005b6  f7fffffe          BL       __aeabi_f2d
0005ba  ec410b10          VMOV     d0,r0,r1              ;198
0005be  f7fffffe          BL       __hardfp_cos
0005c2  ec532b19          VMOV     r2,r3,d9              ;198
0005c6  ec510b10          VMOV     r0,r1,d0              ;198
0005ca  f7fffffe          BL       __aeabi_dmul
0005ce  ec410b19          VMOV     d9,r0,r1              ;198
0005d2  6830              LDR      r0,[r6,#0]            ;198
0005d4  f7fffffe          BL       __aeabi_f2d
0005d8  ec532b19          VMOV     r2,r3,d9              ;198
0005dc  f7fffffe          BL       __aeabi_dsub
0005e0  f7fffffe          BL       __aeabi_d2f
0005e4  62f8              STR      r0,[r7,#0x2c]         ;198  ; RectificationY
                  |L9.1510|
0005e6  48fe              LDR      r0,|L9.2528|
0005e8  6a40              LDR      r0,[r0,#0x24]         ;201  ; ControlOutputValue
0005ea  f7fffffe          BL       __aeabi_f2d
0005ee  e9cd011e          STRD     r0,r1,[sp,#0x78]      ;201
0005f2  ed970a08          VLDR     s0,[r7,#0x20]         ;201
0005f6  ee200a08          VMUL.F32 s0,s0,s16             ;201
0005fa  ee100a10          VMOV     r0,s0                 ;201
0005fe  f7fffffe          BL       __aeabi_f2d
000602  e9cd011c          STRD     r0,r1,[sp,#0x70]      ;201
000606  ed970a07          VLDR     s0,[r7,#0x1c]         ;201
00060a  ee200a08          VMUL.F32 s0,s0,s16             ;201
00060e  ee100a10          VMOV     r0,s0                 ;201
000612  f7fffffe          BL       __aeabi_f2d
000616  e9cd011a          STRD     r0,r1,[sp,#0x68]      ;201
00061a  ed970a0b          VLDR     s0,[r7,#0x2c]         ;201
00061e  ee200a08          VMUL.F32 s0,s0,s16             ;201
000622  ee100a10          VMOV     r0,s0                 ;201
000626  f7fffffe          BL       __aeabi_f2d
00062a  e9cd0118          STRD     r0,r1,[sp,#0x60]      ;201
00062e  ed970a0a          VLDR     s0,[r7,#0x28]         ;201
000632  ee200a08          VMUL.F32 s0,s0,s16             ;201
000636  ee100a10          VMOV     r0,s0                 ;201
00063a  f7fffffe          BL       __aeabi_f2d
00063e  e9cd0116          STRD     r0,r1,[sp,#0x58]      ;201
000642  ed970a09          VLDR     s0,[r7,#0x24]         ;201
000646  ee600a0a          VMUL.F32 s1,s0,s20             ;201
00064a  ee800aa8          VDIV.F32 s0,s1,s17             ;201
00064e  ee100a10          VMOV     r0,s0                 ;201
000652  f7fffffe          BL       __aeabi_f2d
000656  e9cd0114          STRD     r0,r1,[sp,#0x50]      ;201
00065a  f8db000c          LDR      r0,[r11,#0xc]         ;201  ; camerparameter
00065e  f7fffffe          BL       __aeabi_f2d
000662  e9cd0112          STRD     r0,r1,[sp,#0x48]      ;201
000666  ed960a00          VLDR     s0,[r6,#0]            ;201
00066a  ee200a08          VMUL.F32 s0,s0,s16             ;201
00066e  ee100a10          VMOV     r0,s0                 ;201
000672  f7fffffe          BL       __aeabi_f2d
000676  ed950a00          VLDR     s0,[r5,#0]            ;201
00067a  4681              MOV      r9,r0                 ;201
00067c  468a              MOV      r10,r1                ;201
00067e  ee200a08          VMUL.F32 s0,s0,s16             ;201
000682  ee100a10          VMOV     r0,s0                 ;201
000686  f7fffffe          BL       __aeabi_f2d
00068a  ec410b10          VMOV     d0,r0,r1              ;201
00068e  f89400ba          LDRB     r0,[r4,#0xba]         ;201
000692  9010              STR      r0,[sp,#0x40]         ;201
000694  e9dd011e          LDRD     r0,r1,[sp,#0x78]      ;201
000698  e9cd010e          STRD     r0,r1,[sp,#0x38]      ;201
00069c  e9dd011c          LDRD     r0,r1,[sp,#0x70]      ;201
0006a0  e9cd010c          STRD     r0,r1,[sp,#0x30]      ;201
0006a4  e9dd011a          LDRD     r0,r1,[sp,#0x68]      ;201
0006a8  e9cd010a          STRD     r0,r1,[sp,#0x28]      ;201
0006ac  e9dd0118          LDRD     r0,r1,[sp,#0x60]      ;201
0006b0  e9cd0108          STRD     r0,r1,[sp,#0x20]      ;201
0006b4  e9dd0116          LDRD     r0,r1,[sp,#0x58]      ;201
0006b8  e9cd0106          STRD     r0,r1,[sp,#0x18]      ;201
0006bc  e9dd0114          LDRD     r0,r1,[sp,#0x50]      ;201
0006c0  e9cd0104          STRD     r0,r1,[sp,#0x10]      ;201
0006c4  e9dd0112          LDRD     r0,r1,[sp,#0x48]      ;201
0006c8  e9cd9a00          STRD     r9,r10,[sp,#0]        ;201
0006cc  e9cd0102          STRD     r0,r1,[sp,#8]         ;201
0006d0  ec532b10          VMOV     r2,r3,d0              ;201
0006d4  49c3              LDR      r1,|L9.2532|
0006d6  2001              MOVS     r0,#1                 ;201
0006d8  f7fffffe          BL       Uart_Printf
0006dc  2100              MOVS     r1,#0                 ;205
0006de  48c2              LDR      r0,|L9.2536|
0006e0  f7fffffe          BL       TIM_SetCounter
0006e4  e1a4              B        |L9.2608|
                  |L9.1766|
0006e6  ed941a09          VLDR     s2,[r4,#0x24]         ;213
0006ea  ed871a0c          VSTR     s2,[r7,#0x30]         ;213
0006ee  f9b4000c          LDRSH    r0,[r4,#0xc]          ;214
0006f2  ee000a10          VMOV     s0,r0                 ;214
0006f6  eeb80ac0          VCVT.F32.S32 s0,s0                 ;214
0006fa  eec00a08          VDIV.F32 s1,s0,s16             ;214
0006fe  edc70a0d          VSTR     s1,[r7,#0x34]         ;214
000702  4aba              LDR      r2,|L9.2540|
000704  ed940a10          VLDR     s0,[r4,#0x40]         ;216
000708  1f11              SUBS     r1,r2,#4              ;216
00070a  1f08              SUBS     r0,r1,#4              ;216
00070c  f7fffffe          BL       agvEncoderIntegrate
000710  6c20              LDR      r0,[r4,#0x40]         ;218
000712  f7fffffe          BL       __aeabi_f2d
000716  ec410b19          VMOV     d9,r0,r1              ;218
00071a  6a78              LDR      r0,[r7,#0x24]         ;218  ; AngleDifference
00071c  f7fffffe          BL       __aeabi_f2d
000720  ec410b10          VMOV     d0,r0,r1              ;218
000724  f7fffffe          BL       __hardfp_cos
000728  ec532b19          VMOV     r2,r3,d9              ;218
00072c  ec510b10          VMOV     r0,r1,d0              ;218
000730  f7fffffe          BL       __aeabi_dmul
000734  ec410b19          VMOV     d9,r0,r1              ;218
000738  69f8              LDR      r0,[r7,#0x1c]         ;218  ; IntegralX
00073a  f7fffffe          BL       __aeabi_f2d
00073e  ec532b19          VMOV     r2,r3,d9              ;218
000742  f7fffffe          BL       __aeabi_dadd
000746  f7fffffe          BL       __aeabi_d2f
00074a  62b8              STR      r0,[r7,#0x28]         ;218  ; RectificationX
00074c  6c20              LDR      r0,[r4,#0x40]         ;219
00074e  f7fffffe          BL       __aeabi_f2d
000752  ec410b19          VMOV     d9,r0,r1              ;219
000756  6a78              LDR      r0,[r7,#0x24]         ;219  ; AngleDifference
000758  f7fffffe          BL       __aeabi_f2d
00075c  ec410b10          VMOV     d0,r0,r1              ;219
000760  f7fffffe          BL       __hardfp_sin
000764  ec532b19          VMOV     r2,r3,d9              ;219
000768  ec510b10          VMOV     r0,r1,d0              ;219
00076c  f7fffffe          BL       __aeabi_dmul
000770  ec410b19          VMOV     d9,r0,r1              ;219
000774  6a38              LDR      r0,[r7,#0x20]         ;219  ; IntegralY
000776  f7fffffe          BL       __aeabi_f2d
00077a  ec532b19          VMOV     r2,r3,d9              ;219
00077e  f7fffffe          BL       __aeabi_dadd
000782  f7fffffe          BL       __aeabi_d2f
000786  62f8              STR      r0,[r7,#0x2c]         ;219  ; RectificationY
                  |L9.1928|
000788  e0ec              B        |L9.2404|
                  |L9.1930|
00078a  ed941a0a          VLDR     s2,[r4,#0x28]         ;223
00078e  ed871a0c          VSTR     s2,[r7,#0x30]         ;223
000792  f9b4000e          LDRSH    r0,[r4,#0xe]          ;224
000796  ee000a10          VMOV     s0,r0                 ;224
00079a  eeb80ac0          VCVT.F32.S32 s0,s0                 ;224
00079e  eec00a08          VDIV.F32 s1,s0,s16             ;224
0007a2  edc70a0d          VSTR     s1,[r7,#0x34]         ;224
0007a6  4a91              LDR      r2,|L9.2540|
0007a8  ed940a10          VLDR     s0,[r4,#0x40]         ;226
0007ac  1f11              SUBS     r1,r2,#4              ;226
0007ae  1f08              SUBS     r0,r1,#4              ;226
0007b0  f7fffffe          BL       agvEncoderIntegrate
0007b4  6c20              LDR      r0,[r4,#0x40]         ;228
0007b6  f7fffffe          BL       __aeabi_f2d
0007ba  ec410b19          VMOV     d9,r0,r1              ;228
0007be  6a78              LDR      r0,[r7,#0x24]         ;228  ; AngleDifference
0007c0  f7fffffe          BL       __aeabi_f2d
0007c4  ec410b10          VMOV     d0,r0,r1              ;228
0007c8  f7fffffe          BL       __hardfp_cos
0007cc  ec532b19          VMOV     r2,r3,d9              ;228
0007d0  ec510b10          VMOV     r0,r1,d0              ;228
0007d4  f7fffffe          BL       __aeabi_dmul
0007d8  ec410b19          VMOV     d9,r0,r1              ;228
0007dc  69f8              LDR      r0,[r7,#0x1c]         ;228  ; IntegralX
0007de  f7fffffe          BL       __aeabi_f2d
0007e2  ec532b19          VMOV     r2,r3,d9              ;228
0007e6  f7fffffe          BL       __aeabi_dsub
0007ea  f7fffffe          BL       __aeabi_d2f
0007ee  62b8              STR      r0,[r7,#0x28]         ;228  ; RectificationX
0007f0  6c20              LDR      r0,[r4,#0x40]         ;229
0007f2  f7fffffe          BL       __aeabi_f2d
0007f6  ec410b19          VMOV     d9,r0,r1              ;229
0007fa  6a78              LDR      r0,[r7,#0x24]         ;229  ; AngleDifference
0007fc  f7fffffe          BL       __aeabi_f2d
000800  ec410b10          VMOV     d0,r0,r1              ;229
000804  f7fffffe          BL       __hardfp_sin
000808  ec532b19          VMOV     r2,r3,d9              ;229
00080c  ec510b10          VMOV     r0,r1,d0              ;229
000810  f7fffffe          BL       __aeabi_dmul
000814  ec410b19          VMOV     d9,r0,r1              ;229
000818  6a38              LDR      r0,[r7,#0x20]         ;229  ; IntegralY
00081a  f7fffffe          BL       __aeabi_f2d
00081e  ec532b19          VMOV     r2,r3,d9              ;229
000822  f7fffffe          BL       __aeabi_dsub
000826  f7fffffe          BL       __aeabi_d2f
00082a  62f8              STR      r0,[r7,#0x2c]         ;229  ; RectificationY
00082c  e09a              B        |L9.2404|
                  |L9.2094|
00082e  ed941a0b          VLDR     s2,[r4,#0x2c]         ;233
000832  ed871a0c          VSTR     s2,[r7,#0x30]         ;233
000836  f9b40010          LDRSH    r0,[r4,#0x10]         ;234
00083a  ee000a90          VMOV     s1,r0                 ;234
00083e  eeb82ae0          VCVT.F32.S32 s4,s1                 ;234
000842  eec20a08          VDIV.F32 s1,s4,s16             ;234
000846  edc70a0d          VSTR     s1,[r7,#0x34]         ;234
00084a  ee300a21          VADD.F32 s0,s0,s3              ;236
00084e  ed870a0e          VSTR     s0,[r7,#0x38]         ;236
000852  4a66              LDR      r2,|L9.2540|
000854  3214              ADDS     r2,r2,#0x14           ;237
000856  f1a20118          SUB      r1,r2,#0x18           ;237
00085a  1f08              SUBS     r0,r1,#4              ;237
00085c  ed9f0a64          VLDR     s0,|L9.2544|
000860  f7fffffe          BL       agvEncoderIntegrate
000864  6a78              LDR      r0,[r7,#0x24]         ;239  ; AngleDifference
000866  f7fffffe          BL       __aeabi_f2d
00086a  ec410b10          VMOV     d0,r0,r1              ;239
00086e  f7fffffe          BL       __hardfp_sin
000872  ec532b1b          VMOV     r2,r3,d11             ;239
000876  ec510b10          VMOV     r0,r1,d0              ;239
00087a  f7fffffe          BL       __aeabi_dmul
00087e  ec410b19          VMOV     d9,r0,r1              ;239
000882  69f8              LDR      r0,[r7,#0x1c]         ;239  ; IntegralX
000884  f7fffffe          BL       __aeabi_f2d
000888  ec532b19          VMOV     r2,r3,d9              ;239
00088c  f7fffffe          BL       __aeabi_dsub
000890  f7fffffe          BL       __aeabi_d2f
000894  62b8              STR      r0,[r7,#0x28]         ;239  ; RectificationX
000896  6a78              LDR      r0,[r7,#0x24]         ;240  ; AngleDifference
000898  f7fffffe          BL       __aeabi_f2d
00089c  ec410b10          VMOV     d0,r0,r1              ;240
0008a0  f7fffffe          BL       __hardfp_cos
0008a4  ec532b1b          VMOV     r2,r3,d11             ;240
0008a8  ec510b10          VMOV     r0,r1,d0              ;240
0008ac  f7fffffe          BL       __aeabi_dmul
0008b0  ec410b19          VMOV     d9,r0,r1              ;240
0008b4  6a38              LDR      r0,[r7,#0x20]         ;240  ; IntegralY
0008b6  f7fffffe          BL       __aeabi_f2d
0008ba  ec532b19          VMOV     r2,r3,d9              ;240
0008be  f7fffffe          BL       __aeabi_dadd
0008c2  f7fffffe          BL       __aeabi_d2f
0008c6  62f8              STR      r0,[r7,#0x2c]         ;240  ; RectificationY
0008c8  e04c              B        |L9.2404|
                  |L9.2250|
;;;243    		{
;;;244    			RequstAngle = ControlInputValue->AgvFourthRudderWheelAngle;
0008ca  ed941a0c          VLDR     s2,[r4,#0x30]
0008ce  ed871a0c          VSTR     s2,[r7,#0x30]
;;;245    			RequstSpeed = (float)ControlInputValue->AgvActualFourthSpeed / 1000.0f;
0008d2  f9b40012          LDRSH    r0,[r4,#0x12]
0008d6  ee000a90          VMOV     s1,r0
0008da  eef81ae0          VCVT.F32.S32 s3,s1
0008de  eec10a88          VDIV.F32 s1,s3,s16
0008e2  edc70a0d          VSTR     s1,[r7,#0x34]
;;;246
;;;247    			Angle = AngleDifference + 3.0f * PI / 2.0f;
0008e6  ee300a02          VADD.F32 s0,s0,s4
0008ea  ed870a0e          VSTR     s0,[r7,#0x38]
;;;248    			agvEncoderIntegrate(PINGYI_WHEEL_BASE_BACK,&IntegralX,&IntegralY,&Angle,RequstSpeed,RequstAngle);
0008ee  4a3f              LDR      r2,|L9.2540|
0008f0  3214              ADDS     r2,r2,#0x14
0008f2  f1a20118          SUB      r1,r2,#0x18
0008f6  1f08              SUBS     r0,r1,#4
0008f8  ed9f0a3e          VLDR     s0,|L9.2548|
0008fc  f7fffffe          BL       agvEncoderIntegrate
;;;249
;;;250    			RectificationX = IntegralX + sin(AngleDifference) * PINGYI_WHEEL_BASE;
000900  6a78              LDR      r0,[r7,#0x24]  ; AngleDifference
000902  f7fffffe          BL       __aeabi_f2d
000906  ec410b10          VMOV     d0,r0,r1
00090a  f7fffffe          BL       __hardfp_sin
00090e  ec532b1b          VMOV     r2,r3,d11
000912  ec510b10          VMOV     r0,r1,d0
000916  f7fffffe          BL       __aeabi_dmul
00091a  ec410b19          VMOV     d9,r0,r1
00091e  69f8              LDR      r0,[r7,#0x1c]  ; IntegralX
000920  f7fffffe          BL       __aeabi_f2d
000924  ec532b19          VMOV     r2,r3,d9
000928  f7fffffe          BL       __aeabi_dadd
00092c  f7fffffe          BL       __aeabi_d2f
000930  62b8              STR      r0,[r7,#0x28]  ; RectificationX
;;;251    			RectificationY = IntegralY - cos(AngleDifference) * PINGYI_WHEEL_BASE;
000932  6a78              LDR      r0,[r7,#0x24]  ; AngleDifference
000934  f7fffffe          BL       __aeabi_f2d
000938  ec410b10          VMOV     d0,r0,r1
00093c  f7fffffe          BL       __hardfp_cos
000940  ec532b1b          VMOV     r2,r3,d11
000944  ec510b10          VMOV     r0,r1,d0
000948  f7fffffe          BL       __aeabi_dmul
00094c  ec410b19          VMOV     d9,r0,r1
000950  6a38              LDR      r0,[r7,#0x20]  ; IntegralY
000952  f7fffffe          BL       __aeabi_f2d
000956  ec532b19          VMOV     r2,r3,d9
00095a  f7fffffe          BL       __aeabi_dsub
00095e  f7fffffe          BL       __aeabi_d2f
000962  62f8              STR      r0,[r7,#0x2c]  ; RectificationY
                  |L9.2404|
;;;252    		}
;;;253
;;;254
;;;255    		Uart_Printf(COM1,"积分数据x=%.1f	y=%.1f	w=%f	%d	speed=%.1f	angle=%f	dir=%d	offset=%.1f\r\n",
000964  481e              LDR      r0,|L9.2528|
000966  6a40              LDR      r0,[r0,#0x24]  ; ControlOutputValue
000968  f7fffffe          BL       __aeabi_f2d
00096c  ed970a0c          VLDR     s0,[r7,#0x30]
000970  4605              MOV      r5,r0
000972  460e              MOV      r6,r1
000974  ee600a0a          VMUL.F32 s1,s0,s20
000978  ee800aa8          VDIV.F32 s0,s1,s17
00097c  ee100a10          VMOV     r0,s0
000980  f7fffffe          BL       __aeabi_f2d
000984  ed970a0d          VLDR     s0,[r7,#0x34]
000988  4681              MOV      r9,r0
00098a  468a              MOV      r10,r1
00098c  ee200a08          VMUL.F32 s0,s0,s16
000990  ee100a10          VMOV     r0,s0
000994  f7fffffe          BL       __aeabi_f2d
000998  e9cd011e          STRD     r0,r1,[sp,#0x78]
00099c  ed970a0e          VLDR     s0,[r7,#0x38]
0009a0  ee600a0a          VMUL.F32 s1,s0,s20
0009a4  ee800aa8          VDIV.F32 s0,s1,s17
0009a8  ee100a10          VMOV     r0,s0
0009ac  f7fffffe          BL       __aeabi_f2d
0009b0  e9cd011c          STRD     r0,r1,[sp,#0x70]
0009b4  ed970a08          VLDR     s0,[r7,#0x20]
0009b8  ee200a08          VMUL.F32 s0,s0,s16
0009bc  ee100a10          VMOV     r0,s0
0009c0  f7fffffe          BL       __aeabi_f2d
0009c4  e9cd011a          STRD     r0,r1,[sp,#0x68]
0009c8  ed970a07          VLDR     s0,[r7,#0x1c]
0009cc  ee200a08          VMUL.F32 s0,s0,s16
0009d0  ee100a10          VMOV     r0,s0
0009d4  f7fffffe          BL       __aeabi_f2d
0009d8  ec410b10          VMOV     d0,r0,r1
0009dc  e00c              B        |L9.2552|
0009de  0000              DCW      0x0000
                  |L9.2528|
                          DCD      ControlOutputValue
                  |L9.2532|
                          DCD      ||.conststring||
                  |L9.2536|
                          DCD      0x40000400
                  |L9.2540|
                          DCD      ||.data||+0x24
                  |L9.2544|
0009f0  3f30a3d7          DCFS     0x3f30a3d7 ; 0.68999999761581421
                  |L9.2548|
0009f4  3ec7ae14          DCFS     0x3ec7ae14 ; 0.38999998569488525
                  |L9.2552|
0009f8  e9cd560c          STRD     r5,r6,[sp,#0x30]
0009fc  f89400ba          LDRB     r0,[r4,#0xba]
000a00  900a              STR      r0,[sp,#0x28]
000a02  e9dd011e          LDRD     r0,r1,[sp,#0x78]
000a06  e9cd9a08          STRD     r9,r10,[sp,#0x20]
000a0a  e9cd0106          STRD     r0,r1,[sp,#0x18]
000a0e  f89b0000          LDRB     r0,[r11,#0]  ; camerparameter
000a12  9004              STR      r0,[sp,#0x10]
000a14  e9dd011c          LDRD     r0,r1,[sp,#0x70]
000a18  e9cd0102          STRD     r0,r1,[sp,#8]
000a1c  e9dd011a          LDRD     r0,r1,[sp,#0x68]
000a20  e9cd0100          STRD     r0,r1,[sp,#0]
000a24  ec532b10          VMOV     r2,r3,d0
000a28  4926              LDR      r1,|L9.2756|
000a2a  2001              MOVS     r0,#1
000a2c  f7fffffe          BL       Uart_Printf
                  |L9.2608|
;;;256    		IntegralX*1000.0f,IntegralY*1000.0f,Angle*180.0f/PI,camerparameter.TwoDCdirection,
;;;257    		RequstSpeed*1000,RequstAngle*180.0f/PI,ControlInputValue->Direction,ControlOutputValue.AgvOffset);
;;;258    	}
;;;259
;;;260    	if(*Theta >= 2*PI)*Theta-= 2.0f*PI;
000a30  ed980a00          VLDR     s0,[r8,#0]
000a34  4924              LDR      r1,|L9.2760|
000a36  ee100a10          VMOV     r0,s0
000a3a  eddf0a24          VLDR     s1,|L9.2764|
000a3e  4288              CMP      r0,r1
000a40  db03              BLT      |L9.2634|
000a42  ee300a60          VSUB.F32 s0,s0,s1
000a46  ed880a00          VSTR     s0,[r8,#0]
                  |L9.2634|
;;;261    	if(*Theta < 0)*Theta+= 2.0f*PI;
000a4a  ed980a00          VLDR     s0,[r8,#0]
000a4e  eeb50ac0          VCMPE.F32 s0,#0.0
000a52  eef1fa10          VMRS     APSR_nzcv,FPSCR
000a56  d203              BCS      |L9.2656|
000a58  ee300a20          VADD.F32 s0,s0,s1
000a5c  ed880a00          VSTR     s0,[r8,#0]
                  |L9.2656|
;;;262
;;;263    	if(ControlInputValue->Direction == 1)ControlOutputValue.AgvOffset = (RectificationY*1000.0f - 150.0f);//前进
000a60  f89400ba          LDRB     r0,[r4,#0xba]
000a64  ed9f0a1a          VLDR     s0,|L9.2768|
000a68  2801              CMP      r0,#1
000a6a  d011              BEQ      |L9.2704|
;;;264    	else if(ControlInputValue->Direction == 2)ControlOutputValue.AgvOffset = -(RectificationY*1000.0f - 150.0f);	//后退
000a6c  2802              CMP      r0,#2
000a6e  d017              BEQ      |L9.2720|
;;;265    	else if(ControlInputValue->Direction == 3)ControlOutputValue.AgvOffset = -(RectificationX*1000.0f);//前进//平移前进
000a70  2803              CMP      r0,#3
000a72  d01d              BEQ      |L9.2736|
;;;266    	else if(ControlInputValue->Direction == 4)ControlOutputValue.AgvOffset = (RectificationX*1000.0f);//前进//平移后退
000a74  2804              CMP      r0,#4
000a76  d106              BNE      |L9.2694|
000a78  ed970a0a          VLDR     s0,[r7,#0x28]
000a7c  4815              LDR      r0,|L9.2772|
000a7e  ee200a08          VMUL.F32 s0,s0,s16
000a82  ed800a09          VSTR     s0,[r0,#0x24]
                  |L9.2694|
;;;267    }
000a86  b021              ADD      sp,sp,#0x84
000a88  ecbd8b0a          VPOP     {d8-d12}
000a8c  e8bd8ff0          POP      {r4-r11,pc}
                  |L9.2704|
000a90  edd70a0b          VLDR     s1,[r7,#0x2c]         ;263
000a94  480f              LDR      r0,|L9.2772|
000a96  ee100a88          VNMLS.F32 s0,s1,s16             ;263
000a9a  ed800a09          VSTR     s0,[r0,#0x24]         ;263
000a9e  e7f2              B        |L9.2694|
                  |L9.2720|
000aa0  edd70a0b          VLDR     s1,[r7,#0x2c]         ;264
000aa4  480b              LDR      r0,|L9.2772|
000aa6  ee000ac8          VMLS.F32 s0,s1,s16             ;264
000aaa  ed800a09          VSTR     s0,[r0,#0x24]         ;264
000aae  e7ea              B        |L9.2694|
                  |L9.2736|
000ab0  ed970a0a          VLDR     s0,[r7,#0x28]         ;265
000ab4  eddf0a08          VLDR     s1,|L9.2776|
000ab8  4806              LDR      r0,|L9.2772|
000aba  ee200a20          VMUL.F32 s0,s0,s1              ;265
000abe  ed800a09          VSTR     s0,[r0,#0x24]         ;265
000ac2  e7e0              B        |L9.2694|
;;;268
                          ENDP

                  |L9.2756|
                          DCD      ||.conststring||+0x5c
                  |L9.2760|
                          DCD      0x40c90fda
                  |L9.2764|
000acc  40c90fda          DCFS     0x40c90fda ; 6.2831850051879883
                  |L9.2768|
000ad0  43160000          DCFS     0x43160000 ; 150
                  |L9.2772|
                          DCD      ControlOutputValue
                  |L9.2776|
000ad8  c47a0000          DCFS     0xc47a0000 ; -1000

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

                  agvLineControlFun PROC
;;;269    //直线控制算法
;;;270    float agvLineControlFun(float offset)
000000  4770              BX       lr
;;;271    {
;;;272
;;;273
;;;274    }
;;;275
                          ENDP


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

                  agvThreeWheelControlFun PROC
;;;374
;;;375    void agvThreeWheelControlFun(agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  b510              PUSH     {r4,lr}
;;;376    {
000002  4604              MOV      r4,r0
;;;377    	static float Xbias = 0.0f,Ybias = 0.0f,Theata = 0.0f;
;;;378
;;;379    	agvIntegratedLaserAndIntegralData(&Xbias,&Ybias,&Theata,ControlInputValue);
000004  4a08              LDR      r2,|L11.40|
000006  4623              MOV      r3,r4
000008  1f11              SUBS     r1,r2,#4
00000a  1f08              SUBS     r0,r1,#4
00000c  f7fffffe          BL       agvIntegratedLaserAndIntegralData
;;;380
;;;381    	HandlePlcData.SendPlcData.CurrentLandMark = camerparameter.id;
000010  4806              LDR      r0,|L11.44|
000012  4907              LDR      r1,|L11.48|
000014  8840              LDRH     r0,[r0,#2]  ; camerparameter
000016  f8a100f2          STRH     r0,[r1,#0xf2]
;;;382
;;;383    	if(ControlInputValue->agvControlStatus == 0)//手动
00001a  f89400b4          LDRB     r0,[r4,#0xb4]
00001e  2800              CMP      r0,#0
000020  d001              BEQ      |L11.38|
;;;384    	{
;;;385
;;;386    		return;
;;;387    	}
;;;388
;;;389    	if(ControlInputValue->Direction == 1)//前进
000022  f89400ba          LDRB     r0,[r4,#0xba]
                  |L11.38|
;;;390    	{
;;;391
;;;392    	}
;;;393    	else if(ControlInputValue->Direction == 2)	//后退
;;;394    	{
;;;395
;;;396    	}
;;;397    	else if(ControlInputValue->Direction == 3)//平移前进
;;;398    	{
;;;399
;;;400    	}
;;;401    	else if(ControlInputValue->Direction == 4)//平移后退
;;;402    	{
;;;403
;;;404    	}
;;;405    }
000026  bd10              POP      {r4,pc}
;;;406
                          ENDP

                  |L11.40|
                          DCD      ||.data||+0x58
                  |L11.44|
                          DCD      ||.bss||+0x10
                  |L11.48|
                          DCD      HandlePlcData

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

                  agvTwoDimensionalCodeControlAlgorithm PROC
;;;408
;;;409    void agvTwoDimensionalCodeControlAlgorithm(agvCalculationTurningSteering *CalculationTurningSteering,agvControlInputValue *ControlInputValue,agvControlOutputValue *ControlOutputValue)
000000  4603              MOV      r3,r0
;;;410    {
000002  b430              PUSH     {r4,r5}
000004  4608              MOV      r0,r1
000006  4611              MOV      r1,r2
;;;411    	switch(ControlInputValue->TurnningMode)
000008  f890203c          LDRB     r2,[r0,#0x3c]
00000c  2a05              CMP      r2,#5
00000e  d20d              BCS      |L12.44|
000010  e8dff002          TBB      [pc,r2]
000014  030c0c0c          DCB      0x03,0x0c,0x0c,0x0c
000018  0900              DCB      0x09,0x00
;;;412    	{
;;;413    		case 0:
;;;414    			QecodeControlFunction(CalculationTurningSteering,ControlInputValue,ControlOutputValue);
00001a  460a              MOV      r2,r1
00001c  4601              MOV      r1,r0
00001e  bc30              POP      {r4,r5}
000020  4618              MOV      r0,r3
000022  f7ffbffe          B.W      QecodeControlFunction
;;;415
;;;416    			break;
;;;417    		case 1:
;;;418    			break;
;;;419    		case 2:
;;;420    			break;
;;;421    		case 3:
;;;422    			break;
;;;423    		case 4://三舵轮、并且带全向行走
;;;424    			agvThreeWheelControlFun(ControlInputValue,ControlOutputValue);
000026  bc30              POP      {r4,r5}
000028  f7ffbffe          B.W      agvThreeWheelControlFun
                  |L12.44|
;;;425
;;;426    			break;
;;;427    		default:
;;;428    			break;
;;;429    	}
;;;430
;;;431    }
00002c  bc30              POP      {r4,r5}
00002e  4770              BX       lr
;;;432
                          ENDP


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

                  Tagdata
                          %        16
                  camerparameter
                          %        32

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

000000  b6feceac          DCB      182,254,206,172,194,235,202,253,190,221,"=%.1f\ty=%.1f\t"
000004  c2ebcafd
000008  bedd3d25
00000c  2e316609
000010  793d252e
000014  316609
000017  773d2566          DCB      "w=%f\t",188,208,189,199,"=%f\t",179,181,205,183,"x=%.1f"
00001b  09bcd0bd
00001f  c73d2566
000023  09b3b5cd
000027  b7783d25
00002b  2e3166
00002e  09793d25          DCB      "\ty=%.1f\t",187,253,183,214,181,227,"x=%.1f\ty=%.1f\tof"
000032  2e316609
000036  bbfdb7d6
00003a  b5e3783d
00003e  252e3166
000042  09793d25
000046  2e316609
00004a  6f66
00004c  66736574          DCB      "fset=%.1f\tD%d\r\n",0
000050  3d252e31
000054  66094425
000058  640d0a00
00005c  bbfdb7d6          DCB      187,253,183,214,202,253,190,221,"x=%.1f\ty=%.1f\tw=%f\t%"
000060  cafdbedd
000064  783d252e
000068  31660979
00006c  3d252e31
000070  6609773d
000074  25660925
000078  64097370          DCB      "d\tspeed=%.1f\tangle=%f\tdir=%d\toffset=%.1f\r\n",0
00007c  6565643d
000080  252e3166
000084  09616e67
000088  6c653d25
00008c  66096469
000090  723d2564
000094  096f6666
000098  7365743d
00009c  252e3166
0000a0  0d0a00

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

                  readposandstatedata
000000  c837              DCB      0xc8,0x37
                  selectpostag
000002  e41b              DCB      0xe4,0x1b
                  i
                          DCD      0x00000000
                  CurDis
                          DCD      0x00000000
                  LastDis
                          DCD      0x00000000
                  ||nn||
                          DCD      0x00000000
                  CenterPx
000014  00000000          DCFS     0x00000000 ; 0
                  CenterPy
000018  00000000          DCFS     0x00000000 ; 0
                  IntegralX
00001c  00000000          DCFS     0x00000000 ; 0
                  IntegralY
000020  00000000          DCFS     0x00000000 ; 0
                  AngleDifference
000024  00000000          DCFS     0x00000000 ; 0
                  RectificationX
000028  00000000          DCFS     0x00000000 ; 0
                  RectificationY
00002c  43160000          DCFS     0x43160000 ; 150
                  RequstAngle
000030  00000000          DCFS     0x00000000 ; 0
                  RequstSpeed
000034  00000000          DCFS     0x00000000 ; 0
                  Angle
000038  00000000          DCFS     0x00000000 ; 0
                  AngleMigration
00003c  00000000          DCFS     0x00000000 ; 0
                  OutPutData
000040  00000000          DCFS     0x00000000 ; 0
                  Xbias
000044  00000000          DCFS     0x00000000 ; 0
                  Ybias
000048  00000000          DCFS     0x00000000 ; 0
                  Theata
00004c  00000000          DCFS     0x00000000 ; 0
                  |symbol_number.64|
000050  00000000          DCFS     0x00000000 ; 0
                  |symbol_number.65|
000054  00000000          DCFS     0x00000000 ; 0
                  |symbol_number.66|
000058  00000000          DCFS     0x00000000 ; 0

;*** Start embedded assembler ***

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

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

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

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***