AGV / agv_shengyiPPxiaoche · Files

; generated by Component: ARM Compiler 5.06 update 6 (build 750) Tool: ArmCC [4d3637]
; commandline ArmCC [--c99 --list --split_sections --debug -c --asm --interleave -o.\flash\obj\canupdate.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\canupdate.d --cpu=Cortex-M4.fp --apcs=interwork -O1 --diag_suppress=9931,870 -I..\..\Libraries\CMSIS\Include -I..\..\Libraries\CMSIS\Device\ST\STM32F4xx\Include -I..\..\Libraries\STM32F4xx_StdPeriph_Driver\inc -I..\..\uCOS-III\uC-CPU -I..\..\uCOS-III\uC-LIB -I..\..\uCOS-III\uCOS-III\Ports -I..\..\uCOS-III\uCOS-III\Source -I..\..\uCOS-III\uC-CPU\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uC-LIB\Ports\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uCOS-III\Ports\ARM-Cortex-M4\Generic\RealView -I..\..\User -I..\..\User\bsp -I..\..\User\bsp\inc -I..\..\User\libapp -I..\..\RL-ARM\Config -I..\..\RL-ARM\Driver -I..\..\RL-ARM\RL-RTX\inc -I..\..\User\bsp\BSP -I..\..\RL-ARM\RL-CAN -I..\..\Libraries\DSP_LIB\Include -I..\..\MODBUS\modbus\rtu -I..\..\MODBUS\BARE\port -I..\..\MODBUS\modbus\include -I..\..\User\bsp\BSP -I..\..\PLC -I..\..\Avoid -I..\..\User\parameter -I..\..\User\LaserMotionCtr -I..\..\User\W5100S -I..\..\User\bsp -I..\..\User\CHASSIS -I..\..\User\CONTROLFUNCTION -I..\..\User\DATAUPDATE -I..\..\User\HARAWARE -I..\..\User\MOTORDRIVER -I..\..\User\NAVAGATION -I..\..\User\PLATFORM -I..\..\User\SENSOR -I.\RTE\_Flash -IC:\Users\YDJ\AppData\Local\Arm\Packs\ARM\CMSIS\5.5.1\CMSIS\Core\Include -IC:\Users\YDJ\AppData\Local\Arm\Packs\Keil\STM32F4xx_DFP\2.13.0\Drivers\CMSIS\Device\ST\STM32F4xx\Include -D__UVISION_VERSION=527 -D_RTE_ -DSTM32F407xx -DUSE_STDPERIPH_DRIVER -DSTM32F40_41xxx -D__RTX -D__FPU_USED=1 --omf_browse=.\flash\obj\canupdate.crf ..\..\User\CanUpdate.c]
                          THUMB

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

                  DriverFunction PROC
