scon2.s

M8制作的舵机控制器

;         stopt1 -> y+2
;         stopt0 -> y+0
;         barke1 -> R20
;         barke0 -> R22
;           dst1 -> R10
;           dst0 -> R12
;        action1 -> R14
;        action0 -> y+20
;          temp1 -> y+18
;          temp0 -> y+16
;           adc1 -> y+14
;           adc0 -> y+12
;           pos1 -> y+10
;           pos0 -> y+8
	.even
_main::
	sbiw R28,21
	.dbline -1
	.dbline 184
; }
; 
; 
; 
; 
; 
; main()
; {
	.dbline 193
; Uchar dst0,dst1;//距离定点的数值
; Uint stopt0,stopt1;//刹车时间
; Uint adc0,adc1;//舵机电位
; Uint pos0,pos1;//舵机电位
; Uint temp0,temp1;//舵机电位
; Uchar barke0,barke1;//强力刹车，由全速运行转换到爬行需要强力刹车即长时间反向制动
; Uchar action0,action1;//电机动作
; 
; init_devices();
	rcall _init_devices
	rjmp L36
L35:
	.dbline 197
; 
; 
; while(1)
; {
	.dbline 198
; adc0=ReadADC (0);
	clr R16
	rcall _ReadADC
	std y+13,R17
	std y+12,R16
	.dbline 199
; adc1=ReadADC (1);
	ldi R16,1
	rcall _ReadADC
	std y+15,R17
	std y+14,R16
	.dbline 200
	ldd R24,y+16
	ldd R25,y+17
	adiw R24,1
	ldd R0,y+12
	ldd R1,y+13
	cp R24,R0
	cpc R25,R1
	brlo L40
	ldd R24,y+16
	ldd R25,y+17
	sbiw R24,1
	cp R0,R24
	cpc R1,R25
	brsh L38
L40:
	.dbline 200
; if((adc0>temp0+1)||(adc0<temp0-1))temp0=adc0;
	ldd R0,y+12
	ldd R1,y+13
	std y+17,R1
	std y+16,R0
L38:
	.dbline 201
	ldd R24,y+18
	ldd R25,y+19
	adiw R24,1
	ldd R0,y+14
	ldd R1,y+15
	cp R24,R0
	cpc R25,R1
	brlo L43
	ldd R24,y+18
	ldd R25,y+19
	sbiw R24,1
	cp R0,R24
	cpc R1,R25
	brsh L41
L43:
	.dbline 201
; if((adc1>temp1+1)||(adc1<temp1-1))temp1=adc1;
	ldd R0,y+14
	ldd R1,y+15
	std y+19,R1
	std y+18,R0
L41:
	.dbline 203
; 
; pos0=temp0>>2;
	ldd R2,y+16
	ldd R3,y+17
	lsr R3
	ror R2
	lsr R3
	ror R2
	std y+9,R3
	std y+8,R2
	.dbline 204
; pos1=temp1>>2;
	ldd R2,y+18
	ldd R3,y+19
	lsr R3
	ror R2
	lsr R3
	ror R2
	std y+11,R3
	std y+10,R2
	.dbline 206
; 
; switch(action0)
	ldd R2,y+20
	clr R3
	std y+5,R3
	std y+4,R2
	movw R24,R2
	cpi R24,1
	ldi R30,0
	cpc R25,R30
	breq L47
	cpi R24,2
	ldi R30,0
	cpc R25,R30
	breq L48
	cpi R24,3
	ldi R30,0
	cpc R25,R30
	breq L49
	rjmp L44
X5:
	.dbline 207
; {
L47:
	.dbline 208
; case 1:Forward0 break;
	sbi 0x18,0
	.dbline 208
	cbi 0x18,1
	.dbline 208
	rjmp L45
L48:
	.dbline 209
; case 2:Backward0 break;
	sbi 0x18,1
	.dbline 209
	cbi 0x18,0
	.dbline 209
	rjmp L45
L49:
	.dbline 210
; case 3:Stop0 break;
	in R24,0x18
	ori R24,3
	out 0x18,R24
	.dbline 210
L44:
L45:
	.dbline 213
; }
; 
; action0=3;
	ldi R24,3
	std y+20,R24
	.dbline 214
; if(pos0>(time1+2))
	lds R24,_time1
	lds R25,_time1+1
	adiw R24,2
	ldd R0,y+8
	ldd R1,y+9
	cp R24,R0
	cpc R25,R1
	brsh L50
	.dbline 215
; {action0=1 ;dst0=pos0-time1;
	.dbline 215
	clr R0
	inc R0
	std y+20,R0
	.dbline 215
	lds R2,_time1
	lds R3,_time1+1
	ldd R12,y+8
	ldd R13,y+9
	sub R12,R2
	sbc R13,R3
	.dbline 216
; if(dst0<15) //爬行状态，运转-反向制动-普通制动
	mov R24,R12
	cpi R24,15
	brsh L52
	.dbline 217
;  {
	.dbline 218
;  stopt0=dst0<<3;
	mov R2,R24
	clr R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	std y+1,R3
	std y+0,R2
	.dbline 219
;  if(!barke0)Delay (200+stopt0);//制动时正向马上关闭
	tst R22
	brne L54
	.dbline 219
	movw R16,R2
	subi R16,56  ; offset = 200
	sbci R17,255
	rcall _Delay
L54:
	.dbline 220
;  Backward0 
	sbi 0x18,1
	.dbline 220
	cbi 0x18,0
	.dbline 221
;   Delay (125-stopt0);//正常爬行的制动时间
	ldd R2,y+0
	ldd R3,y+1
	ldi R16,125
	ldi R17,0
	sub R16,R2
	sbc R17,R3
	rcall _Delay
	.dbline 222
;  Stop0
	in R24,0x18
	ori R24,3
	out 0x18,R24
	.dbline 223
; if(barke0)barke0--;
	tst R22
	breq L53
	.dbline 223
	dec R22
	.dbline 225
; 
;  }
	rjmp L53
L52:
	.dbline 226
;  else barke0=100;
	ldi R22,100
L53:
	.dbline 227
; }
L50:
	.dbline 229
; 
; if(pos0<(time1-2))
	lds R24,_time1
	lds R25,_time1+1
	sbiw R24,2
	ldd R0,y+8
	ldd R1,y+9
	cp R0,R24
	cpc R1,R25
	brsh L58
	.dbline 230
; {action0=2 ;dst0=time1-pos0;
	.dbline 230
	ldi R24,2
	std y+20,R24
	.dbline 230
	lds R12,_time1
	ldd R0,y+8
	ldd R1,y+9
	sub R12,R0
	sbc R13,R1
	.dbline 231
; if(dst0<15)  //爬行状态，运转-反向制动-普通制动
	mov R24,R12
	cpi R24,15
	brsh L60
	.dbline 232
;  {
	.dbline 233
;  stopt0=dst0<<3;
	mov R2,R24
	clr R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	std y+1,R3
	std y+0,R2
	.dbline 234
;  if(!barke0)Delay (200+stopt0);
	tst R22
	brne L62
	.dbline 234
	movw R16,R2
	subi R16,56  ; offset = 200
	sbci R17,255
	rcall _Delay
L62:
	.dbline 235
;  Forward0  
	sbi 0x18,0
	.dbline 235
	cbi 0x18,1
	.dbline 236
;   Delay (125-stopt0);//正常爬行的制动时间        
	ldd R2,y+0
	ldd R3,y+1
	ldi R16,125
	ldi R17,0
	sub R16,R2
	sbc R17,R3
	rcall _Delay
	.dbline 237
;  Stop0
	in R24,0x18
	ori R24,3
	out 0x18,R24
	.dbline 238
;  if(barke0)barke0--;
	tst R22
	breq L61
	.dbline 238
	dec R22
	.dbline 239
;  }
	rjmp L61
L60:
	.dbline 240
;  else barke0=100;
	ldi R22,100
L61:
	.dbline 241
; }
L58:
	.dbline 244
; 
; ///////////////////////////////////////////////////////////////////
; switch(action1)
	mov R2,R14
	clr R3
	std y+7,R3
	std y+6,R2
	movw R24,R2
	cpi R24,1
	ldi R30,0
	cpc R25,R30
	breq L69
	cpi R24,2
	ldi R30,0
	cpc R25,R30
	breq L70
	cpi R24,3
	ldi R30,0
	cpc R25,R30
	breq L71
	rjmp L66
X6:
	.dbline 245
; {
L69:
	.dbline 246
; case 1:Forward1 break;
	sbi 0x15,4
	.dbline 246
	cbi 0x15,5
	.dbline 246
	rjmp L67
L70:
	.dbline 247
; case 2:Backward1 break;
	sbi 0x15,5
	.dbline 247
	cbi 0x15,4
	.dbline 247
	rjmp L67
L71:
	.dbline 248
; case 3:Stop1 break;
	in R24,0x15
	ori R24,48
	out 0x15,R24
	.dbline 248
L66:
L67:
	.dbline 251
; }
; 
; action1=3;
	ldi R24,3
	mov R14,R24
	.dbline 252
; if(pos1>(time2+2))
	lds R24,_time2
	lds R25,_time2+1
	adiw R24,2
	ldd R0,y+10
	ldd R1,y+11
	cp R24,R0
	cpc R25,R1
	brsh L72
	.dbline 253
; {action1=1 ;dst1=pos1-time2;
	.dbline 253
	clr R14
	inc R14
	.dbline 253
	lds R2,_time2
	lds R3,_time2+1
	movw R10,R0
	sub R10,R2
	sbc R11,R3
	.dbline 254
; if(dst1<15) //爬行状态，运转-反向制动-普通制动
	mov R24,R10
	cpi R24,15
	brsh L74
	.dbline 255
;  {
	.dbline 256
;  stopt1=dst1<<3;
	mov R2,R24
	clr R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	std y+3,R3
	std y+2,R2
	.dbline 257
;  if(!barke1)Delay (200+stopt1);//制动时正向马上关闭
	tst R20
	brne L76
	.dbline 257
	movw R16,R2
	subi R16,56  ; offset = 200
	sbci R17,255
	rcall _Delay
L76:
	.dbline 258
;  Backward1 
	sbi 0x15,5
	.dbline 258
	cbi 0x15,4
	.dbline 259
;   Delay (125-stopt1);//正常爬行的制动时间
	ldd R2,y+2
	ldd R3,y+3
	ldi R16,125
	ldi R17,0
	sub R16,R2
	sbc R17,R3
	rcall _Delay
	.dbline 260
;  Stop1
	in R24,0x15
	ori R24,48
	out 0x15,R24
	.dbline 261
; if(barke1)barke1--;
	tst R20
	breq L75
	.dbline 261
	dec R20
	.dbline 263
; 
;  }
	rjmp L75
L74:
	.dbline 264
;  else barke1=100;
	ldi R20,100
L75:
	.dbline 265
; }
L72:
	.dbline 267
; 
; if(pos1<(time2-2))
	lds R24,_time2
	lds R25,_time2+1
	sbiw R24,2
	ldd R0,y+10
	ldd R1,y+11
	cp R0,R24
	cpc R1,R25
	brsh L80
	.dbline 268
; {action1=2 ;dst1=time2-pos1;
	.dbline 268
	ldi R24,2
	mov R14,R24
	.dbline 268
	lds R10,_time2
	sub R10,R0
	sbc R11,R1
	.dbline 269
; if(dst1<15)  //爬行状态，运转-反向制动-普通制动
	mov R24,R10
	cpi R24,15
	brsh L82
	.dbline 270
;  {
	.dbline 271
;  stopt1=dst1<<3;
	mov R2,R24
	clr R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	lsl R2
	rol R3
	std y+3,R3
	std y+2,R2
	.dbline 272
;  if(!barke1)Delay (200+stopt1);
	tst R20
	brne L84
	.dbline 272
	movw R16,R2
	subi R16,56  ; offset = 200
	sbci R17,255
	rcall _Delay
L84:
	.dbline 273
;  Forward1  
	sbi 0x15,4
	.dbline 273
	cbi 0x15,5
	.dbline 274
;   Delay (125-stopt1);//正常爬行的制动时间        
	ldd R2,y+2
	ldd R3,y+3
	ldi R16,125
	ldi R17,0
	sub R16,R2
	sbc R17,R3
	rcall _Delay
	.dbline 275
;  Stop1
	in R24,0x15
	ori R24,48
	out 0x15,R24
	.dbline 276
;  if(barke1)barke1--;
	tst R20
	breq L83
	.dbline 276
	dec R20
	.dbline 277
;  }
	rjmp L83
L82:
	.dbline 278
;  else barke1=100;
	ldi R20,100
L83:
	.dbline 279
; }
L80:
	.dbline 281
L36:
	.dbline 196
	rjmp L35
X7:
	.dbline -2
L34:
	adiw R28,21
	.dbline 0 ; func end
	ret
	.dbsym l stopt1 2 i
	.dbsym l stopt0 0 i
	.dbsym r barke1 20 c
	.dbsym r barke0 22 c
	.dbsym r dst1 10 c
	.dbsym r dst0 12 c
	.dbsym r action1 14 c
	.dbsym l action0 20 c
	.dbsym l temp1 18 i
	.dbsym l temp0 16 i
	.dbsym l adc1 14 i
	.dbsym l adc0 12 i
	.dbsym l pos1 10 i
	.dbsym l pos0 8 i
	.dbend