📄 rd_usedlocate_lib.s

📁 全场地位系统：小车静止或移动过程中码盘进行全场定位,用的是avr单片机
💻 S
📖 第 1 页 / 共 2 页
字号:
12 下一页
	.module RD_UseDLocate_LIB.c
	.area text(rom, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
_g_fLastAngle::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
_g_fX::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
_g_fY::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
	.area text(rom, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
	.dbfunc e Get_Absolute_Angle _Get_Absolute_Angle fD
	.even
_Get_Absolute_Angle::
	rcall push_xgsetF000
	.dbline -1
	.dbline 118
; /***********************************************************
; *   函数库说明：差分定位数值计算函数库                     *
; *   版本：      v1.00beta                                  *
; *   作者：      王卓然                                     *
; *   创建日期：  2008年3月29日                              *
; * -------------------------------------------------------- *
; *  [支 持 库]                                              *
; *   支持库名称：                                           *
; *   需要版本：                                             *
; *   支持库说明：                                           *
; * -------------------------------------------------------- *
; *  [版本更新]                                              *
; *   修改：                                                 *
; *   修改日期：                                             *
; *   版本：                                                 *
; * -------------------------------------------------------- *
; *  [版本历史]                                              *
; * -------------------------------------------------------- *
; *  [使用说明]                                              *
; ***********************************************************/
; 
; /********************
; * 头 文 件 配 置 区 *
; ********************/
; # include "RD_MacroAndConst.h"
; # include "DL_Config.h"
; # include "RD_UseDLocate_LIB.h"
; # include "PF_Config.h"
; # include "Math.h"
; 
; /********************
; *   系 统 宏 定 义  *
; ********************/
; 
; /*------------------*
; *   常 数 宏 定 义  *
; *------------------*/
; 
; //轮子直径
; #ifndef WHEEL_D_L
;     #error No define WHEEL_D_L!
; #endif
; #ifndef WHEEL_D_R
;     #error No define WHEEL_D_R!
; #endif
; 
; //轮间距
; #ifndef D_BTW_WHEEL
;     #error No define D_BTW_WHEEL
; #endif
; 
; //每圈线精度
; #ifndef N_L
;     #error No define N_L
; #endif
; #ifndef N_R
;     #error No define N_R
; #endif
; 
; //码盘倍频数
; #ifndef MULTIPLE
;     #error No define MULTIPLE
; #endif
; 
; //计数器 到 弧长
; # define K_L    ((PI * (WHEEL_D_L)) / ((MULTIPLE) * (N_L)))
; # define K_R    ((PI * (WHEEL_D_R)) / ((MULTIPLE) * (N_R)))
; 
; 
; 
; 
; /*------------------*
; *   动 作 宏 定 义  *
; *------------------*/
; 
; /********************
; *  自定义类型声明区 *
; ********************/
; 
; /********************
; *  模块结构体定义区 *
; ********************/
; 
; /********************
; *   函 数 声 明 区  *
; ********************/
; float Get_Absolute_Angle(void);
; float fMod(float fNumberA,float fNumberB);
; BOOL PROC_Difference_Locate(void);
; 
; /********************
; *   模块函数声明区  *
; ********************/
; 
; /********************
; *   模块变量声明区  *
; ********************/
; 
; /********************
; *   全局变量声明区  *
; ********************/
; float g_fLastAngle = 0.0;
; float g_fX = 0.0;
; float g_fY = 0.0;
; 
; /********************
; *  全局变量声引用区 *
; ********************/
; extern BOOL g_bRunFlagB;
; 
; /***********************************************************
; *   函数说明：获取当前绝对角度函数                         *
; *   输入：    无                                           *
; *   输出：    角度值                                       *
; *   调用函数：无                                           *
; ***********************************************************/
; float Get_Absolute_Angle(void)
; {
	.dbline 120
;     return 
;         (
	ldi R16,<L2
	ldi R17,>L2
	rcall lpm32
	movw R2,R16
	movw R4,R18
	ldi R16,<L3
	ldi R17,>L3
	rcall lpm32
	movw R6,R16
	movw R8,R18
	lds R18,_g_lCounterR+2
	lds R19,_g_lCounterR+2+1
	lds R16,_g_lCounterR
	lds R17,_g_lCounterR+1
	rcall long2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	movw R16,R6
	movw R18,R8
	rcall fpmule2
	movw R6,R16
	movw R8,R18
	ldi R16,<L4
	ldi R17,>L4
	rcall lpm32
	movw R20,R16
	movw R22,R18
	lds R18,_g_lCounterL+2
	lds R19,_g_lCounterL+2+1
	lds R16,_g_lCounterL
	lds R17,_g_lCounterL+1
	rcall long2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	movw R16,R20
	movw R18,R22
	rcall fpmule2x
	movw R16,R6
	movw R18,R8
	rcall fpsub2x
	movw R16,R2
	movw R18,R4
	rcall fpmule2
	.dbline -2
L1:
	.dbline 0 ; func end
	rjmp pop_xgsetF000
	.dbend
	.dbfunc e Get_Relative_Angle _Get_Relative_Angle fD
;           lDCR -> y+8
;           lDCL -> y+4
	.even
_Get_Relative_Angle::
	rcall push_arg4
	rcall push_xgsetF000
	.dbline -1
	.dbline 133
;             (float)g_lCounterR * (float)K_R 
;              - (float)g_lCounterL * (float)K_L
;          ) * (1.0 / (float)D_BTW_WHEEL);
; }
; 
; /***********************************************************
; *   函数说明：获取当前相对角度函数                         *
; *   输入：    左编码器微分量，右编码器微分量               *
; *   输出：    角度值（弧度）                               *
; *   调用函数：无                                           *
; ***********************************************************/
; float Get_Relative_Angle(INT32 lDCL,INT32 lDCR)
; {
	.dbline 134
;     if (lDCL == lDCR)
	ldd R2,y+8
	ldd R3,y+9
	ldd R4,y+10
	ldd R5,y+11
	ldd R6,y+4
	ldd R7,y+5
	ldd R8,y+6
	ldd R9,y+7
	cp R6,R2
	cpc R7,R3
	cpc R8,R4
	cpc R9,R5
	brne L6
X0:
	.dbline 135
;     {
	.dbline 136
;         return 0.0;
	ldi R16,<L8
	ldi R17,>L8
	rcall lpm32
	rjmp L5
L6:
	.dbline 140
;     }
;     
;     return 
;         (
	ldi R16,<L2
	ldi R17,>L2
	rcall lpm32
	movw R2,R16
	movw R4,R18
	ldi R16,<L3
	ldi R17,>L3
	rcall lpm32
	movw R6,R16
	movw R8,R18
	ldd R16,y+8
	ldd R17,y+9
	ldd R18,y+10
	ldd R19,y+11
	rcall long2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	movw R16,R6
	movw R18,R8
	rcall fpmule2
	movw R6,R16
	movw R8,R18
	ldi R16,<L4
	ldi R17,>L4
	rcall lpm32
	movw R20,R16
	movw R22,R18
	ldd R16,y+4
	ldd R17,y+5
	ldd R18,y+6
	ldd R19,y+7
	rcall long2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	movw R16,R20
	movw R18,R22
	rcall fpmule2x
	movw R16,R6
	movw R18,R8
	rcall fpsub2x
	movw R16,R2
	movw R18,R4
	rcall fpmule2
	.dbline -2
L5:
	.dbline 0 ; func end
	rcall pop_xgsetF000
	adiw R28,4
	ret
	.dbsym l lDCR 8 L
	.dbsym l lDCL 4 L
	.dbend
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
L10:
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
L11:
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
	.area text(rom, con, rel)
	.dbfile E:\单片机\ROOBCO~1\全向定位系统\被动小车\Passive_Encoder\RD_UseDLocate_LIB.c
	.dbfunc e PROC_Difference_Locate _PROC_Difference_Locate fc
	.dbsym s s_lLastCounterR L11 L
	.dbsym s s_lLastCounterL L10 L
;            fR1 -> y+20
;            fR0 -> y+16
; nDeltaCounterL0 -> R10,R11
;    fTempAngle0 -> y+16
; fAbsoluteAngle0 -> y+12
;   fDeltaAngle0 -> y+8
;            fR0 -> y+8
; nDeltaCounterL0 -> R10,R11
;           Reg8 -> y+4
	.even
_PROC_Difference_Locate::
	rcall push_xgsetF00C
	sbiw R28,24
	.dbline -1
	.dbline 155
;             (float)lDCR * (float)K_R 
;              - (float)lDCL * (float)K_L
;          ) * (1.0 / (float)D_BTW_WHEEL);
; }
; 
; 
; 
; /***********************************************************
; *   函数说明：差分定位计算函数                             *
; *   输入：    无                                           *
; *   输出：    FALSE                                        *
; *   调用函数：无                                           *
; ***********************************************************/
; BOOL PROC_Difference_Locate(void)
; {
	.dbline 159
;     static INT32 s_lLastCounterL = 0;
;     static INT32 s_lLastCounterR = 0;
;     
;     if ((g_lCounterLImage - s_lLastCounterL)
	lds R4,L11+2
	lds R5,L11+2+1
	lds R2,L11
	lds R3,L11+1
	lds R8,_g_lCounterRImage+2
	lds R9,_g_lCounterRImage+2+1
	lds R6,_g_lCounterRImage
	lds R7,_g_lCounterRImage+1
	sub R6,R2
	sbc R7,R3
	sbc R8,R4
	sbc R9,R5
	lds R4,L10+2
	lds R5,L10+2+1
	lds R2,L10
	lds R3,L10+1
	lds R22,_g_lCounterLImage+2
	lds R23,_g_lCounterLImage+2+1
	lds R20,_g_lCounterLImage
	lds R21,_g_lCounterLImage+1
	sub R20,R2
	sbc R21,R3
	sbc R22,R4
	sbc R23,R5
	cp R20,R6
	cpc R21,R7
	cpc R22,R8
	cpc R23,R9
	breq X2
	rjmp L12
X2:
X1:
	.dbline 161
;          == (g_lCounterRImage - s_lLastCounterR))
;     {
	.dbline 162
;         INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
	lds R10,_g_lCounterLImage
	lds R11,_g_lCounterLImage+1
	sub R10,R2
	sbc R11,R3
	.dbline 163
;         float fR = ((float)nDeltaCounterL * (float)K_L);
	ldi R16,<L4
	ldi R17,>L4
	rcall lpm32
	movw R2,R16
	movw R4,R18
	movw R16,R10
	rcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	movw R16,R2
	movw R18,R4
	rcall fpmule2
	std y+8,R16
	std y+9,R17
	std y+10,R18
	std y+11,R19
	.dbline 165
;         
;         g_fX += fR * cos(g_fLastAngle);
	lds R18,_g_fLastAngle+2
	lds R19,_g_fLastAngle+2+1
	lds R16,_g_fLastAngle
	lds R17,_g_fLastAngle+1
	rcall _cosf
	movw R2,R16
	movw R4,R18
	lds R8,_g_fX+2
	lds R9,_g_fX+2+1
	lds R6,_g_fX
	lds R7,_g_fX+1
	ldd R16,y+8
	ldd R17,y+9
	ldd R18,y+10
	ldd R19,y+11
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall fpmule2x
	movw R16,R6
	movw R18,R8
	rcall fpadd2
	sts _g_fX+1,R17
	sts _g_fX,R16
	sts _g_fX+2+1,R19
	sts _g_fX+2,R18
	.dbline 166
;         g_fY += fR * sin(g_fLastAngle);
	lds R18,_g_fLastAngle+2
	lds R19,_g_fLastAngle+2+1
	lds R16,_g_fLastAngle
	lds R17,_g_fLastAngle+1
	rcall _sinf
	movw R2,R16
	movw R4,R18
	lds R8,_g_fY+2
	lds R9,_g_fY+2+1
	lds R6,_g_fY
	lds R7,_g_fY+1
	ldd R16,y+8
	ldd R17,y+9
	ldd R18,y+10
	ldd R19,y+11
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall fpmule2x
	movw R16,R6
	movw R18,R8
	rcall fpadd2
	sts _g_fY+1,R17
	sts _g_fY,R16
	sts _g_fY+2+1,R19
	sts _g_fY+2,R18
	.dbline 167
;     }
	rjmp L13
L12:
	.dbline 169
;     else
;     {
	.dbline 172
;         float fDeltaAngle,fAbsoluteAngle;
;         //计算角度微元
;         {
	.dbline 173
;             float fTempAngle = Get_Relative_Angle
	lds R4,_g_lCounterRImage+2
	lds R5,_g_lCounterRImage+2+1
	lds R2,_g_lCounterRImage
	lds R3,_g_lCounterRImage+1
	std y+0,R2
	std y+1,R3
	std y+2,R4
	std y+3,R5
	lds R18,_g_lCounterLImage+2
	lds R19,_g_lCounterLImage+2+1
	lds R16,_g_lCounterLImage
	lds R17,_g_lCounterLImage+1
	rcall _Get_Relative_Angle
	std y+16,R16
	std y+17,R17
	std y+18,R18
	std y+19,R19
	.dbline 178
;                             (
;                                 g_lCounterLImage,
;                                 g_lCounterRImage
;                             );
;             fDeltaAngle = (fTempAngle - g_fLastAngle);
	ldd R16,y+16
	ldd R17,y+17
	ldd R18,y+18
	ldd R19,y+19
	ldi R24,<_g_fLastAngle
	ldi R25,>_g_fLastAngle
	st -y,R25
	st -y,R24
	rcall fpsub1
	std y+8,R16
	std y+9,R17
	std y+10,R18
	std y+11,R19
	.dbline 179
;             fAbsoluteAngle = fDeltaAngle * 0.5 + g_fLastAngle;
	ldi R16,<L14
	ldi R17,>L14
	rcall lpm32
	movw R24,R28
	adiw R24,8
	st -y,R25
	st -y,R24
	rcall fpmule1
	std y+4,R16
	std y+5,R17
	std y+6,R18
	std y+7,R19
	ldd R16,y+4
	ldd R17,y+5
	ldd R18,y+6
	ldd R19,y+7
	ldi R24,<_g_fLastAngle
	ldi R25,>_g_fLastAngle
	st -y,R25
	st -y,R24
	rcall fpadd1
	std y+12,R16
	std y+13,R17
	std y+14,R18
	std y+15,R19
	.dbline 181
;     
;             g_fLastAngle = fTempAngle;
	ldd R2,y+16
	ldd R3,y+17
	ldd R4,y+18
	ldd R5,y+19
	sts _g_fLastAngle+1,R3
12 下一页
💿 文件大小 114 K
👤 上传用户 fy8855773
📂 所属分类单片机开发
🏷️ 相关标签

#avr #移动 #定位
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -