; generated by Component: ARM Compiler 5.06 update 6 (build 750) Tool: ArmCC [4d3637]
; commandline ArmCC [--list --debug -c --asm --interleave -o.\obj\pid.o --asm_dir=.\list\ --list_dir=.\list\ --depend=.\obj\pid.d --cpu=Cortex-M3 --apcs=interwork -O0 --diag_suppress=9931 -I.\FWlib\inc -I.\user -I.\CM3 -I.\dmp -I.\RTE\_STM32-FD -If:\Users\Administrator\AppData\Local\Arm\Packs\ARM\CMSIS\5.7.0\CMSIS\Core\Include -If:\Users\Administrator\AppData\Local\Arm\Packs\Keil\STM32F1xx_DFP\2.4.1\Device\Include -D__MICROLIB -D__UVISION_VERSION=530 -D_RTE_ -DSTM32F10X_MD -D_RTE_ -DUSE_STDPERIPH_DRIVER -DSTM32F10X_MD --omf_browse=.\obj\pid.crf user\PID.c]
                          THUMB

                          AREA ||.text||, CODE, READONLY, ALIGN=2

                  PID_Init PROC
;;;4      //--------------ֺPIDʼ-----------------------------------
;;;5      void PID_Init(PID_Struct* p)
000000  49a5              LDR      r1,|L1.664|
;;;6      {
;;;7      	p->turns_Kp=0.3;//0.1;
000002  6001              STR      r1,[r0,#0]
;;;8      	p->turns_Ki=0.2;
000004  49a5              LDR      r1,|L1.668|
000006  6041              STR      r1,[r0,#4]
;;;9      	//p->turns_Kp=0;
;;;10     	//p->turns_Ki=0;
;;;11     	p->turns_Kd=0;
000008  2100              MOVS     r1,#0
00000a  6081              STR      r1,[r0,#8]
;;;12     	p->Ur=2000;
00000c  f44f61fa          MOV      r1,#0x7d0
000010  60c1              STR      r1,[r0,#0xc]
;;;13     	p->PID_is_Enable=0;
000012  2100              MOVS     r1,#0
000014  7401              STRB     r1,[r0,#0x10]
;;;14     	p->Un=0;
000016  6141              STR      r1,[r0,#0x14]
;;;15     	p->En_1=0;
000018  6181              STR      r1,[r0,#0x18]
;;;16     	p->En_2=0;
00001a  61c1              STR      r1,[r0,#0x1c]
;;;17     	p->PWM=0;
00001c  6201              STR      r1,[r0,#0x20]
;;;18     }
00001e  4770              BX       lr
;;;19     
                          ENDP

                  Balance_up_PID_Init PROC
;;;20     //----------------ƽ⳵ֱʼ---------------------------------
;;;21     void Balance_up_PID_Init(Balance_PID_Struct* p)
000020  499f              LDR      r1,|L1.672|
;;;22     {
;;;23     	p->Balance_Kp=480;//180;//0.1;
000022  6001              STR      r1,[r0,#0]
;;;24     	p->Balance_Ki=0;
000024  2100              MOVS     r1,#0
000026  6041              STR      r1,[r0,#4]
;;;25     	//p->turns_Kp=0;
;;;26     	//p->turns_Ki=0;
;;;27     	p->Balance_Kd=2.0;//2;//300
000028  f04f4180          MOV      r1,#0x40000000
00002c  6081              STR      r1,[r0,#8]
;;;28     	//p->Balance_Kd=500;//300
;;;29     	p->Ur=2000;
00002e  f44f61fa          MOV      r1,#0x7d0
000032  60c1              STR      r1,[r0,#0xc]
;;;30     	p->Balance_PID_is_Enable=1;
000034  2101              MOVS     r1,#1
000036  7401              STRB     r1,[r0,#0x10]
;;;31     	p->Un=0;
000038  2100              MOVS     r1,#0
00003a  6141              STR      r1,[r0,#0x14]
;;;32     	p->roll_1=0;
00003c  6181              STR      r1,[r0,#0x18]
;;;33     	p->roll_2=0;
00003e  61c1              STR      r1,[r0,#0x1c]
;;;34     	p->PWM=0;
000040  6201              STR      r1,[r0,#0x20]
;;;35     }
000042  4770              BX       lr
;;;36     
                          ENDP

                  Balance_up_PID PROC
;;;37     //------------------ƽ⳵ֱ----------------------
;;;38     int Balance_up_PID(unsigned char wheel,float TargetRoll,float currentRoll,Balance_PID_Struct* p)
000044  e92d4ff8          PUSH     {r3-r11,lr}
;;;39     {  
000048  460f              MOV      r7,r1
00004a  4616              MOV      r6,r2
00004c  461c              MOV      r4,r3
;;;40     	float En;
;;;41       int max_pwm=1900;           
00004e  f2407a6c          MOV      r10,#0x76c
;;;42     	if(p->Balance_PID_is_Enable){         //ֱʹ
000052  7c20              LDRB     r0,[r4,#0x10]
000054  b1f8              CBZ      r0,|L1.150|
;;;43     		En=currentRoll-TargetRoll;
000056  4639              MOV      r1,r7
000058  4630              MOV      r0,r6
00005a  f7fffffe          BL       __aeabi_fsub
00005e  4605              MOV      r5,r0
;;;44     		//p->Un=(p->Balance_Kp*(En))+p->Balance_Kd*(p->roll_1-p->roll_2);  
;;;45     		p->Un=(p->Balance_Kp*(En))+p->Balance_Kd*(gyrox);  //&΢
000060  4890              LDR      r0,|L1.676|
000062  f9b00000          LDRSH    r0,[r0,#0]  ; gyrox
000066  f7fffffe          BL       __aeabi_i2f
00006a  4683              MOV      r11,r0
00006c  68a1              LDR      r1,[r4,#8]
00006e  f7fffffe          BL       __aeabi_fmul
000072  4681              MOV      r9,r0
000074  4629              MOV      r1,r5
000076  6820              LDR      r0,[r4,#0]
000078  f7fffffe          BL       __aeabi_fmul
00007c  4649              MOV      r1,r9
00007e  9000              STR      r0,[sp,#0]
000080  f7fffffe          BL       __aeabi_fadd
000084  4680              MOV      r8,r0
000086  f7fffffe          BL       __aeabi_f2iz
00008a  6160              STR      r0,[r4,#0x14]
;;;46     		p->roll_2=p->roll_1;
00008c  69a0              LDR      r0,[r4,#0x18]
00008e  61e0              STR      r0,[r4,#0x1c]
;;;47     		p->roll_1=En;
000090  61a5              STR      r5,[r4,#0x18]
;;;48     		p->PWM=p->Un;	
000092  6960              LDR      r0,[r4,#0x14]
000094  6220              STR      r0,[r4,#0x20]
                  |L1.150|
;;;49     		
;;;50     	}	
;;;51     	return p->Un;		
000096  6960              LDR      r0,[r4,#0x14]
;;;52     }
000098  e8bd8ff8          POP      {r3-r11,pc}
;;;53     
                          ENDP

                  velocity PROC
;;;55     //------------------ƽ⳵ٶȻ----------------------
;;;56     int velocity(int encoder_left,int encoder_right){  
00009c  e92d4ff0          PUSH     {r4-r11,lr}
0000a0  b085              SUB      sp,sp,#0x14
0000a2  4605              MOV      r5,r0
0000a4  460e              MOV      r6,r1
;;;57         static float Velocity,Encoder_Least,Encoder,Movement;
;;;58         static float Encoder_Integral;
;;;59     	  //static int dict=0;
;;;60     	  int max_dis=10000;
0000a6  f2427410          MOV      r4,#0x2710
;;;61     	  float kp=90;
0000aa  f8df91fc          LDR      r9,|L1.680|
;;;62     	  float ki=kp/200.0;
0000ae  4648              MOV      r0,r9
0000b0  f7fffffe          BL       __aeabi_f2d
0000b4  4607              MOV      r7,r0
0000b6  2200              MOVS     r2,#0
0000b8  4b7c              LDR      r3,|L1.684|
0000ba  f7fffffe          BL       __aeabi_ddiv
0000be  4682              MOV      r10,r0
0000c0  f7fffffe          BL       __aeabi_d2f
0000c4  9004              STR      r0,[sp,#0x10]
;;;63     	/*
;;;64     	  if(dict==0 && roll>zero) dict=1; 
;;;65     	  else if(dict==0 && roll<zero) dict=-1; 
;;;66     	  else if(dict==1 && roll<zero){
;;;67     			dict=-1; 
;;;68     		  Encoder=0;
;;;69     			Encoder_Least=0;
;;;70     			Encoder_Integral=0;
;;;71     		}
;;;72     		else if(dict==-1 && roll>zero){
;;;73     			dict=1; 
;;;74     		  Encoder=0;
;;;75     			Encoder_Least=0;
;;;76     			Encoder_Integral=0;
;;;77     		}
;;;78     		*/
;;;79        //=============ٶPI=======================//  
;;;80         Encoder_Least =(encoder_right+encoder_left)-0;      
0000c6  1971              ADDS     r1,r6,r5
0000c8  4608              MOV      r0,r1
0000ca  f7fffffe          BL       __aeabi_i2f
0000ce  4978              LDR      r1,|L1.688|
0000d0  6008              STR      r0,[r1,#0]  ; Encoder_Least
;;;81         //===ȡٶƫ==ٶȣұ֮ͣ-Ŀٶȣ˴Ϊ㣩 
;;;82         Encoder *= 0.7;          //===һ׵ͨ˲       
0000d2  4878              LDR      r0,|L1.692|
0000d4  6800              LDR      r0,[r0,#0]  ; Encoder
0000d6  f7fffffe          BL       __aeabi_f2d
0000da  4607              MOV      r7,r0
0000dc  f04f3266          MOV      r2,#0x66666666
0000e0  4b75              LDR      r3,|L1.696|
0000e2  f7fffffe          BL       __aeabi_dmul
0000e6  4682              MOV      r10,r0
0000e8  f7fffffe          BL       __aeabi_d2f
0000ec  4971              LDR      r1,|L1.692|
0000ee  6008              STR      r0,[r1,#0]  ; Encoder
;;;83         Encoder += Encoder_Least*0.3;   //===һ׵ͨ˲    
0000f0  486f              LDR      r0,|L1.688|
0000f2  6800              LDR      r0,[r0,#0]  ; Encoder_Least
0000f4  f7fffffe          BL       __aeabi_f2d
0000f8  4607              MOV      r7,r0
0000fa  f04f3233          MOV      r2,#0x33333333
0000fe  4b6f              LDR      r3,|L1.700|
000100  f7fffffe          BL       __aeabi_dmul
000104  4682              MOV      r10,r0
000106  468b              MOV      r11,r1
000108  486a              LDR      r0,|L1.692|
00010a  6800              LDR      r0,[r0,#0]  ; Encoder
00010c  f7fffffe          BL       __aeabi_f2d
000110  4652              MOV      r2,r10
000112  465b              MOV      r3,r11
000114  e9cd0100          STRD     r0,r1,[sp,#0]
000118  f7fffffe          BL       __aeabi_dadd
00011c  e9cd0102          STRD     r0,r1,[sp,#8]
000120  f7fffffe          BL       __aeabi_d2f
000124  4963              LDR      r1,|L1.692|
000126  6008              STR      r0,[r1,#0]  ; Encoder
;;;84         Encoder_Integral +=Encoder; //===ֳλ ʱ䣺10ms
000128  4608              MOV      r0,r1
00012a  6801              LDR      r1,[r0,#0]  ; Encoder
00012c  4864              LDR      r0,|L1.704|
00012e  6800              LDR      r0,[r0,#0]  ; Encoder_Integral
000130  f7fffffe          BL       __aeabi_fadd
000134  4962              LDR      r1,|L1.704|
000136  6008              STR      r0,[r1,#0]  ; Encoder_Integral
;;;85         if(Encoder_Integral>max_dis)    Encoder_Integral=max_dis;   
000138  4620              MOV      r0,r4
00013a  f7fffffe          BL       __aeabi_i2f
00013e  4607              MOV      r7,r0
000140  485f              LDR      r0,|L1.704|
000142  6801              LDR      r1,[r0,#0]  ; Encoder_Integral
000144  4638              MOV      r0,r7
000146  f7fffffe          BL       __aeabi_cfcmple
00014a  d204              BCS      |L1.342|
00014c  4620              MOV      r0,r4
00014e  f7fffffe          BL       __aeabi_i2f
000152  495b              LDR      r1,|L1.704|
000154  6008              STR      r0,[r1,#0]  ; Encoder_Integral
                  |L1.342|
;;;86         //===޷
;;;87         if(Encoder_Integral<-max_dis)    Encoder_Integral=-max_dis;   
000156  4260              RSBS     r0,r4,#0
000158  f7fffffe          BL       __aeabi_i2f
00015c  4607              MOV      r7,r0
00015e  4858              LDR      r0,|L1.704|
000160  6801              LDR      r1,[r0,#0]  ; Encoder_Integral
000162  4638              MOV      r0,r7
000164  f7fffffe          BL       __aeabi_cfrcmple
000168  d204              BCS      |L1.372|
00016a  4260              RSBS     r0,r4,#0
00016c  f7fffffe          BL       __aeabi_i2f
000170  4953              LDR      r1,|L1.704|
000172  6008              STR      r0,[r1,#0]  ; Encoder_Integral
                  |L1.372|
;;;88         //===޷  
;;;89         Velocity=Encoder*kp+Encoder_Integral*ki;
000174  4852              LDR      r0,|L1.704|
000176  9904              LDR      r1,[sp,#0x10]
000178  6800              LDR      r0,[r0,#0]  ; Encoder_Integral
00017a  f7fffffe          BL       __aeabi_fmul
00017e  4607              MOV      r7,r0
000180  4649              MOV      r1,r9
000182  484c              LDR      r0,|L1.692|
000184  6800              LDR      r0,[r0,#0]  ; Encoder
000186  f7fffffe          BL       __aeabi_fmul
00018a  4680              MOV      r8,r0
00018c  4639              MOV      r1,r7
00018e  f7fffffe          BL       __aeabi_fadd
000192  494c              LDR      r1,|L1.708|
000194  6008              STR      r0,[r1,#0]  ; Velocity
;;;90         
;;;91         //===ٶȿ  
;;;92         if(roll<-90||roll>90)   Encoder_Integral=0;   
000196  494c              LDR      r1,|L1.712|
000198  484c              LDR      r0,|L1.716|
00019a  6800              LDR      r0,[r0,#0]  ; roll
00019c  f7fffffe          BL       __aeabi_cfcmple
0001a0  d305              BCC      |L1.430|
0001a2  4941              LDR      r1,|L1.680|
0001a4  4849              LDR      r0,|L1.716|
0001a6  6800              LDR      r0,[r0,#0]  ; roll
0001a8  f7fffffe          BL       __aeabi_cfrcmple
0001ac  d202              BCS      |L1.436|
                  |L1.430|
0001ae  2000              MOVS     r0,#0
0001b0  4943              LDR      r1,|L1.704|
0001b2  6008              STR      r0,[r1,#0]  ; Encoder_Integral
                  |L1.436|
;;;93         //===رպ
;;;94         return Velocity;
0001b4  4843              LDR      r0,|L1.708|
0001b6  6800              LDR      r0,[r0,#0]  ; Velocity
0001b8  f7fffffe          BL       __aeabi_f2iz
;;;95     }
0001bc  b005              ADD      sp,sp,#0x14
0001be  e8bd8ff0          POP      {r4-r11,pc}
;;;96     //---------------ֺpid ڣڵתٱֻһ-------------------------
                          ENDP

                  Turns_PID PROC
;;;97     void Turns_PID(unsigned char wheel,int TargetTurns,int currentTurns,PID_Struct* p)
0001c2  e92d4ff0          PUSH     {r4-r11,lr}
;;;98     {  
0001c6  b085              SUB      sp,sp,#0x14
0001c8  4606              MOV      r6,r0
0001ca  460f              MOV      r7,r1
0001cc  4690              MOV      r8,r2
0001ce  461c              MOV      r4,r3
;;;99     	int En;
;;;100    	if(p->PID_is_Enable){
0001d0  7c20              LDRB     r0,[r4,#0x10]
0001d2  2800              CMP      r0,#0
0001d4  d05c              BEQ      |L1.656|
;;;101    		En=TargetTurns-currentTurns;
0001d6  eba70508          SUB      r5,r7,r8
;;;102    		p->Un+=(p->turns_Kp*(En-p->En_1))+(p->turns_Ki*En)+p->turns_Kd*(En-2*p->En_1+p->En_2);  //PID
0001da  2202              MOVS     r2,#2
0001dc  69a1              LDR      r1,[r4,#0x18]
0001de  fb025111          MLS      r1,r2,r1,r5
0001e2  69e2              LDR      r2,[r4,#0x1c]
0001e4  1888              ADDS     r0,r1,r2
0001e6  f7fffffe          BL       __aeabi_i2f
0001ea  4683              MOV      r11,r0
0001ec  68a1              LDR      r1,[r4,#8]
0001ee  f7fffffe          BL       __aeabi_fmul
0001f2  9002              STR      r0,[sp,#8]
0001f4  4628              MOV      r0,r5
0001f6  f7fffffe          BL       __aeabi_i2f
0001fa  9000              STR      r0,[sp,#0]
0001fc  6861              LDR      r1,[r4,#4]
0001fe  f7fffffe          BL       __aeabi_fmul
000202  4683              MOV      r11,r0
000204  69a1              LDR      r1,[r4,#0x18]
000206  1a68              SUBS     r0,r5,r1
000208  f7fffffe          BL       __aeabi_i2f
00020c  9000              STR      r0,[sp,#0]
00020e  6821              LDR      r1,[r4,#0]
000210  f7fffffe          BL       __aeabi_fmul
000214  4659              MOV      r1,r11
000216  9001              STR      r0,[sp,#4]
000218  f7fffffe          BL       __aeabi_fadd
00021c  4682              MOV      r10,r0
00021e  9902              LDR      r1,[sp,#8]
000220  f7fffffe          BL       __aeabi_fadd
000224  9004              STR      r0,[sp,#0x10]
000226  6960              LDR      r0,[r4,#0x14]
000228  f7fffffe          BL       __aeabi_i2f
00022c  9003              STR      r0,[sp,#0xc]
00022e  9904              LDR      r1,[sp,#0x10]
000230  f7fffffe          BL       __aeabi_fadd
000234  4681              MOV      r9,r0
000236  f7fffffe          BL       __aeabi_f2iz
00023a  6160              STR      r0,[r4,#0x14]
;;;103    		p->En_2=p->En_1;
00023c  69a0              LDR      r0,[r4,#0x18]
00023e  61e0              STR      r0,[r4,#0x1c]
;;;104    		p->En_1=En;
000240  61a5              STR      r5,[r4,#0x18]
;;;105    		p->PWM=p->Un;
000242  6960              LDR      r0,[r4,#0x14]
000244  6220              STR      r0,[r4,#0x20]
;;;106    	
;;;107    		if(wheel==2){         //Ի׼Ϊʱǰת
000246  2e02              CMP      r6,#2
000248  d110              BNE      |L1.620|
;;;108    			Left1_Val=PWM_Val+p->PWM;		
00024a  4921              LDR      r1,|L1.720|
00024c  6a20              LDR      r0,[r4,#0x20]
00024e  6809              LDR      r1,[r1,#0]  ; PWM_Val
000250  4408              ADD      r0,r0,r1
000252  4920              LDR      r1,|L1.724|
000254  6008              STR      r0,[r1,#0]  ; Left1_Val
;;;109    		  TIM_SetCompare1(TIM4,0);
000256  2100              MOVS     r1,#0
000258  481f              LDR      r0,|L1.728|
00025a  f7fffffe          BL       TIM_SetCompare1
;;;110    		  TIM_SetCompare2(TIM4,Left1_Val);
00025e  481d              LDR      r0,|L1.724|
000260  8800              LDRH     r0,[r0,#0]  ; Left1_Val
000262  b281              UXTH     r1,r0
000264  481c              LDR      r0,|L1.728|
000266  f7fffffe          BL       TIM_SetCompare2
00026a  e011              B        |L1.656|
                  |L1.620|
;;;111    		}
;;;112    		else if(wheel==1){    //Ի׼Ϊʱǰת
00026c  2e01              CMP      r6,#1
00026e  d10f              BNE      |L1.656|
;;;113    			Right1_Val=PWM_Val+p->PWM;
000270  4917              LDR      r1,|L1.720|
000272  6a20              LDR      r0,[r4,#0x20]
000274  6809              LDR      r1,[r1,#0]  ; PWM_Val
000276  4408              ADD      r0,r0,r1
000278  4918              LDR      r1,|L1.732|
00027a  6008              STR      r0,[r1,#0]  ; Right1_Val
;;;114    			TIM_SetCompare3(TIM4,Right1_Val);
00027c  4608              MOV      r0,r1
00027e  8800              LDRH     r0,[r0,#0]  ; Right1_Val
000280  b281              UXTH     r1,r0
000282  4815              LDR      r0,|L1.728|
000284  f7fffffe          BL       TIM_SetCompare3
;;;115    			TIM_SetCompare4(TIM4,0);
000288  2100              MOVS     r1,#0
00028a  4813              LDR      r0,|L1.728|
00028c  f7fffffe          BL       TIM_SetCompare4
                  |L1.656|
;;;116    		}
;;;117    	}	
;;;118    }
000290  b005              ADD      sp,sp,#0x14
000292  e8bd8ff0          POP      {r4-r11,pc}
;;;119    /******************* (C) COPYRIGHT 2023 ܶSTM32 *****END OF FILE****/
                          ENDP

000296  0000              DCW      0x0000
                  |L1.664|
                          DCD      0x3e99999a
                  |L1.668|
                          DCD      0x3e4ccccd
                  |L1.672|
                          DCD      0x43f00000
                  |L1.676|
                          DCD      gyrox
                  |L1.680|
                          DCD      0x42b40000
                  |L1.684|
                          DCD      0x40690000
                  |L1.688|
                          DCD      Encoder_Least
                  |L1.692|
                          DCD      Encoder
                  |L1.696|
                          DCD      0x3fe66666
                  |L1.700|
                          DCD      0x3fd33333
                  |L1.704|
                          DCD      Encoder_Integral
                  |L1.708|
                          DCD      Velocity
                  |L1.712|
                          DCD      0xc2b40000
                  |L1.716|
                          DCD      roll
                  |L1.720|
                          DCD      PWM_Val
                  |L1.724|
                          DCD      Left1_Val
                  |L1.728|
                          DCD      0x40000800
                  |L1.732|
                          DCD      Right1_Val

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

                  Velocity
                          DCD      0x00000000
                  Encoder_Least
                          DCD      0x00000000
                  Encoder
                          DCD      0x00000000
                  Movement
                          DCD      0x00000000
                  Encoder_Integral
                          DCD      0x00000000
