robbot.s

寻迹机械人系统的设计,这是能自行行动的机械人的设计.

	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	mov R30,R28
	mov R31,R29
 ; stack offset 4
	ldd R2,z+4
	ldd R3,z+5
	ldd R4,z+6
	ldd R5,z+7
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	ldi R16,<L27
	ldi R17,>L27
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	rcall div32fs
	rcall mpy32f
	rcall fp2int
	mov R12,R16
	mov R13,R17
	.dbline 140
;    temp1=GetOutputComReg1B();
	rcall _GetOutputComReg1B
	mov R10,R16
	mov R11,R17
	.dbline 141
;    temp1=temp1+z;
	add R10,R12
	adc R11,R13
	.dbline 142
;    SetOutputComReg1B(temp1);
	mov R16,R10
	mov R17,R11
	rcall _SetOutputComReg1B
	.dbline 143
;    EkB_2=EkB_1;
	lds R4,_EkB_1+2
	lds R5,_EkB_1+2+1
	lds R2,_EkB_1
	lds R3,_EkB_1+1
	sts _EkB_2+1,R3
	sts _EkB_2,R2
	sts _EkB_2+2+1,R5
	sts _EkB_2+2,R4
	.dbline 144
;    EkB_1=EkB;}
	lds R4,_EkB+2
	lds R5,_EkB+2+1
	lds R2,_EkB
	lds R3,_EkB+1
	sts _EkB_1+1,R3
	sts _EkB_1,R2
	sts _EkB_1+2+1,R5
	sts _EkB_1+2,R4
	.dbline 144
L30:
	.dbline 145
;  TCNT2=0x00;
	clr R2
	out 0x24,R2
	.dbline -2
	.dbline 146
; }
L28:
	adiw R28,4
	rcall pop_gset4
	.dbline 0 ; func end
	ret
	.dbsym r z 12 I
	.dbsym l u 0 D
	.dbsym r y 10 c
	.dbsym r temp1 10 I
	.dbend
	.dbfunc e Talone _Talone fc
	.even
_Talone::
	.dbline -1
	.dbline 150
; 
; //***************************寻线处理函数1***************************//
; char Talone()              
; {if ((forlight&0x09)==0)     //3单独亮
	.dbline 150
	lds R24,_forlight
	andi R24,9
	brne L34
	.dbline 151
;     return(0);
	clr R16
	rjmp L33
L34:
	.dbline 153
;  else
;     return(1);               //返回为校偏状态值 
	ldi R16,1
	.dbline -2
L33:
	.dbline 0 ; func end
	ret
	.dbend
	.dbfunc e FUalone _FUalone fc
	.even
_FUalone::
	.dbline -1
	.dbline 158
; }
; 
; //***************************寻线处理函数2***************************//
; char FUalone()               //4单独亮
; {if ((forlight&0x05)==0)
	.dbline 158
	lds R24,_forlight
	andi R24,5
	brne L37
	.dbline 159
;     return(0);
	clr R16
	rjmp L36
L37:
	.dbline 161
;  else
;     return(2);
	ldi R16,2
	.dbline -2
L36:
	.dbline 0 ; func end
	ret
	.dbend
	.dbfunc e FIalone _FIalone fc
	.even
_FIalone::
	.dbline -1
	.dbline 166
; }
; 
; //***************************寻线处理函数3***************************//
; char FIalone()               //5单独亮
; {if ((forlight&0x22)==0)
	.dbline 166
	lds R24,_forlight
	andi R24,34
	brne L40
	.dbline 167
;     return(0);
	clr R16
	rjmp L39
L40:
	.dbline 169
;  else
;     return(1);
	ldi R16,1
	.dbline -2
L39:
	.dbline 0 ; func end
	ret
	.dbend
	.dbfunc e Salone _Salone fc
	.even
_Salone::
	.dbline -1
	.dbline 174
; }
; 
; //***************************寻线处理函数4***************************//
; char Salone()                //6单独亮
; {if ((forlight&0x12)==0)
	.dbline 174
	lds R24,_forlight
	andi R24,18
	brne L43
	.dbline 175
;     return(0);
	clr R16
	rjmp L42
L43:
	.dbline 177
;  else
;     return(2);
	ldi R16,2
	.dbline -2
L42:
	.dbline 0 ; func end
	ret
	.dbend
	.dbfunc e InLine1 _InLine1 fc
;    discrepancy -> R16
;             l2 -> R16
	.even
_InLine1::
	.dbline -1
	.dbline 182
; }
; 
; //***************************寻线处理函数5****************************//
; char InLine1(void)           //线上偏离判断
; {char discrepancy;           //行进状态
	.dbline 184
;  char l2;                    //传感器状态
;  l2=0xFF^PINA;
	ldi R24,255
	ldi R25,0
	in R16,0x19
	clr R17
	eor R16,R24
	eor R17,R25
	.dbline 185
;  l2=0x3C&l2;
	andi R16,60
	.dbline 186
;  l2=l2>>2;
	lsr R16
	lsr R16
	.dbline 187
;  switch(l2)                  
	clr R17
	cpi R16,0
	cpc R16,R17
	breq L49
X0:
	cpi R16,1
	ldi R30,0
	cpc R17,R30
	breq L50
	cpi R16,2
	ldi R30,0
	cpc R17,R30
	breq L51
	cpi R16,4
	ldi R30,0
	cpc R17,R30
	breq L52
	cpi R16,5
	ldi R30,0
	cpc R17,R30
	breq L53
	cpi R16,6
	ldi R30,0
	cpc R17,R30
	breq L54
	cpi R16,8
	ldi R30,0
	cpc R17,R30
	breq L55
	cpi R16,9
	ldi R30,0
	cpc R17,R30
	breq L56
	cpi R16,10
	ldi R30,0
	cpc R17,R30
	breq L57
	rjmp L46
X1:
	.dbline 188
L49:
	.dbline 188
;   {case 0x00:    discrepancy=0;   break;         //0000b未偏
	clr R16
	.dbline 188
	rjmp L47
L50:
	.dbline 189
;    case 0x01:    discrepancy=0;   break;         
	clr R16
	.dbline 189
	rjmp L47
L51:
	.dbline 190
;    case 0x02:    discrepancy=0;   break;         
	clr R16
	.dbline 190
	rjmp L47
L52:
	.dbline 191
;    case 0x04:    discrepancy=2;   break;         //0010b左偏
	ldi R16,2
	.dbline 191
	rjmp L47
L53:
	.dbline 192
;    case 0x05:    discrepancy=2;   break;        
	ldi R16,2
	.dbline 192
	rjmp L47
L54:
	.dbline 193
;    case 0x06:    discrepancy=2;   break;        
	ldi R16,2
	.dbline 193
	rjmp L47
L55:
	.dbline 194
;    case 0x08:    discrepancy=1;   break;         //0001b右偏
	ldi R16,1
	.dbline 194
	rjmp L47
L56:
	.dbline 195
;    case 0x09:    discrepancy=1;   break;        
	ldi R16,1
	.dbline 195
	rjmp L47
L57:
	.dbline 196
;    case 0x0A:    discrepancy=1;   break;}
	ldi R16,1
	.dbline 196
L46:
L47:
	.dbline 197
;  return(discrepancy); 
	.dbline -2
L45:
	.dbline 0 ; func end
	ret
	.dbsym r discrepancy 16 c
	.dbsym r l2 16 c
	.dbend
	.dbfunc e InLine2 _InLine2 fc
;    discrepancy -> R16
;             l2 -> R16
	.even
_InLine2::
	.dbline -1
	.dbline 202
; }
; 
; //***************************寻线处理函数6****************************//
; char InLine2(void)            //线上偏离判断
; {char discrepancy;
	.dbline 204
;  char l2;
;  l2=0xFF^PINA;
	ldi R24,255
	ldi R25,0
	in R16,0x19
	clr R17
	eor R16,R24
	eor R17,R25
	.dbline 205
;  l2=0x3C&l2;//3,4,5,6灯
	andi R16,60
	.dbline 206
;  l2=l2>>2;
	lsr R16
	lsr R16
	.dbline 207
;  switch(l2)
	clr R17
	cpi R16,0
	cpc R16,R17
	breq L62
X2:
	cpi R16,1
	ldi R30,0
	cpc R17,R30
	breq L63
	cpi R16,2
	ldi R30,0
	cpc R17,R30
	breq L64
	cpi R16,4
	ldi R30,0
	cpc R17,R30
	breq L65
	cpi R16,5
	ldi R30,0
	cpc R17,R30
	breq L66
	cpi R16,6
	ldi R30,0
	cpc R17,R30
	breq L67
	cpi R16,8
	ldi R30,0
	cpc R17,R30
	breq L68
	cpi R16,9
	ldi R30,0
	cpc R17,R30
	breq L69
	cpi R16,10
	ldi R30,0
	cpc R17,R30
	breq L70
	rjmp L59
X3:
	.dbline 208
L62:
	.dbline 208
;   {case 0x00:    discrepancy=0;   break;         //0000b//未偏
	clr R16
	.dbline 208
	rjmp L60
L63:
	.dbline 209
;    case 0x01:    discrepancy=1;   break;         //0001b右偏
	ldi R16,1
	.dbline 209
	rjmp L60
L64:
	.dbline 210
;    case 0x02:    discrepancy=2;   break;         //0010b左偏
	ldi R16,2
	.dbline 210
	rjmp L60
L65:
	.dbline 211
;    case 0x04:    discrepancy=0;   break;  
	clr R16
	.dbline 211
	rjmp L60
L66:
	.dbline 212
;    case 0x05:    discrepancy=1;   break;  
	ldi R16,1
	.dbline 212
	rjmp L60
L67:
	.dbline 213
;    case 0x06:    discrepancy=2;   break;  
	ldi R16,2
	.dbline 213
	rjmp L60
L68:
	.dbline 214
;    case 0x08:    discrepancy=0;   break;  
	clr R16
	.dbline 214
	rjmp L60
L69:
	.dbline 215
;    case 0x09:    discrepancy=1;   break;  
	ldi R16,1
	.dbline 215
	rjmp L60
L70:
	.dbline 216
;    case 0x0A:    discrepancy=2;   break;}
	ldi R16,2
	.dbline 216
L59:
L60:
	.dbline 217
;  return(discrepancy);  
	.dbline -2
L58:
	.dbline 0 ; func end
	ret
	.dbsym r discrepancy 16 c
	.dbsym r l2 16 c
	.dbend
	.dbfunc e OutLine _OutLine fc
;    discrepancy -> R20
;             l2 -> R20
	.even
_OutLine::
	rcall push_gset1
	.dbline -1
	.dbline 222
;  }
;  
; //***************************寻线处理函数7****************************//
; char OutLine(void)           //线外偏离判断
; {char discrepancy;
	.dbline 224
;  char l2;
;  l2=PINA^0xFF; 
	ldi R24,255
	ldi R25,0
	in R20,0x19
	clr R21
	eor R20,R24
	eor R21,R25
	.dbline 225
;  l2=0x3C&l2;                 //00111100b
	andi R20,60
	.dbline 226
;  l2=l2>>2;
	lsr R20
	lsr R20
	.dbline 227
;  switch(l2)
	clr R21
	cpi R20,0
	cpc R20,R21
	breq L75
X4:
	cpi R20,1
	ldi R30,0
	cpc R21,R30
	breq L76
	cpi R20,2
	ldi R30,0
	cpc R21,R30
	breq L77
	cpi R20,4
	ldi R30,0
	cpc R21,R30
	breq L78
	cpi R20,5
	ldi R30,0
	cpc R21,R30
	breq L79
	cpi R20,6
	ldi R30,0
	cpc R21,R30
	breq L80
	cpi R20,8
	ldi R30,0
	cpc R21,R30
	breq L81
	cpi R20,9
	ldi R30,0
	cpc R21,R30
	breq L82
	cpi R20,10
	ldi R30,0
	cpc R21,R30
	breq L83
	cpi R20,15
	ldi R30,0
	cpc R21,R30
	breq L84
	rjmp L72
X5:
	.dbline 228
L75:
	.dbline 228
;   {case 0x00:    discrepancy=forflag;    break;  //0000b
	lds R20,_forflag
	.dbline 228
	rjmp L73
L76:
	.dbline 229
;    case 0x01:    discrepancy=Talone();   break;  //0001b
	rcall _Talone
	mov R20,R16
	.dbline 229
	rjmp L73
L77:
	.dbline 230
;    case 0x02:    discrepancy=FUalone();  break;  //0010b
	rcall _FUalone
	mov R20,R16
	.dbline 230
	rjmp L73
L78:
	.dbline 231
;    case 0x04:    discrepancy=FIalone();  break;  //0100b
	rcall _FIalone
	mov R20,R16
	.dbline 231
	rjmp L73
L79:
	.dbline 232
;    case 0x05:    discrepancy=0;          break;  //0101b
	clr R20
	.dbline 232
	rjmp L73
L80:
	.dbline 233
;    case 0x06:    discrepancy=0;          break;  //0110b
	clr R20
	.dbline 233
	rjmp L73
L81:
	.dbline 234
;    case 0x08:    discrepancy=Salone();   break;  //1000b
	rcall _Salone
	mov R20,R16
	.dbline 234
	rjmp L73
L82:
	.dbline 235
;    case 0x09:    discrepancy=0;          break;  //1001b
	clr R20
	.dbline 235
	rjmp L73
L83:
	.dbline 236
;    case 0x0A:    discrepancy=0;          break;  //1010b
	clr R20
	.dbline 236
	rjmp L73
L84:
	.dbline 237
;    case 0x0F:    discrepancy=0;          break;  //1111b
	clr R20
	.dbline 237
L72:
L73:
	.dbline 239
;   }
;   return(discrepancy);
	mov R16,R20
	.dbline -2
L71:
	rcall pop_gset1
	.dbline 0 ; func end
	ret
	.dbsym r discrepancy 20 c
	.dbsym r l2 20 c
	.dbend
	.dbfunc e Revise _Revise fV
;    discrepancy -> R20
	.even
_Revise::
	rcall push_gset1
	mov R20,R16
	.dbline -1
	.dbline 244
; }  
; 
; //***************************校偏函数********************************//
; void Revise(char discrepancy)
; {if(discrepancy==0)
	.dbline 244
	tst R20
	brne L86
	.dbline 245
;   {forflag=0;
	.dbline 245
	clr R2
	sts _forflag,R2
	.dbline 246
;    if(fob==1)
	lds R24,_fob
	cpi R24,1
	brne L88
	.dbline 247
; 	{PORTB=0x00;
	.dbline 247
	out 0x18,R2
	.dbline 248
;    return;}
	rjmp L85
L88:
	.dbline 249
;    PORTB=0x0C;
	ldi R24,12
	out 0x18,R24
	.dbline 250
;    return;}
	rjmp L85
L86:
	.dbline 251
;  if(discrepancy==1)
	cpi R20,1
	brne L90
	.dbline 252
;   {PORTB=0x08;               //A为左边电机
	.dbline 252
	ldi R24,8
	out 0x18,R24
	.dbline 253
;    delay(0x0100);
	ldi R16,256
	ldi R17,1
	rcall _delay
	.dbline 254
;    PORTB=0x0C;
	ldi R24,12
	out 0x18,R24
	.dbline 255
;    forflag=1;
	ldi R24,1
	sts _forflag,R24
	.dbline 256
;    return;}
	rjmp L85
L90:
	.dbline 257
;   if(discrepancy==2)         //B为右边电机
	cpi R20,2
	brne L92
	.dbline 258
;    {forflag=2;
	.dbline 258
	ldi R24,2
	sts _forflag,R24
	.dbline 259
;     PORTB=0x04;
	ldi R24,4
	out 0x18,R24
	.dbline 260
;     delay(0x0100);
	ldi R16,256
	ldi R17,1
	rcall _delay
	.dbline 261
;     PORTB=0x0C;
	ldi R24,12
	out 0x18,R24
	.dbline 262
;     return;}
	rjmp L85
L92:
	.dbline 263
;   if(discrepancy==3)
	cpi R20,3
	brne L94
	.dbline 264
;    {PORTB=0x08;              //A为左边电机
	.dbline 264
	ldi R24,8
	out 0x18,R24
	.dbline 265
;   	forflag=1;
	ldi R24,1
	sts _forflag,R24
	.dbline 266
;     return;}
	rjmp L85
L94:
	.dbline 267
	cpi R20,4
	brne L96
	.dbline 268
	.dbline 268
	ldi R24,2
	sts _forflag,R24
	.dbline 269
	ldi R24,4
	out 0x18,R24
	.dbline 270
L96:
	.dbline -2
	.dbline 271
;   if(discrepancy==4)
;    {forflag=2;
;     PORTB=0x04;
;     return;}
; }
L85:
	rcall pop_gset1
	.dbline 0 ; func end
	ret
	.dbsym r discrepancy 20 c
	.dbend
	.dbfunc e SearchLine _SearchLine fV
