ex_support.s

移动机器人（小车）的物理坐标和逻辑坐标的转换源码

;         }
;         else if (fNumberA > 0)
;         {
;             while((fNumberA + fNumberB) > fNumberB)
;             {
;                 fNumberA += fNumberB;
;             }
;             
;             return fNumberA;
;         }
;         else
;         {
;             return 0.0;
;         }
;     }
;     else if (fNumberB > 0)
;     {
;         if (fNumberA < 0)
;         {
;             while((fNumberA + fNumberB) < fNumberB)
;             {
;                 fNumberA += fNumberB;
;             }
;             
;             return fNumberA;
;         }
;         else if (fNumberA > 0)
;         {
;             while(fNumberA > fNumberB)
;             {
;                 fNumberA -= fNumberB;
;             }
;             
;             return fNumberA;
;         }
;         else
;         {
;             return 0.0;
;         }
;     }
;     else
;     {
;         return 0.0;
;     }
; }
; */
; 
; /***********************************************************
; *   函数说明：  相对坐标转换函数                           *
; *   输入：      无                                         *
; *   输出：      FALSE                                      *
; *   调用函数：  无                                         *
; ***********************************************************/
; BOOL PROC_Get_Relative_Location(void)
; {
	.dbline 200
;     float fTempAngle,fTempX,fTempY;
;     
;     SAFE_CODE_PERFORMANCE
	.dbline 200
	in R10,0x3f
	.dbline 200
	cli
	.dbline 200
	lds R4,_g_fAbsoluteAngle+2
	lds R5,_g_fAbsoluteAngle+2+1
	lds R2,_g_fAbsoluteAngle
	lds R3,_g_fAbsoluteAngle+1
	std y+0,R2
	std y+1,R3
	std y+2,R4
	std y+3,R5
	.dbline 200
	lds R4,_g_fAbsoluteX+2
	lds R5,_g_fAbsoluteX+2+1
	lds R2,_g_fAbsoluteX
	lds R3,_g_fAbsoluteX+1
	std y+4,R2
	std y+5,R3
	std y+6,R4
	std y+7,R5
	.dbline 200
	lds R4,_g_fAbsoluteY+2
	lds R5,_g_fAbsoluteY+2+1
	lds R2,_g_fAbsoluteY
	lds R3,_g_fAbsoluteY+1
	std y+8,R2
	std y+9,R3
	std y+10,R4
	std y+11,R5
	.dbline 200
	.dbline 200
	out 0x3f,R10
	.dbline 200
	.dbline 207
;     (
;         fTempAngle = g_fAbsoluteAngle;
;         fTempX = g_fAbsoluteX;
;         fTempY = g_fAbsoluteY;
;     )
;     
;     g_fAngle = K_ANGLE * (float)g_wCounter + fTempAngle;  
	ldi R16,<L3
	ldi R17,>L3
	rcall lpm32
	movw R2,R16
	movw R4,R18
	lds R16,_g_wCounter
	lds R17,_g_wCounter+1
	rcall uint2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	movw R16,R2
	movw R18,R4
	rcall fpmule2
	movw R24,R28
	st -y,R25
	st -y,R24
	rcall fpadd1
	sts _g_fAngle+1,R17
	sts _g_fAngle,R16
	sts _g_fAngle+2+1,R19
	sts _g_fAngle+2,R18
	.dbline 208
;     g_fX = fTempX + RELATIVE_L * cos(fTempAngle);        
	ldd R16,y+0
	ldd R17,y+1
	ldd R18,y+2
	ldd R19,y+3
	rcall _cosf
	movw R2,R16
	movw R4,R18
	ldd R6,y+4
	ldd R7,y+5
	ldd R8,y+6
	ldd R9,y+7
	ldi R16,<L4
	ldi R17,>L4
	rcall lpm32
	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 209
;     g_fY = fTempY + RELATIVE_L * sin(fTempAngle); 
	ldd R16,y+0
	ldd R17,y+1
	ldd R18,y+2
	ldd R19,y+3
	rcall _sinf
	movw R2,R16
	movw R4,R18
	ldd R6,y+8
	ldd R7,y+9
	ldd R8,y+10
	ldd R9,y+11
	ldi R16,<L4
	ldi R17,>L4
	rcall lpm32
	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 211
;     
;     return FALSE;
	clr R16
	.dbline -2
L2:
	.dbline 0 ; func end
	adiw R28,12
	ld R11,y+
	ld R10,y+
	ret
	.dbsym r cflags0 10 c
	.dbsym l fTempY0 8 D
	.dbsym l fTempX0 4 D
	.dbsym l fTempAngle0 0 D
	.dbend
	.dbfunc e Delay_MS _Delay_MS fV
;          wTime -> R16,R17
	.even
_Delay_MS::
	.dbline -1
	.dbline 221
; }
; 
; /***********************************************************
; *   函数说明：系统毫秒延时函数                             *
; *   输入：    需要延时的时间长度                           *
; *   输出：    无                                           *
; *   调用函数：无                                           *
; ***********************************************************/
; void Delay_MS(uint16 wTime)
; {
	.dbline 222
;     s_wSystemDelayTimerCounter = wTime;
	sts _s_wSystemDelayTimerCounter+1,R17
	sts _s_wSystemDelayTimerCounter,R16
L6:
	.dbline 224
;     
;     while(s_wSystemDelayTimerCounter);
L7:
	.dbline 224
	lds R2,_s_wSystemDelayTimerCounter
	lds R3,_s_wSystemDelayTimerCounter+1
	tst R2
	brne L6
	tst R3
	brne L6
X0:
	.dbline -2
L5:
	.dbline 0 ; func end
	ret
	.dbsym r wTime 16 i
	.dbend
	.dbfunc e Insert_Timer0_OVF_ISR_Code _Insert_Timer0_OVF_ISR_Code fV
	.even
_Insert_Timer0_OVF_ISR_Code::
	.dbline -1
	.dbline 234
; }
; 
; /***********************************************************
; *   函数说明：定时器0中断处理程序代码插入函数              *
; *   输入：    无                                           *
; *   输出：    无                                           *
; *   调用函数：无                                           *
; ***********************************************************/
; void Insert_Timer0_OVF_ISR_Code(void)
; {
	.dbline 235
;     g_wSystemTimer++;
	lds R24,_g_wSystemTimer
	lds R25,_g_wSystemTimer+1
	adiw R24,1
	sts _g_wSystemTimer+1,R25
	sts _g_wSystemTimer,R24
	.dbline 237
; 
;     if (s_wSystemDelayTimerCounter)
	lds R2,_s_wSystemDelayTimerCounter
	lds R3,_s_wSystemDelayTimerCounter+1
	tst R2
	brne X1
	tst R3
	breq L10
X1:
	.dbline 238
;     {
	.dbline 239
;         s_wSystemDelayTimerCounter--;
	movw R24,R2
	sbiw R24,1
	sts _s_wSystemDelayTimerCounter+1,R25
	sts _s_wSystemDelayTimerCounter,R24
	.dbline 240
;     }
L10:
	.dbline 242
;     
;     if (!(g_wSystemTimer & (BIT(9) - 1)))
	lds R24,_g_wSystemTimer
	lds R25,_g_wSystemTimer+1
	andi R25,1
	cpi R24,0
	cpc R24,R25
	brne L12
X2:
	.dbline 243
;     {
	.dbline 244
;         PORTC ^= BIT(PC2);
	ldi R24,4
	in R2,0x8
	eor R2,R24
	out 0x8,R2
	.dbline 245
;     }
L12:
	.dbline -2
L9:
	.dbline 0 ; func end
	ret
	.dbend
	.area lit(rom, con, rel)
L4:
	.word 0x8000,0x4317
L3:
	.word 0xfdb,0x3bc9
; }
;