;;;22     u8 readPos[] = {0x40,0x63,0x60,0x00,0x00,0x00,0x00,0x00};
;;;23     void DriverFunction(DriverStruct *Motor) //行走 举升 移轴 推杆电机
000000  f8901058          LDRB     r1,[r0,#0x58]
;;;24     {
;;;25         if (Motor->Private.initStep != 255) //*****************************************如果未初始化先初始化
000004  29ff              CMP      r1,#0xff
000006  d001              BEQ      |L1.12|
;;;26         {
;;;27             init_driver(Motor); //非阻塞函数
000008  f7ffbffe          B.W      init_driver
                  |L1.12|
;;;28         }
;;;29         else //**************************************************************************无其他问题则发速度
;;;30         {
;;;31             if (set_speed(Motor) == 1) //返回0证明发了指令 则本轮不要发指令 否则会造成上一条指令无法匹配
00000c  f7ffbffe          B.W      set_speed
;;;32             {
;;;33             }
;;;34         }
;;;35     }
;;;36     
                          ENDP


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

                  GetSpeedSlope PROC
;;;48     
;;;49     float GetSpeedSlope(float SlopeSpeed, float CurSpeed,float TargetSpeed)
000000  eef01a40          VMOV.F32 s3,s0
;;;50     {
000004  eeb00a60          VMOV.F32 s0,s1
;;;51     
;;;52     	if (TargetSpeed > CurSpeed)
000008  eeb41ac0          VCMPE.F32 s2,s0
00000c  eef1fa10          VMRS     APSR_nzcv,FPSCR
000010  dd02              BLE      |L2.24|
;;;53     	{
;;;54     		CurSpeed += SlopeSpeed;
000012  ee300a21          VADD.F32 s0,s0,s3
000016  e002              B        |L2.30|
                  |L2.24|
;;;55     	}
;;;56     	else if (TargetSpeed < CurSpeed)
000018  d201              BCS      |L2.30|
;;;57     	{
;;;58     		CurSpeed -= SlopeSpeed;
00001a  ee300a61          VSUB.F32 s0,s0,s3
                  |L2.30|
;;;59     	}
;;;60     
;;;61     	if (_fabsf(CurSpeed - TargetSpeed) <= SlopeSpeed)
00001e  ee700a41          VSUB.F32 s1,s0,s2
000022  eef00ae0          VABS.F32 s1,s1
000026  eef40ae1          VCMPE.F32 s1,s3
00002a  eef1fa10          VMRS     APSR_nzcv,FPSCR
00002e  d801              BHI      |L2.52|
;;;62     	{
;;;63     		CurSpeed = TargetSpeed;
000030  eeb00a41          VMOV.F32 s0,s2
                  |L2.52|
;;;64     	}
;;;65     	
;;;66     	return CurSpeed;
;;;67     }
000034  4770              BX       lr
;;;68     
                          ENDP


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

                  LiftDataProcess PROC
;;;183    
;;;184    void LiftDataProcess(CanRxMsg *CanRxMsg)
000000  7ac1              LDRB     r1,[r0,#0xb]
;;;185    {
;;;186    	static int lastEncodeValue = 0,EncodeValue = 0;
;;;187    	if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x60 && CanRxMsg->Data[2] == 0x60)//设置速度模式
;;;188    	{
;;;189    		Lifter1.initState = 1;
000002  4a2b              LDR      r2,|L3.176|
000004  2960              CMP      r1,#0x60              ;187
000006  d105              BNE      |L3.20|
000008  7b03              LDRB     r3,[r0,#0xc]          ;187
00000a  2b60              CMP      r3,#0x60              ;187
00000c  d102              BNE      |L3.20|
00000e  7b43              LDRB     r3,[r0,#0xd]          ;187
000010  2b60              CMP      r3,#0x60              ;187
000012  d002              BEQ      |L3.26|
                  |L3.20|
;;;190    	}
;;;191    	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x83 && CanRxMsg->Data[2] == 0x60)//设置加速度
000014  2960              CMP      r1,#0x60
000016  d003              BEQ      |L3.32|
000018  e008              B        |L3.44|
                  |L3.26|
00001a  2101              MOVS     r1,#1                 ;189
00001c  7011              STRB     r1,[r2,#0]            ;189
00001e  e01f              B        |L3.96|
                  |L3.32|
000020  7b03              LDRB     r3,[r0,#0xc]
000022  2b83              CMP      r3,#0x83
000024  d102              BNE      |L3.44|
000026  7b43              LDRB     r3,[r0,#0xd]
000028  2b60              CMP      r3,#0x60
00002a  d002              BEQ      |L3.50|
                  |L3.44|
;;;192    	{
;;;193    		Lifter1.initState = 2;
;;;194    	}
;;;195    	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x84 && CanRxMsg->Data[2] == 0x60)//设置减速度
00002c  2960              CMP      r1,#0x60
00002e  d003              BEQ      |L3.56|
000030  e008              B        |L3.68|
                  |L3.50|
000032  2102              MOVS     r1,#2                 ;193
000034  7011              STRB     r1,[r2,#0]            ;193
000036  e013              B        |L3.96|
                  |L3.56|
000038  7b03              LDRB     r3,[r0,#0xc]
00003a  2b84              CMP      r3,#0x84
00003c  d102              BNE      |L3.68|
00003e  7b43              LDRB     r3,[r0,#0xd]
000040  2b60              CMP      r3,#0x60
000042  d002              BEQ      |L3.74|
                  |L3.68|
;;;196    	{
;;;197    		Lifter1.initState = 3;
;;;198    	}
;;;199    	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60)//使能
000044  2960              CMP      r1,#0x60
000046  d003              BEQ      |L3.80|
000048  e00a              B        |L3.96|
                  |L3.74|
00004a  2103              MOVS     r1,#3                 ;197
00004c  7011              STRB     r1,[r2,#0]            ;197
00004e  e007              B        |L3.96|
                  |L3.80|
000050  7b01              LDRB     r1,[r0,#0xc]
000052  2940              CMP      r1,#0x40
000054  d104              BNE      |L3.96|
000056  7b41              LDRB     r1,[r0,#0xd]
000058  2960              CMP      r1,#0x60
00005a  d101              BNE      |L3.96|
;;;200    	{
;;;201    		Lifter1.initState = 4;//初始化完成
00005c  2104              MOVS     r1,#4
00005e  7011              STRB     r1,[r2,#0]
                  |L3.96|
;;;202    	}
;;;203    	//读电机位置
;;;204    	if(CanRxMsg->Data[0] == 0x43 && CanRxMsg->Data[1] == 0x63 && CanRxMsg->Data[2] == 0x60)//
000060  7ac1              LDRB     r1,[r0,#0xb]
000062  2943              CMP      r1,#0x43
000064  d122              BNE      |L3.172|
000066  7b01              LDRB     r1,[r0,#0xc]
000068  2963              CMP      r1,#0x63
00006a  d11f              BNE      |L3.172|
00006c  7b41              LDRB     r1,[r0,#0xd]
00006e  2960              CMP      r1,#0x60
000070  d11c              BNE      |L3.172|
;;;205    	{
;;;206    		EncodeValue = CanRxMsg->Data[4]|CanRxMsg->Data[5]<<8|CanRxMsg->Data[6]<<16|CanRxMsg->Data[7]<<24;
000072  7bc1              LDRB     r1,[r0,#0xf]
000074  7c03              LDRB     r3,[r0,#0x10]
000076  ea412103          ORR      r1,r1,r3,LSL #8
00007a  7c43              LDRB     r3,[r0,#0x11]
00007c  7c80              LDRB     r0,[r0,#0x12]
00007e  041b              LSLS     r3,r3,#16
000080  ea436000          ORR      r0,r3,r0,LSL #24
000084  4301              ORRS     r1,r1,r0
000086  480b              LDR      r0,|L3.180|
000088  6241              STR      r1,[r0,#0x24]  ; EncodeValue
;;;207    		Lifter1.encodePos +=  (float)(EncodeValue - lastEncodeValue)/80000;
00008a  6a03              LDR      r3,[r0,#0x20]  ; lastEncodeValue
00008c  ed9f1a0a          VLDR     s2,|L3.184|
000090  1acb              SUBS     r3,r1,r3
000092  ee003a10          VMOV     s0,r3
000096  eef80ac0          VCVT.F32.S32 s1,s0
00009a  ee800a81          VDIV.F32 s0,s1,s2
00009e  edd20a03          VLDR     s1,[r2,#0xc]
0000a2  ee300a20          VADD.F32 s0,s0,s1
0000a6  ed820a03          VSTR     s0,[r2,#0xc]
;;;208    		lastEncodeValue = EncodeValue;
0000aa  6201              STR      r1,[r0,#0x20]  ; lastEncodeValue
                  |L3.172|
;;;209    	}
;;;210    }
0000ac  4770              BX       lr
;;;211    
                          ENDP

0000ae  0000              DCW      0x0000
                  |L3.176|
                          DCD      Lifter1
                  |L3.180|
                          DCD      ||area_number.15||
                  |L3.184|
0000b8  479c4000          DCFS     0x479c4000 ; 80000

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

                  RotateDataProcess PROC
;;;238    int EncodeValue = 0;
;;;239    void RotateDataProcess(CanRxMsg *CanRxMsg)
000000  b510              PUSH     {r4,lr}
;;;240    {
;;;241    	static u8 FirstFlag = 1;//第一次上电，角度置0
;;;242    	static int lastEncodeValue = 0;
;;;243    	if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x60 && CanRxMsg->Data[2] == 0x60)//设置速度模式
000002  7ac1              LDRB     r1,[r0,#0xb]
;;;244    	{
;;;245    		Rotate1.initState = 1;
000004  4a3b              LDR      r2,|L4.244|
000006  2960              CMP      r1,#0x60              ;243
000008  d105              BNE      |L4.22|
00000a  7b03              LDRB     r3,[r0,#0xc]          ;243
00000c  2b60              CMP      r3,#0x60              ;243
00000e  d102              BNE      |L4.22|
000010  7b43              LDRB     r3,[r0,#0xd]          ;243
000012  2b60              CMP      r3,#0x60              ;243
000014  d002              BEQ      |L4.28|
                  |L4.22|
;;;246    	}
;;;247    	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x83 && CanRxMsg->Data[2] == 0x60)//设置加速度
000016  2960              CMP      r1,#0x60
000018  d003              BEQ      |L4.34|
00001a  e008              B        |L4.46|
                  |L4.28|
00001c  2101              MOVS     r1,#1                 ;245
00001e  7011              STRB     r1,[r2,#0]            ;245
000020  e01f              B        |L4.98|
                  |L4.34|
000022  7b03              LDRB     r3,[r0,#0xc]
000024  2b83              CMP      r3,#0x83
000026  d102              BNE      |L4.46|
000028  7b43              LDRB     r3,[r0,#0xd]
00002a  2b60              CMP      r3,#0x60
00002c  d002              BEQ      |L4.52|
                  |L4.46|
;;;248    	{
;;;249    		Rotate1.initState = 2;
;;;250    	}
;;;251    	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x84 && CanRxMsg->Data[2] == 0x60)//设置减速度
00002e  2960              CMP      r1,#0x60
000030  d003              BEQ      |L4.58|
000032  e008              B        |L4.70|
                  |L4.52|
000034  2102              MOVS     r1,#2                 ;249
000036  7011              STRB     r1,[r2,#0]            ;249
000038  e013              B        |L4.98|
                  |L4.58|
00003a  7b03              LDRB     r3,[r0,#0xc]
00003c  2b84              CMP      r3,#0x84
00003e  d102              BNE      |L4.70|
000040  7b43              LDRB     r3,[r0,#0xd]
000042  2b60              CMP      r3,#0x60
000044  d002              BEQ      |L4.76|
                  |L4.70|
;;;252    	{
;;;253    		Rotate1.initState = 3;
;;;254    	}
;;;255    	else if(CanRxMsg->Data[0] == 0x60 && CanRxMsg->Data[1] == 0x40 && CanRxMsg->Data[2] == 0x60)//使能
000046  2960              CMP      r1,#0x60
000048  d003              BEQ      |L4.82|
00004a  e00a              B        |L4.98|
                  |L4.76|
00004c  2103              MOVS     r1,#3                 ;253
00004e  7011              STRB     r1,[r2,#0]            ;253
000050  e007              B        |L4.98|
                  |L4.82|
000052  7b01              LDRB     r1,[r0,#0xc]
000054  2940              CMP      r1,#0x40
000056  d104              BNE      |L4.98|
000058  7b41              LDRB     r1,[r0,#0xd]
00005a  2960              CMP      r1,#0x60
00005c  d101              BNE      |L4.98|
;;;256    	{
;;;257    		Rotate1.initState = 4;//初始化完成
00005e  2104              MOVS     r1,#4
000060  7011              STRB     r1,[r2,#0]
                  |L4.98|
;;;258    	}
;;;259    	//读电机位置
;;;260    	if(CanRxMsg->Data[0] == 0x43 && CanRxMsg->Data[1] == 0x63 && CanRxMsg->Data[2] == 0x60)//
000062  7ac1              LDRB     r1,[r0,#0xb]
000064  2943              CMP      r1,#0x43
000066  d143              BNE      |L4.240|
000068  7b01              LDRB     r1,[r0,#0xc]
00006a  2963              CMP      r1,#0x63
00006c  d140              BNE      |L4.240|
00006e  7b41              LDRB     r1,[r0,#0xd]
000070  2960              CMP      r1,#0x60
000072  d13d              BNE      |L4.240|
;;;261    	{
;;;262    		EncodeValue = CanRxMsg->Data[4]|CanRxMsg->Data[5]<<8|CanRxMsg->Data[6]<<16|CanRxMsg->Data[7]<<24;
000074  7bc1              LDRB     r1,[r0,#0xf]
000076  7c03              LDRB     r3,[r0,#0x10]
000078  4c1f              LDR      r4,|L4.248|
00007a  ea412103          ORR      r1,r1,r3,LSL #8
00007e  7c43              LDRB     r3,[r0,#0x11]
000080  7c80              LDRB     r0,[r0,#0x12]
000082  041b              LSLS     r3,r3,#16
000084  ea436000          ORR      r0,r3,r0,LSL #24
000088  4301              ORRS     r1,r1,r0
00008a  6061              STR      r1,[r4,#4]  ; EncodeValue
;;;263    		if(FirstFlag)//第一次上电，角度置0
00008c  7820              LDRB     r0,[r4,#0]  ; FirstFlag
00008e  b110              CBZ      r0,|L4.150|
;;;264    		{
;;;265    			FirstFlag = 0;
000090  2000              MOVS     r0,#0
000092  7020              STRB     r0,[r4,#0]
;;;266    			lastEncodeValue = EncodeValue;
000094  62a1              STR      r1,[r4,#0x28]  ; lastEncodeValue
                  |L4.150|
;;;267    		}
;;;268    		if(abs(EncodeValue - lastEncodeValue) > 100)
000096  6aa3              LDR      r3,[r4,#0x28]  ; lastEncodeValue
000098  1ac8              SUBS     r0,r1,r3
00009a  d500              BPL      |L4.158|
00009c  4240              RSBS     r0,r0,#0
                  |L4.158|
00009e  2864              CMP      r0,#0x64
0000a0  dd26              BLE      |L4.240|
;;;269    		{
;;;270    			Rotate1.encodeAngle +=  (float)(lastEncodeValue - EncodeValue)*360/2418000;
0000a2  1a58              SUBS     r0,r3,r1
0000a4  ee000a10          VMOV     s0,r0
0000a8  eddf0a14          VLDR     s1,|L4.252|
0000ac  eddf1a14          VLDR     s3,|L4.256|
0000b0  eeb80ac0          VCVT.F32.S32 s0,s0
0000b4  ee201a20          VMUL.F32 s2,s0,s1
0000b8  ee810a21          VDIV.F32 s0,s2,s3
0000bc  ed921a03          VLDR     s2,[r2,#0xc]
0000c0  ee300a01          VADD.F32 s0,s0,s2
0000c4  ed820a03          VSTR     s0,[r2,#0xc]
;;;271    			if(Rotate1.encodeAngle > 180)
0000c8  ee100a10          VMOV     r0,s0
0000cc  4b0d              LDR      r3,|L4.260|
0000ce  4298              CMP      r0,r3
0000d0  dd04              BLE      |L4.220|
;;;272    			{
;;;273    				Rotate1.encodeAngle -= 360;
0000d2  ee300a60          VSUB.F32 s0,s0,s1
0000d6  ed820a03          VSTR     s0,[r2,#0xc]
0000da  e008              B        |L4.238|
                  |L4.220|
;;;274    			}
;;;275    			else if(Rotate1.encodeAngle < -180)
0000dc  ee100a10          VMOV     r0,s0
0000e0  4b09              LDR      r3,|L4.264|
0000e2  4298              CMP      r0,r3
0000e4  d903              BLS      |L4.238|
;;;276    			{
;;;277    				Rotate1.encodeAngle += 360;
0000e6  ee300a20          VADD.F32 s0,s0,s1
0000ea  ed820a03          VSTR     s0,[r2,#0xc]
                  |L4.238|
;;;278    			}
;;;279    			lastEncodeValue = EncodeValue;	
0000ee  62a1              STR      r1,[r4,#0x28]  ; lastEncodeValue
                  |L4.240|
;;;280    		}
;;;281    	}
;;;282    }
0000f0  bd10              POP      {r4,pc}
;;;283    
                          ENDP

0000f2  0000              DCW      0x0000
                  |L4.244|
                          DCD      Rotate1
                  |L4.248|
                          DCD      ||area_number.15||
                  |L4.252|
0000fc  43b40000          DCFS     0x43b40000 ; 360
                  |L4.256|
000100  4a139540          DCFS     0x4a139540 ; 2418000
                  |L4.260|
                          DCD      0x43340000
                  |L4.264|
                          DCD      0xc3340000

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

                  _AppCanUpdate PROC
;;;283    
;;;284    void _AppCanUpdate(void)
000000  b570              PUSH     {r4-r6,lr}
;;;285    {
;;;286    	static int i_First = 1,i_Count = 0,lastTime = 0,lastTime1 = 0,lastTime2 = 0,lastTime3 = 0;
;;;287    /****************************行走驱动************************************************/
;;;288    #if P_SETUP_MOTOR_NUM > 0
;;;289        DriverFunction(&DriverMotor1); //创建行走驱动实例
;;;290    #endif
;;;291    
;;;292    #if P_SETUP_MOTOR_NUM > 1
;;;293        DriverFunction(&DriverMotor2);
;;;294        os_dly_wait(1);
;;;295    #endif
;;;296    
;;;297    #if P_SETUP_MOTOR_NUM > 2
;;;298        DriverFunction(&DriverMotor3);
;;;299    #endif
;;;300    
;;;301    #if P_SETUP_MOTOR_NUM > 3
;;;302        DriverFunction(&DriverMotor4);
;;;303        os_dly_wait(1);
;;;304    #endif
;;;305    
;;;306    #if P_SETUP_MOTOR_NUM > 0
;;;307        os_dly_wait(1);
;;;308        DriverState(&DriverMotor1, 10.0f); //创建行走读状态实例 保证发送速度指令之间无延迟
;;;309    #endif
;;;310    
;;;311    #if P_SETUP_MOTOR_NUM > 1
;;;312        os_dly_wait(1);
;;;313        DriverState(&DriverMotor2, 10.0f);
;;;314    #endif
;;;315    
;;;316    #if P_SETUP_MOTOR_NUM > 2
;;;317        os_dly_wait(1);
;;;318        DriverState(&DriverMotor3, 10.0f);
;;;319    #endif
;;;320    
;;;321    #if P_SETUP_MOTOR_NUM > 3
;;;322        os_dly_wait(1);
;;;323        DriverState(&DriverMotor4, 10.0f);
;;;324    #endif
;;;325    
;;;326        /****************************转向驱动************************************************/
;;;327    #if P_SETUP_STEERING_NUM > 0
;;;328        steeringFunction(&DriverSteering1, &DriverMotor1); //创建转向驱动实例
;;;329        os_dly_wait(1);
;;;330    #endif
;;;331    
;;;332    #if P_SETUP_STEERING_NUM > 1
;;;333        steeringFunction(&DriverSteering2, &DriverMotor2);
;;;334    #endif
;;;335    
;;;336    #if P_SETUP_STEERING_NUM > 2
;;;337        steeringFunction(&DriverSteering3, &DriverMotor3);
;;;338        os_dly_wait(1);
;;;339    #endif
;;;340    
;;;341    #if P_SETUP_STEERING_NUM > 3
;;;342        steeringFunction(&DriverSteering4, &DriverMotor4);
;;;343    #endif
;;;344    
;;;345    #if P_SETUP_STEERING_NUM > 0
;;;346        os_dly_wait(1);
;;;347        DriverState(&DriverSteering1, 0.01f); //创建转向驱动实例
;;;348    #endif
;;;349    
;;;350    #if P_SETUP_STEERING_NUM > 1
;;;351        os_dly_wait(1);
;;;352        DriverState(&DriverSteering2, 0.01f);
;;;353    #endif
;;;354    
;;;355    #if P_SETUP_STEERING_NUM > 2
;;;356        os_dly_wait(1);
;;;357        DriverState(&DriverSteering3, 0.01f);
;;;358    #endif
;;;359    
;;;360    #if P_SETUP_STEERING_NUM > 3
;;;361        os_dly_wait(1);
;;;362        DriverState(&DriverSteering4, 0.01f);
;;;363    #endif
;;;364        /****************************举升驱动************************************************/
;;;365    #if P_SETUP_LIFTER_NUM > 0
;;;366        os_dly_wait(1);
;;;367        DriverFunction(&DriverLifter1); //创建举升驱动实例
;;;368    #endif
;;;369    
;;;370    #if P_SETUP_LIFTER_NUM > 1
;;;371        os_dly_wait(1);
;;;372        DriverFunction(&DriverLifter2);
;;;373    #endif
;;;374    
;;;375    #if P_SETUP_LIFTER_NUM > 2
;;;376        os_dly_wait(1);
;;;377        DriverFunction(&DriverLifter3);
;;;378    #endif
;;;379    
;;;380    #if P_SETUP_LIFTER_NUM > 3
;;;381        os_dly_wait(1);
;;;382        DriverFunction(&DriverLifter4);
;;;383    #endif
;;;384    
;;;385    #if P_SETUP_LIFTER_NUM > 0
;;;386        os_dly_wait(1);
;;;387        DriverState(&DriverLifter1, 10.0f); //创建举升驱动实例
;;;388    #endif
;;;389    
;;;390    #if P_SETUP_LIFTER_NUM > 1
;;;391        os_dly_wait(1);
;;;392        DriverState(&DriverLifter2, 10.0f);
;;;393    #endif
;;;394    
;;;395    #if P_SETUP_LIFTER_NUM > 2
;;;396        os_dly_wait(1);
;;;397        DriverState(&DriverLifter3, 10.0f);
;;;398    #endif
;;;399    
;;;400    #if P_SETUP_LIFTER_NUM > 3
;;;401        os_dly_wait(1);
;;;402        DriverState(&DriverLifter4, 10.0f);
;;;403    #endif
;;;404        /****************************移轴驱动************************************************/
;;;405    #if P_SETUP_SHIFTER_NUM > 0
;;;406        os_dly_wait(1);
;;;407        DriverFunction(&DriverShifter1); //创建移轴驱动实例
;;;408    #endif
;;;409    
;;;410    #if P_SETUP_SHIFTER_NUM > 1
;;;411        os_dly_wait(1);
;;;412        DriverFunction(&DriverShifter2);
;;;413    #endif
;;;414    
;;;415    #if P_SETUP_SHIFTER_NUM > 2
;;;416        os_dly_wait(1);
;;;417        DriverFunction(&DriverShifter3);
;;;418    #endif
;;;419    
;;;420    #if P_SETUP_SHIFTER_NUM > 0
;;;421        os_dly_wait(1);
;;;422        DriverState(&DriverShifter1, 10.0f); //创建移轴读状态实例
;;;423    #endif
;;;424    
;;;425    #if P_SETUP_SHIFTER_NUM > 1
;;;426        os_dly_wait(1);
;;;427        DriverState(&DriverShifter2, 10.0f);
;;;428    #endif
;;;429    
;;;430    #if P_SETUP_SHIFTER_NUM > 2
;;;431        os_dly_wait(1);
;;;432        DriverState(&DriverShifter3, 10.0f);
;;;433    #endif
;;;434    
;;;435        /****************************推杆驱动************************************************/
;;;436    #if P_SETUP_PUSHER_NUM > 0
;;;437        os_dly_wait(1);
;;;438        DriverFunction(&DriverPusher1); //创建推杆驱动实例
;;;439    #endif
;;;440    
;;;441    #if P_SETUP_PUSHER_NUM > 1
;;;442        os_dly_wait(1);
;;;443        DriverFunction(&DriverPusher2);
;;;444    #endif
;;;445    
;;;446    #if P_SETUP_PUSHER_NUM > 2
;;;447        os_dly_wait(1);
;;;448        DriverFunction(&DriverPusher3);
;;;449    #endif
;;;450    
;;;451    #if P_SETUP_PUSHER_NUM > 0
;;;452        os_dly_wait(1);
;;;453        DriverState(&DriverPusher1, 10.0f); //创建推杆读状态实例
;;;454    #endif
;;;455    
;;;456    #if P_SETUP_PUSHER_NUM > 1
;;;457        os_dly_wait(1);
;;;458        DriverState(&DriverPusher2, 10.0f);
;;;459    #endif
;;;460    
;;;461    #if P_SETUP_PUSHER_NUM > 2
;;;462        os_dly_wait(1);
;;;463        DriverState(&DriverPusher3, 10.0f);
;;;464    #endif
;;;465    
;;;466    #if P_SETUP_BATTERYTYPE == 1
;;;467        SendBatteryData();
;;;468        os_dly_wait(1);
;;;469    #endif
;;;470    		if(i_First == 1)
000002  4c27              LDR      r4,|L5.160|
;;;471    		{
;;;472    			if(i_Count++ >3000)
;;;473    			{
;;;474    				Music_Select(Music_Start, agv.Command.Vol);
000004  4d27              LDR      r5,|L5.164|
000006  6ae1              LDR      r1,[r4,#0x2c]         ;470  ; i_First
000008  89a8              LDRH     r0,[r5,#0xc]
00000a  2901              CMP      r1,#1                 ;470
00000c  d032              BEQ      |L5.116|
;;;475    			}
;;;476    			if(i_Count++ >10000)
;;;477    			{
;;;478    				i_First = 0;
;;;479    			}
;;;480    			
;;;481    		}
;;;482    		else
;;;483    			Music_Select(agv.Command.MusicFlag, agv.Command.Vol);
00000e  b2c1              UXTB     r1,r0
000010  7a68              LDRB     r0,[r5,#9]  ; agv
000012  f7fffffe          BL       Music_Select
                  |L5.22|
;;;484    		sendKincoSpeed();
000016  f7fffffe          BL       sendKincoSpeed
;;;485    		UpdateCameraData();
00001a  f7fffffe          BL       UpdateCameraData
;;;486    //		UpdateCameraData2();
;;;487    		DecodeIMUdata();//获取惯导数据
00001e  f7fffffe          BL       DecodeIMUdata
;;;488    
;;;489    		if(agv.Public.SystemTime - lastTime > 9)
000022  f8d500bc          LDR      r0,[r5,#0xbc]  ; agv
000026  6b61              LDR      r1,[r4,#0x34]  ; lastTime
000028  1a41              SUBS     r1,r0,r1
00002a  2909              CMP      r1,#9
00002c  d902              BLS      |L5.52|
;;;490    		{
;;;491    			lastTime = agv.Public.SystemTime;
00002e  6360              STR      r0,[r4,#0x34]  ; lastTime
;;;492    			sendLiftSpeed();
000030  f7fffffe          BL       sendLiftSpeed
                  |L5.52|
;;;493    		}
;;;494    		
;;;495    		if(agv.Public.SystemTime - lastTime1 > 11)
000034  f8d500bc          LDR      r0,[r5,#0xbc]  ; agv
000038  6ba1              LDR      r1,[r4,#0x38]  ; lastTime1
00003a  1a41              SUBS     r1,r0,r1
00003c  290b              CMP      r1,#0xb
00003e  d908              BLS      |L5.82|
;;;496    		{
;;;497    			lastTime1 = agv.Public.SystemTime;
000040  63a0              STR      r0,[r4,#0x38]  ; lastTime1
;;;498    			CAN1_Send_Msg(0x603,readPos,8,2);
000042  4917              LDR      r1,|L5.160|
000044  2302              MOVS     r3,#2
000046  2208              MOVS     r2,#8
000048  3168              ADDS     r1,r1,#0x68
00004a  f2406003          MOV      r0,#0x603
00004e  f7fffffe          BL       CAN1_Send_Msg
                  |L5.82|
;;;499    		}
;;;500    		
;;;501    		
;;;502    		if(agv.Public.SystemTime - lastTime3 > 13)
000052  f8d500bc          LDR      r0,[r5,#0xbc]  ; agv
000056  6be1              LDR      r1,[r4,#0x3c]  ; lastTime3
000058  1a41              SUBS     r1,r0,r1
00005a  290d              CMP      r1,#0xd
00005c  d91f              BLS      |L5.158|
;;;503    		{
;;;504    			lastTime3 = agv.Public.SystemTime;
00005e  63e0              STR      r0,[r4,#0x3c]  ; lastTime3
;;;505    			CAN1_Send_Msg(0x604,readPos,8,2);
000060  e8bd4070          POP      {r4-r6,lr}
000064  490e              LDR      r1,|L5.160|
000066  2302              MOVS     r3,#2
000068  2208              MOVS     r2,#8
00006a  3168              ADDS     r1,r1,#0x68
00006c  f2406004          MOV      r0,#0x604
000070  f7ffbffe          B.W      CAN1_Send_Msg
                  |L5.116|
000074  6b21              LDR      r1,[r4,#0x30]         ;472  ; i_Count
000076  1c4a              ADDS     r2,r1,#1              ;472
000078  6322              STR      r2,[r4,#0x30]         ;472  ; i_Count
00007a  f64032b8          MOV      r2,#0xbb8             ;472
00007e  4291              CMP      r1,r2                 ;472
000080  dd03              BLE      |L5.138|
000082  b2c1              UXTB     r1,r0                 ;474
000084  200e              MOVS     r0,#0xe               ;474
000086  f7fffffe          BL       Music_Select
                  |L5.138|
00008a  6b20              LDR      r0,[r4,#0x30]         ;476  ; i_Count
00008c  1c41              ADDS     r1,r0,#1              ;476
00008e  6321              STR      r1,[r4,#0x30]         ;476  ; i_Count
000090  f2427110          MOV      r1,#0x2710            ;476
000094  4288              CMP      r0,r1                 ;476
000096  ddbe              BLE      |L5.22|
000098  2000              MOVS     r0,#0                 ;478
00009a  62e0              STR      r0,[r4,#0x2c]         ;478  ; i_First
00009c  e7bb              B        |L5.22|
                  |L5.158|
;;;506    		}
;;;507    
;;;508    }
00009e  bd70              POP      {r4-r6,pc}
                          ENDP

                  |L5.160|
                          DCD      ||area_number.15||
                  |L5.164|
                          DCD      agv

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

                  sendKincoSpeed PROC
;;;68     
;;;69     void sendKincoSpeed()
000000  e92d4ff0          PUSH     {r4-r11,lr}
;;;70     {
000004  ed2d8b06          VPUSH    {d8-d10}
000008  b081              SUB      sp,sp,#4
;;;71     	static int lastTime1 = 0,lastTime2 = 0;
;;;72     	static u8 data1[8] = {0};
;;;73     	static u8 data2[8] = {0};
;;;74     	static float curSpeed1 = 0,curSpeed2 = 0;
;;;75     	static float speedSlope1 = 0,speedSlope2 = 0;
;;;76     //	if(agv.Command.CurDirection == 3 || agv.Command.HandMotorState == 5 || agv.Command.CurDirection == 4 || agv.Command.HandMotorState == 6)//旋转
;;;77     //	{
;;;78     //		if(fabs(KincoStruct1.setSpeed) > fabs(curSpeed1))
;;;79     //			speedSlope1 = 0.8;
;;;80     //		else
;;;81     //			speedSlope1 = 0.8;
;;;82     //		
;;;83     //		if(fabs(KincoStruct2.setSpeed) > fabs(curSpeed2))
;;;84     //			speedSlope2 = 0.8;
;;;85     //		else
;;;86     //			speedSlope2 = 0.8;
;;;87     //	}
;;;88     //	else//直行
;;;89     	{
;;;90     		if(fabs(KincoStruct1.setSpeed) > fabs(curSpeed1))
;;;91     			speedSlope1 = 0.8;
00000a  ed9f9a85          VLDR     s18,|L6.544|
00000e  4f85              LDR      r7,|L6.548|
000010  4e85              LDR      r6,|L6.552|
;;;92     		else
;;;93     			speedSlope1 = 1.5;
;;;94     		
;;;95     		if(fabs(KincoStruct2.setSpeed) > fabs(curSpeed2))
000012  f8df8218          LDR      r8,|L6.556|
000016  eef79a08          VMOV.F32 s19,#1.50000000       ;93
00001a  6930              LDR      r0,[r6,#0x10]         ;90  ; curSpeed1
00001c  f7fffffe          BL       __aeabi_f2d
000020  ec410b10          VMOV     d0,r0,r1              ;90
000024  f7fffffe          BL       __hardfp_fabs
000028  eeb08a40          VMOV.F32 s16,s0                ;90
00002c  eef08a60          VMOV.F32 s17,s1                ;90
000030  6838              LDR      r0,[r7,#0]            ;90  ; KincoStruct1
000032  f7fffffe          BL       __aeabi_f2d
000036  ec410b10          VMOV     d0,r0,r1              ;90
00003a  f7fffffe          BL       __hardfp_fabs
00003e  eeb01a40          VMOV.F32 s2,s0                 ;90
000042  eef01a60          VMOV.F32 s3,s1                 ;90
000046  ec532b18          VMOV     r2,r3,d8              ;90
00004a  ec510b11          VMOV     r0,r1,d1              ;90
00004e  f7fffffe          BL       __aeabi_cdrcmple
000052  d202              BCS      |L6.90|
000054  ed869a06          VSTR     s18,[r6,#0x18]        ;91
000058  e001              B        |L6.94|
                  |L6.90|
00005a  edc69a06          VSTR     s19,[r6,#0x18]        ;93
                  |L6.94|
00005e  6970              LDR      r0,[r6,#0x14]  ; curSpeed2
000060  f7fffffe          BL       __aeabi_f2d
000064  ec410b10          VMOV     d0,r0,r1
000068  f7fffffe          BL       __hardfp_fabs
00006c  eeb08a40          VMOV.F32 s16,s0
000070  eef08a60          VMOV.F32 s17,s1
000074  f8d80000          LDR      r0,[r8,#0]  ; KincoStruct2
000078  f7fffffe          BL       __aeabi_f2d
00007c  ec410b10          VMOV     d0,r0,r1
000080  f7fffffe          BL       __hardfp_fabs
000084  eeb01a40          VMOV.F32 s2,s0
000088  eef01a60          VMOV.F32 s3,s1
00008c  ec532b18          VMOV     r2,r3,d8
000090  ec510b11          VMOV     r0,r1,d1
000094  f7fffffe          BL       __aeabi_cdrcmple
000098  d202              BCS      |L6.160|
;;;96     			speedSlope2 = 0.8;
00009a  ed869a07          VSTR     s18,[r6,#0x1c]
00009e  e001              B        |L6.164|
                  |L6.160|
;;;97     		else
;;;98     			speedSlope2 = 1.5;
0000a0  edc69a07          VSTR     s19,[r6,#0x1c]
                  |L6.164|
;;;99     	}
;;;100    	
;;;101    	KincoStruct1.setSpeed = GetSpeedSlope(speedSlope1,curSpeed1,KincoStruct1.setSpeed);
0000a4  ed971a00          VLDR     s2,[r7,#0]
0000a8  edd60a04          VLDR     s1,[r6,#0x10]
0000ac  ed960a06          VLDR     s0,[r6,#0x18]
0000b0  f7fffffe          BL       GetSpeedSlope
0000b4  ed870a00          VSTR     s0,[r7,#0]
;;;102    	curSpeed1 = KincoStruct1.setSpeed;
0000b8  ed860a04          VSTR     s0,[r6,#0x10]
;;;103    	
;;;104    	KincoStruct2.setSpeed = GetSpeedSlope(speedSlope2,curSpeed2,KincoStruct2.setSpeed);
0000bc  ed981a00          VLDR     s2,[r8,#0]
0000c0  edd60a05          VLDR     s1,[r6,#0x14]
0000c4  ed960a07          VLDR     s0,[r6,#0x1c]
0000c8  f7fffffe          BL       GetSpeedSlope
0000cc  ee105a10          VMOV     r5,s0
0000d0  ed880a00          VSTR     s0,[r8,#0]
;;;105    	curSpeed2 = KincoStruct2.setSpeed;
0000d4  6175              STR      r5,[r6,#0x14]  ; curSpeed2
;;;106    	
;;;107    	int speed1 = (int)(KincoStruct1.setSpeed*17895.42*8/PI*1);
0000d6  ed978a00          VLDR     s16,[r7,#0]
0000da  ee180a10          VMOV     r0,s16
0000de  f7fffffe          BL       __aeabi_f2d
0000e2  ed9f9b53          VLDR     d9,|L6.560|
0000e6  ec532b19          VMOV     r2,r3,d9
0000ea  f7fffffe          BL       __aeabi_dmul
0000ee  ed9f1b52          VLDR     d1,|L6.568|
0000f2  ec532b11          VMOV     r2,r3,d1
0000f6  f7fffffe          BL       __aeabi_dmul
0000fa  ed9fab51          VLDR     d10,|L6.576|
0000fe  ec532b1a          VMOV     r2,r3,d10
000102  f7fffffe          BL       __aeabi_ddiv
000106  f7fffffe          BL       __aeabi_d2iz
00010a  4604              MOV      r4,r0
;;;108    	int speed2 = (int)(KincoStruct2.setSpeed*17895.42*-8/PI);
00010c  4628              MOV      r0,r5
00010e  f7fffffe          BL       __aeabi_f2d
000112  ec532b19          VMOV     r2,r3,d9
000116  f7fffffe          BL       __aeabi_dmul
00011a  ed9f1b4b          VLDR     d1,|L6.584|
00011e  ec532b11          VMOV     r2,r3,d1
000122  f7fffffe          BL       __aeabi_dmul
000126  ec532b1a          VMOV     r2,r3,d10
00012a  f7fffffe          BL       __aeabi_ddiv
00012e  f7fffffe          BL       __aeabi_d2iz
000132  4605              MOV      r5,r0
;;;109    	//左电机
;;;110    	if(agv.Public.SystemTime - lastTime1 > 5)
000134  f8df9118          LDR      r9,|L6.592|
000138  68b1              LDR      r1,[r6,#8]  ; lastTime1
;;;111    	{
;;;112    		if(KincoStruct1.setSpeed == 0&&KincoStruct1.encodeSpeed == 0)
;;;113    		{
;;;114    			KincoStruct1.enable = 0x06;
00013a  f04f0a06          MOV      r10,#6
00013e  f8d900bc          LDR      r0,[r9,#0xbc]         ;110  ; agv
;;;115    		}
;;;116    		else
;;;117    			KincoStruct1.enable = 0x0F;
000142  f04f0b0f          MOV      r11,#0xf
000146  1a41              SUBS     r1,r0,r1              ;110
000148  2905              CMP      r1,#5                 ;110
00014a  d92e              BLS      |L6.426|
00014c  eeb58ac0          VCMPE.F32 s16,#0.0              ;112
000150  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;112
000154  d109              BNE      |L6.362|
000156  ed970a01          VLDR     s0,[r7,#4]            ;112
00015a  eeb50ac0          VCMPE.F32 s0,#0.0               ;112
00015e  eef1fa10          VMRS     APSR_nzcv,FPSCR       ;112
000162  d102              BNE      |L6.362|
000164  f8a7a00c          STRH     r10,[r7,#0xc]         ;114
000168  e001              B        |L6.366|
                  |L6.362|
00016a  f8a7b00c          STRH     r11,[r7,#0xc]
                  |L6.366|
;;;118    		lastTime1 = agv.Public.SystemTime;
00016e  60b0              STR      r0,[r6,#8]  ; lastTime1
;;;119    		data1[0] = -3;
000170  482d              LDR      r0,|L6.552|
000172  21fd              MOVS     r1,#0xfd
000174  3070              ADDS     r0,r0,#0x70
000176  7001              STRB     r1,[r0,#0]
;;;120    		data1[1] = KincoStruct1.enable&0xff;
000178  89b9              LDRH     r1,[r7,#0xc]  ; KincoStruct1
00017a  7041              STRB     r1,[r0,#1]
;;;121    		data1[2] = (KincoStruct1.enable>>8)&0xff;
00017c  0a09              LSRS     r1,r1,#8
00017e  7081              STRB     r1,[r0,#2]
;;;122    		data1[3] = speed1&0xff;
000180  70c4              STRB     r4,[r0,#3]
;;;123    		data1[4] = (speed1>>8)&0xff;
000182  0a21              LSRS     r1,r4,#8
000184  7101              STRB     r1,[r0,#4]
;;;124    		data1[5] = (speed1>>16)&0xff;
000186  0c21              LSRS     r1,r4,#16
000188  7141              STRB     r1,[r0,#5]
;;;125    		data1[6] = (speed1>>24)&0xff;
00018a  0e21              LSRS     r1,r4,#24
00018c  7181              STRB     r1,[r0,#6]
;;;126    		data1[7] = 0;
00018e  2100              MOVS     r1,#0
000190  71c1              STRB     r1,[r0,#7]
;;;127    		CAN_Send_Msg(0x201,data1,8,0,1);
000192  2001              MOVS     r0,#1
000194  4924              LDR      r1,|L6.552|
000196  9000              STR      r0,[sp,#0]
000198  2300              MOVS     r3,#0
00019a  2208              MOVS     r2,#8
00019c  3170              ADDS     r1,r1,#0x70
00019e  f2402001          MOV      r0,#0x201
0001a2  f7fffffe          BL       CAN_Send_Msg
;;;128    		
;;;129    		sendRotateSpeed();
0001a6  f7fffffe          BL       sendRotateSpeed
                  |L6.426|
;;;130    		
;;;131    	}
;;;132    	
;;;133    	
;;;134    	if(agv.Public.SystemTime - lastTime2 > 7)
0001aa  f8d900bc          LDR      r0,[r9,#0xbc]  ; agv
0001ae  68f1              LDR      r1,[r6,#0xc]  ; lastTime2
0001b0  1a41              SUBS     r1,r0,r1
0001b2  2907              CMP      r1,#7
0001b4  d92f              BLS      |L6.534|
;;;135    	{
;;;136    		if(KincoStruct2.setSpeed == 0&&KincoStruct2.encodeSpeed == 0)
0001b6  ed980a00          VLDR     s0,[r8,#0]
0001ba  eeb50ac0          VCMPE.F32 s0,#0.0
0001be  eef1fa10          VMRS     APSR_nzcv,FPSCR
0001c2  d109              BNE      |L6.472|
0001c4  ed980a01          VLDR     s0,[r8,#4]
0001c8  eeb50ac0          VCMPE.F32 s0,#0.0
0001cc  eef1fa10          VMRS     APSR_nzcv,FPSCR
0001d0  d102              BNE      |L6.472|
;;;137    		{
;;;138    			KincoStruct2.enable = 0x06;
0001d2  f8a8a00c          STRH     r10,[r8,#0xc]
0001d6  e001              B        |L6.476|
                  |L6.472|
;;;139    		}
;;;140    		else
;;;141    			KincoStruct2.enable = 0x0F;
0001d8  f8a8b00c          STRH     r11,[r8,#0xc]
                  |L6.476|
;;;142    		lastTime2 = agv.Public.SystemTime;
0001dc  60f0              STR      r0,[r6,#0xc]  ; lastTime2
;;;143    		//右电机
;;;144    		data2[0] = -3;
0001de  4812              LDR      r0,|L6.552|
0001e0  21fd              MOVS     r1,#0xfd
0001e2  3078              ADDS     r0,r0,#0x78
0001e4  7001              STRB     r1,[r0,#0]
;;;145    		data2[1] = KincoStruct2.enable&0xff;
0001e6  f8b8100c          LDRH     r1,[r8,#0xc]  ; KincoStruct2
0001ea  7041              STRB     r1,[r0,#1]
;;;146    		data2[2] = (KincoStruct2.enable>>8)&0xff;
0001ec  0a09              LSRS     r1,r1,#8
0001ee  7081              STRB     r1,[r0,#2]
;;;147    		data2[3] = speed2&0xff;
0001f0  70c5              STRB     r5,[r0,#3]
;;;148    		data2[4] = (speed2>>8)&0xff;
0001f2  0a29              LSRS     r1,r5,#8
0001f4  7101              STRB     r1,[r0,#4]
;;;149    		data2[5] = (speed2>>16)&0xff;
0001f6  0c29              LSRS     r1,r5,#16
0001f8  7141              STRB     r1,[r0,#5]
;;;150    		data2[6] = (speed2>>24)&0xff;
0001fa  0e29              LSRS     r1,r5,#24
0001fc  7181              STRB     r1,[r0,#6]
;;;151    		data2[7] = 0;
0001fe  2100              MOVS     r1,#0
000200  71c1              STRB     r1,[r0,#7]
;;;152    		CAN_Send_Msg(0x202,data2,8,0,1);
000202  2001              MOVS     r0,#1
000204  4908              LDR      r1,|L6.552|
000206  9000              STR      r0,[sp,#0]
000208  2300              MOVS     r3,#0
00020a  2208              MOVS     r2,#8
00020c  3178              ADDS     r1,r1,#0x78
00020e  f2402002          MOV      r0,#0x202
000212  f7fffffe          BL       CAN_Send_Msg
                  |L6.534|
;;;153    		
;;;154    	}
;;;155    }
000216  b001              ADD      sp,sp,#4
000218  ecbd8b06          VPOP     {d8-d10}
00021c  e8bd8ff0          POP      {r4-r11,pc}
;;;156    
                          ENDP

                  |L6.544|
000220  3f4ccccd          DCFS     0x3f4ccccd ; 0.80000001192092896
                  |L6.548|
                          DCD      KincoStruct1
                  |L6.552|
                          DCD      ||area_number.15||
                  |L6.556|
                          DCD      KincoStruct2
                  |L6.560|
000230  e147ae14          DCFD     0x40d179dae147ae14 ; 17895.419999999998
000234  40d179da
                  |L6.568|
000238  00000000          DCFD     0x4020000000000000 ; 8
00023c  40200000
                  |L6.576|
000240  40000000          DCFD     0x400921fb40000000 ; 3.1415925025939941
000244  400921fb
                  |L6.584|
000248  00000000          DCFD     0xc020000000000000 ; -8
00024c  c0200000
                  |L6.592|
                          DCD      agv

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

                  sendLiftSpeed PROC
;;;157    extern void change_data(u8 *_data, int data);
;;;158    void sendLiftSpeed()
000000  b510              PUSH     {r4,lr}
;;;159    {
;;;160    	change_data(setSpeed,(int)Lifter1.setSpeed);
000002  4c1d              LDR      r4,|L7.120|
000004  481d              LDR      r0,|L7.124|
000006  ed940a01          VLDR     s0,[r4,#4]
00000a  eebd0ac0          VCVT.S32.F32 s0,s0
00000e  ee101a10          VMOV     r1,s0
000012  f7fffffe          BL       change_data
;;;161    	//第一次进入速度模式
;;;162    	if(Lifter1.initState == 0)
000016  7821              LDRB     r1,[r4,#0]  ; Lifter1
;;;163    	{
;;;164    		CAN1_Send_Msg(0x603,speedModel,8,2);
000018  f2406003          MOV      r0,#0x603
00001c  b161              CBZ      r1,|L7.56|
;;;165    	}
;;;166    	else if(Lifter1.initState == 1)
00001e  2901              CMP      r1,#1
000020  d012              BEQ      |L7.72|
;;;167    	{
;;;168    		CAN1_Send_Msg(0x603,setSpeedSlope1,8,2);
;;;169    	}
;;;170    	else if(Lifter1.initState == 2)
000022  2902              CMP      r1,#2
000024  d018              BEQ      |L7.88|
;;;171    	{
;;;172    		CAN1_Send_Msg(0x603,setSpeedSlope2,8,2);
;;;173    	}
;;;174    	else if(Lifter1.initState == 3)
000026  2903              CMP      r1,#3
000028  d01e              BEQ      |L7.104|
;;;175    	{
;;;176    		CAN1_Send_Msg(0x603,enable,8,2);
;;;177    	}
;;;178    	else//初始化完成，发送目标速度
;;;179    	{
;;;180    		CAN1_Send_Msg(0x603,setSpeed,8,2);
00002a  2302              MOVS     r3,#2
00002c  e8bd4010          POP      {r4,lr}
000030  2208              MOVS     r2,#8
000032  4912              LDR      r1,|L7.124|
000034  f7ffbffe          B.W      CAN1_Send_Msg
                  |L7.56|
000038  4910              LDR      r1,|L7.124|
00003a  2302              MOVS     r3,#2                 ;164
00003c  2208              MOVS     r2,#8                 ;164
00003e  e8bd4010          POP      {r4,lr}               ;164
000042  3908              SUBS     r1,r1,#8              ;164
000044  f7ffbffe          B.W      CAN1_Send_Msg
                  |L7.72|
000048  490c              LDR      r1,|L7.124|
00004a  2302              MOVS     r3,#2                 ;168
00004c  2208              MOVS     r2,#8                 ;168
00004e  e8bd4010          POP      {r4,lr}               ;168
000052  3920              SUBS     r1,r1,#0x20           ;168
000054  f7ffbffe          B.W      CAN1_Send_Msg
                  |L7.88|
000058  4908              LDR      r1,|L7.124|
00005a  2302              MOVS     r3,#2                 ;172
00005c  2208              MOVS     r2,#8                 ;172
00005e  e8bd4010          POP      {r4,lr}               ;172
000062  3918              SUBS     r1,r1,#0x18           ;172
000064  f7ffbffe          B.W      CAN1_Send_Msg
                  |L7.104|
000068  4904              LDR      r1,|L7.124|
00006a  2302              MOVS     r3,#2                 ;176
00006c  2208              MOVS     r2,#8                 ;176
00006e  e8bd4010          POP      {r4,lr}               ;176
000072  3910              SUBS     r1,r1,#0x10           ;176
000074  f7ffbffe          B.W      CAN1_Send_Msg
;;;181    	}
;;;182    }
;;;183    
                          ENDP

                  |L7.120|
                          DCD      Lifter1
                  |L7.124|
                          DCD      ||area_number.15||+0x60

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

                  sendRotateSpeed PROC
;;;211    
;;;212    void sendRotateSpeed()
000000  b510              PUSH     {r4,lr}
;;;213    {
;;;214    	change_data(setSpeed,(int)Rotate1.setSpeed);
000002  4c1d              LDR      r4,|L8.120|
000004  481d              LDR      r0,|L8.124|
000006  ed940a01          VLDR     s0,[r4,#4]
00000a  eebd0ac0          VCVT.S32.F32 s0,s0
00000e  ee101a10          VMOV     r1,s0
000012  f7fffffe          BL       change_data
;;;215    	//第一次进入速度模式
;;;216    	if(Rotate1.initState == 0)
000016  7821              LDRB     r1,[r4,#0]  ; Rotate1
;;;217    	{
;;;218    		CAN1_Send_Msg(0x603,speedModel,8,2);
000018  f2406003          MOV      r0,#0x603
00001c  b161              CBZ      r1,|L8.56|
;;;219    	}
;;;220    	else if(Rotate1.initState == 1)
00001e  2901              CMP      r1,#1
000020  d012              BEQ      |L8.72|
;;;221    	{
;;;222    		CAN1_Send_Msg(0x603,setSpeedSlope1,8,2);
;;;223    	}
;;;224    	else if(Rotate1.initState == 2)
000022  2902              CMP      r1,#2
000024  d018              BEQ      |L8.88|
;;;225    	{
;;;226    		CAN1_Send_Msg(0x603,setSpeedSlope2,8,2);
;;;227    	}
;;;228    	else if(Rotate1.initState == 3)
000026  2903              CMP      r1,#3
000028  d01e              BEQ      |L8.104|
;;;229    	{
;;;230    		CAN1_Send_Msg(0x603,enable,8,2);
;;;231    	}
;;;232    	else//初始化完成，发送目标速度2418367
;;;233    	{
;;;234    		CAN1_Send_Msg(0x603,setSpeed,8,2);
00002a  2302              MOVS     r3,#2
00002c  e8bd4010          POP      {r4,lr}
000030  2208              MOVS     r2,#8
000032  4912              LDR      r1,|L8.124|
000034  f7ffbffe          B.W      CAN1_Send_Msg
                  |L8.56|
000038  4910              LDR      r1,|L8.124|
00003a  2302              MOVS     r3,#2                 ;218
00003c  2208              MOVS     r2,#8                 ;218
00003e  e8bd4010          POP      {r4,lr}               ;218
000042  3908              SUBS     r1,r1,#8              ;218
000044  f7ffbffe          B.W      CAN1_Send_Msg
                  |L8.72|
000048  490c              LDR      r1,|L8.124|
00004a  2302              MOVS     r3,#2                 ;222
00004c  2208              MOVS     r2,#8                 ;222
00004e  e8bd4010          POP      {r4,lr}               ;222
000052  3920              SUBS     r1,r1,#0x20           ;222
000054  f7ffbffe          B.W      CAN1_Send_Msg
                  |L8.88|
000058  4908              LDR      r1,|L8.124|
00005a  2302              MOVS     r3,#2                 ;226
00005c  2208              MOVS     r2,#8                 ;226
00005e  e8bd4010          POP      {r4,lr}               ;226
000062  3918              SUBS     r1,r1,#0x18           ;226
000064  f7ffbffe          B.W      CAN1_Send_Msg
                  |L8.104|
000068  4904              LDR      r1,|L8.124|
00006a  2302              MOVS     r3,#2                 ;230
00006c  2208              MOVS     r2,#8                 ;230
00006e  e8bd4010          POP      {r4,lr}               ;230
000072  3910              SUBS     r1,r1,#0x10           ;230
000074  f7ffbffe          B.W      CAN1_Send_Msg
;;;235    	}
;;;236    }
;;;237    
                          ENDP

                  |L8.120|
                          DCD      Rotate1
                  |L8.124|
                          DCD      ||area_number.15||+0x60

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

                  steeringFunction PROC
;;;36     
;;;37     void steeringFunction(DriverStruct *Motor, DriverStruct *refMotor) //转向电机 行走轮有速度则开使能 无速度则视情况关使能
000000  f8902058          LDRB     r2,[r0,#0x58]
;;;38     {
;;;39         if (Motor->Private.initStep != 255) //如果未初始化先初始化
000004  2aff              CMP      r2,#0xff
000006  d001              BEQ      |L9.12|
;;;40         {
;;;41             init_driver(Motor); //非阻塞函数
000008  f7ffbffe          B.W      init_driver
                  |L9.12|
;;;42         }
;;;43         else
;;;44         {
;;;45             set_steering_speed(Motor, refMotor); //发速度
00000c  f7ffbffe          B.W      set_steering_speed
;;;46         }
;;;47     }
;;;48     
                          ENDP


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

                  setPosModel
000000  2f606000          DCB      0x2f,0x60,0x60,0x00
000004  01000000          DCB      0x01,0x00,0x00,0x00

                          AREA ||area_number.13||, DATA, ALIGN=0

                          EXPORTAS ||area_number.13||, ||.data||
                  setTargetPos
000000  237a6000          DCB      0x23,0x7a,0x60,0x00
000004  2c370900          DCB      0x2c,0x37,0x09,0x00

                          AREA ||area_number.14||, DATA, ALIGN=0

                          EXPORTAS ||area_number.14||, ||.data||
                  setPosSpeed
000000  23816000          DCB      0x23,0x81,0x60,0x00
000004  0046c323          DCB      0x00,0x46,0xc3,0x23

                          AREA ||area_number.15||, DATA, ALIGN=2

                          EXPORTAS ||area_number.15||, ||.data||
                  FirstFlag
000000  01000000          DCB      0x01,0x00,0x00,0x00
                  EncodeValue
                          DCD      0x00000000
                  lastTime1
                          DCD      0x00000000
                  lastTime2
                          DCD      0x00000000
                  curSpeed1
000010  00000000          DCFS     0x00000000 ; 0
                  curSpeed2
000014  00000000          DCFS     0x00000000 ; 0
                  speedSlope1
000018  00000000          DCFS     0x00000000 ; 0
                  speedSlope2
00001c  00000000          DCFS     0x00000000 ; 0
                  lastEncodeValue
                          DCD      0x00000000
                  |symbol_number.43|
                          DCD      0x00000000
                  |symbol_number.44|
                          DCD      0x00000000
                  i_First
                          DCD      0x00000001
                  i_Count
                          DCD      0x00000000
                  lastTime
                          DCD      0x00000000
                  |symbol_number.48|
                          DCD      0x00000000
                  lastTime3
                          DCD      0x00000000
                  setSpeedSlope1
000040  23836000          DCB      0x23,0x83,0x60,0x00
000044  204e0800          DCB      0x20,0x4e,0x08,0x00
                  setSpeedSlope2
000048  23846000          DCB      0x23,0x84,0x60,0x00
00004c  204e0800          DCB      0x20,0x4e,0x08,0x00
                  enable
000050  2b406000          DCB      0x2b,0x40,0x60,0x00
000054  0f000000          DCB      0x0f,0x00,0x00,0x00
                  speedModel
000058  2f606000          DCB      0x2f,0x60,0x60,0x00
00005c  03000000          DCB      0x03,0x00,0x00,0x00
                  setSpeed
000060  23ff6000          DCB      0x23,0xff,0x60,0x00
000064  00000000          DCB      0x00,0x00,0x00,0x00
                  readPos
000068  40636000          DCB      0x40,0x63,0x60,0x00
00006c  00000000          DCB      0x00,0x00,0x00,0x00
                  data1
000070  00000000          DCB      0x00,0x00,0x00,0x00
                          DCD      0x00000000
                  data2
000078  00000000          DCB      0x00,0x00,0x00,0x00
                          DCD      0x00000000

                          AREA ||area_number.16||, DATA, ALIGN=0

                          EXPORTAS ||area_number.16||, ||.data||
                  PosEnable
000000  2b406000          DCB      0x2b,0x40,0x60,0x00
000004  3f000000          DCB      0x3f,0x00,0x00,0x00

                          AREA ||area_number.17||, DATA, ALIGN=0

                          EXPORTAS ||area_number.17||, ||.data||
                  setZeroPoint1
000000  2f606000          DCB      0x2f,0x60,0x60,0x00
000004  06000000          DCB      0x06,0x00,0x00,0x00

                          AREA ||area_number.18||, DATA, ALIGN=0

                          EXPORTAS ||area_number.18||, ||.data||
                  setZeroPoint2
000000  2f986000          DCB      0x2f,0x98,0x60,0x00
000004  23000000          DCB      0x23,0x00,0x00,0x00

                          AREA ||area_number.19||, DATA, ALIGN=0

                          EXPORTAS ||area_number.19||, ||.data||
                  setZeroPoint3
000000  2b406000          DCB      0x2b,0x40,0x60,0x00
000004  1f000000          DCB      0x1f,0x00,0x00,0x00

;*** Start embedded assembler ***

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

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

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

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***