;              y -> R10
;         online -> R20
;            l78 -> R22
	.even
_SearchLine::
	rcall push_gset3
	.dbline -1
	.dbline 275
; 
; //***************************寻线函数********************************//
; void SearchLine()           
; {char online;               //测点状态寄存器
	.dbline 277
;  char l78;                  //7，8号测点状态寄存器
;  char y=0;
	clr R10
	.dbline 278
;  online=PINA^0xFF;
	ldi R24,255
	ldi R25,0
	in R20,0x19
	clr R21
	eor R20,R24
	eor R21,R25
	.dbline 279
;  forlight=online;
	sts _forlight,R20
	.dbline 280
;  l78=online&0xC0;    
	mov R22,R20
	andi R22,192
	.dbline 281
;  l78=l78>>6;
	lsr R22
	lsr R22
	lsr R22
	lsr R22
	lsr R22
	lsr R22
	.dbline 282
;  if(l78==0x03)               //7，8同时亮无偏
	cpi R22,3
	brne L99
	.dbline 283
;     Revise(0);
	clr R16
	rcall _Revise
L99:
	.dbline 284
;  if((online&0x0D)==0x0D)     //1，3，4都亮，直走
	mov R24,R20
	andi R24,13
	cpi R24,13
	brne L101
	.dbline 285
;   {Revise(0);                //校偏函数
	.dbline 285
	clr R16
	rcall _Revise
	.dbline 286
;    y=1;}
	clr R10
	inc R10
	.dbline 286
L101:
	.dbline 287
;   if(y==0)
	tst R10
	brne L103
	.dbline 288
;    {if(l78==0x01)
	.dbline 288
	cpi R22,1
	brne L105
	.dbline 289
;       Revise(3);             //3为大右偏
	ldi R16,3
	rcall _Revise
L105:
	.dbline 290
;     if(l78==0x02)
	cpi R22,2
	brne L107
	.dbline 291
;       Revise(4);             //4为大左偏
	ldi R16,4
	rcall _Revise
L107:
	.dbline 292
;     if(l78==0x00)
	tst R22
	brne L109
	.dbline 293
;      {online=0x03&online;
	.dbline 293
	andi R20,3