robbot.s

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

	.module Robbot.c
	.area data(ram, con, rel)
_sflag::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e sflag _sflag c
_crossflag::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e crossflag _crossflag c
_forflag::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e forflag _forflag c
_EkA_1::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e EkA_1 _EkA_1 D
_EkA_2::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e EkA_2 _EkA_2 D
_EkB_1::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e EkB_1 _EkB_1 D
_EkB_2::
	.blkb 4
	.area idata
	.word 0x0,0x0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e EkB_2 _EkB_2 D
_flage::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e flage _flage c
_a::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e a _a c
_c::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e c _c c
_desireV::
	.blkb 2
	.area idata
	.word 10
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e desireV _desireV I
_b::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e b _b c
_time::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e time _time c
_fob::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e fob _fob c
_Task::
	.blkb 2
	.area idata
	.byte 55,39
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.blkb 2
	.area idata
	.byte 27,41
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.blkb 2
	.area idata
	.byte 35,27
	.area data(ram, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbsym e Task _Task A[6:6]c
	.area text(rom, con, rel)
	.dbfile H:\第八章源程序\Robbot.c
	.dbfunc e Timer1Init _Timer1Init fV
;           sreg -> R20
;      temptccrB -> y+2
;      temptccrA -> R18,R19
;      temptimsk -> R16,R17
	.even
_Timer1Init::
	rcall push_gset1
	.dbline -1
	.dbline 42
; //***************************FileName:Robbot.c***********************//
; //***************************ICCAVR6.30编译**************************//
; 
; #include <io8535v.h>
; #include <macros.h>
; 
; //****************************宏定义********************************//
; #define Aa 0.5               //PID参数
; #define Ba -0.5              //PID参数
; #define Ca 0                 //PID参数
; #define Ab 0.09              //PID参数
; #define Bb -0.1              //PID参数
; #define Cb 0                 //PID参数
; #define U1 12                //PID参数
; #define MaxSpeed 0x40        //最大速度
; #define MidSpeed 0x18        //中速,用于转弯
; #define Size 6               //任务数组大小
; 
; //*****************************全局变量定义**************************//
; char  sflag=0x00;            //记录上一次校偏状态
; char  crossflag = 0;         //过线标志，用于判断是否过线
; char forflag=0;              //记录上一次机器人行进状态
; char forlight;               //记录上一次A口光电传感器的状态
; float EkA;                   //本次左边电机速度误差
; float EkA_1=0;               //上次左边电机速度误差
; float EkA_2=0;               //上上次左边电机速度误差
; float EkB;                   //本次左边电机速度误差
; float EkB_1=0;               //上次左边电机速度误差
; float EkB_2=0;               //上上次左边电机速度误差
; char  flage=0;
; char  a=0;                   //溢出次数，控制PID窗口时间
; char  c=0;                   //控制寻线频率
; int   desireV=10;
; char  b=0;
; char time=0;                 //机器人行走步数
; char  fob=0;                 //=1后退，=0前进
; char Task[Size]={0x37,0x27,0x1B,0x29,0x23,0x1B};//任务数据数组
; 
; 
; //******************************定时器1初始化*************************//
; void Timer1Init(int temptimsk,int temptccrA,int temptccrB)
; {unsigned char sreg;
	.dbline 43
;  TIMSK = temptimsk;  
	out 0x39,R16
	.dbline 44
;   sreg = SREG;		  	     //保存全局中断标志
	in R20,0x3f
	.dbline 45
;   _CLI();					 //屏蔽所有中断
	cli
	.dbline 46
;   TCCR1A = temptccrA;
	out 0x2f,R18
	.dbline 47
;   TCCR1B = temptccrB;
	ldd R2,y+2
	ldd R3,y+3
	out 0x2e,R2
	.dbline 48
;   SREG = sreg;	   		     //恢复全局中断标志
	out 0x3f,R20
	.dbline -2
	.dbline 49
; }
L1:
	rcall pop_gset1
	.dbline 0 ; func end
	ret
	.dbsym r sreg 20 c
	.dbsym l temptccrB 2 I
	.dbsym r temptccrA 18 I
	.dbsym r temptimsk 16 I
	.dbend
	.dbfunc e SetOutputComReg1A _SetOutputComReg1A fV
;           sreg -> R20
;        tempocr -> R16,R17
	.even
_SetOutputComReg1A::
	rcall push_gset1
	.dbline -1
	.dbline 53
; 
; //******************************写OCR1A寄存器**************************//
; void SetOutputComReg1A(int tempocr)
; {unsigned char sreg;
	.dbline 54
;  sreg = SREG;
	in R20,0x3f
	.dbline 55
;   _CLI();
	cli
	.dbline 56
;   OCR1A = tempocr;
	out 0x2b,R17
	out 0x2a,R16
	.dbline 57
;   SREG = sreg;
	out 0x3f,R20
	.dbline -2
	.dbline 58
; }
L2:
	rcall pop_gset1
	.dbline 0 ; func end
	ret
	.dbsym r sreg 20 c
	.dbsym r tempocr 16 I
	.dbend
	.dbfunc e SetOutputComReg1B _SetOutputComReg1B fV
;           sreg -> R20
;        tempocr -> R16,R17
	.even
_SetOutputComReg1B::
	rcall push_gset1
	.dbline -1
	.dbline 63
; 
; 
; //******************************写OCR1B寄存器**************************//
; void SetOutputComReg1B(int tempocr)
; {unsigned char sreg;
	.dbline 64
;  sreg = SREG;
	in R20,0x3f
	.dbline 65
;  _CLI();
	cli
	.dbline 66
;  OCR1B = tempocr;
	out 0x29,R17
	out 0x28,R16
	.dbline 67
;  SREG = sreg;
	out 0x3f,R20
	.dbline -2
	.dbline 68
; }
L3:
	rcall pop_gset1
	.dbline 0 ; func end
	ret
	.dbsym r sreg 20 c
	.dbsym r tempocr 16 I
	.dbend
	.dbfunc e GetOutputComReg1A _GetOutputComReg1A fI
;           temp -> R16,R17
	.even
_GetOutputComReg1A::
	.dbline -1
	.dbline 73
; 
; 
; //******************************读OCR1A寄存器**************************//
; int GetOutputComReg1A()
; {int temp;
	.dbline 74
;  temp = OCR1A;
	in R16,0x2a
	in R17,0x2b
	.dbline 75
;  return(temp);
	.dbline -2
L4:
	.dbline 0 ; func end
	ret
	.dbsym r temp 16 I
	.dbend
	.dbfunc e GetOutputComReg1B _GetOutputComReg1B fI
;           temp -> R16,R17
	.even
_GetOutputComReg1B::
	.dbline -1
	.dbline 80
; }
; 
; //******************************读OCR1B寄存器**************************//
; int GetOutputComReg1B()
; {int temp;
	.dbline 81
;  temp = OCR1B;
	in R16,0x28
	in R17,0x29
	.dbline 82
;  return(temp);
	.dbline -2
L5:
	.dbline 0 ; func end
	ret
	.dbsym r temp 16 I
	.dbend
	.dbfunc e DELAY _DELAY fV
;              i -> R20,R21
;              j -> R22,R23
;      delaytime -> R16,R17
	.even
_DELAY::
	rcall push_gset2
	.dbline -1
	.dbline 87
; }
; 
; //******************************长延时函数***************************//
; void DELAY(int delaytime)
; {int i,j;
	.dbline 88
;  for(i=0;i<=delaytime;i++)
	clr R20
	clr R21
	rjmp L10
L7:
	.dbline 89
	.dbline 89
	clr R22
	clr R23
	rjmp L14
L11:
	.dbline 89
L12:
	.dbline 89
	subi R22,255  ; offset = 1
	sbci R23,255
L14:
	.dbline 89
	ldi R24,65534
	ldi R25,255
	cp R24,R22
	cpc R25,R23
	brsh L11
	.dbline 89
L8:
	.dbline 88
	subi R20,255  ; offset = 1
	sbci R21,255
L10:
	.dbline 88
	cp R16,R20
	cpc R17,R21
	brge L7
	.dbline -2
	.dbline 90
;   {for (j=0;j<=0xFFFE;j++) ;} 
; }
L6:
	rcall pop_gset2
	.dbline 0 ; func end
	ret
	.dbsym r i 20 I
	.dbsym r j 22 I
	.dbsym r delaytime 16 I
	.dbend
	.dbfunc e delay _delay fV
;              j -> R20,R21
;              i -> R16,R17
	.even
_delay::
	rcall push_gset1
	.dbline -1
	.dbline 94
; 
; //******************************短延时函数***************************//
; void delay(int i)
; {int j;
	.dbline 95
	clr R20
	clr R21
	rjmp L19
L16:
	.dbline 95
L17:
	.dbline 95
	subi R20,255  ; offset = 1
	sbci R21,255
L19:
	.dbline 95
;  for(j=0;j<=i;j++)  ;
	cp R16,R20
	cpc R17,R21
	brge L16
	.dbline -2
	.dbline 96
; }
L15:
	rcall pop_gset1
	.dbline 0 ; func end
	ret
	.dbsym r j 20 I
	.dbsym r i 16 I
	.dbend
	.dbfunc e PIDA _PIDA fV
;              z -> R12,R13
;              u -> y+0
;              y -> R10
;          temp1 -> R10,R11
	.even
_PIDA::
	rcall push_gset4
	sbiw R28,4
	.dbline -1
	.dbline 100
; 
; //******************************PID调节:左电机***********************//
; void PIDA()                  
; {char  y;                    //本次采样速度值
	.dbline 104
;  float u;                    //电压差值
;  int   z;                    //本次输出增量
;  int temp1;                  //临时记录值
;  y = TCNT1;
	in R10,0x2c
	.dbline 105
;  EkA=y-desireV;
	lds R2,_desireV
	lds R3,_desireV+1
	mov R16,R10
	clr R17
	sub R16,R2
	sbc R17,R3
	rcall int2fp
	sts _EkA+1,R17
	sts _EkA,R16
	sts _EkA+2+1,R19
	sts _EkA+2,R18
	.dbline 106
;  if (EkA==(0-desireV))       //当Ek大于某一值时直接加最大
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R2,_desireV
	lds R3,_desireV+1
	clr R16
	clr R17
	sub R16,R2
	sbc R17,R3
	rcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	rcall cmp32f
	brne L21
	.dbline 107
;   {temp1=0x0000;
	.dbline 107
	clr R10
	clr R11
	.dbline 108
;    SetOutputComReg1A(temp1);}
	mov R16,R10
	mov R17,R11
	rcall _SetOutputComReg1A
	.dbline 108
	rjmp L22
L21:
	.dbline 110
;  else
;   {EkA=Aa*EkA;
	.dbline 110
	ldi R16,<L23
	ldi R17,>L23
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R4,_EkA+2
	lds R5,_EkA+2+1
	lds R2,_EkA
	lds R3,_EkA+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall mpy32f
	sts _EkA+1,R17
	sts _EkA,R16
	sts _EkA+2+1,R19
	sts _EkA+2,R18
	.dbline 111
;    EkA_1=Ba*EkA_1;
	ldi R16,<L24
	ldi R17,>L24
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R4,_EkA_1+2
	lds R5,_EkA_1+2+1
	lds R2,_EkA_1
	lds R3,_EkA_1+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall mpy32f
	sts _EkA_1+1,R17
	sts _EkA_1,R16
	sts _EkA_1+2+1,R19
	sts _EkA_1+2,R18
	.dbline 112
;    EkA_2=Ca*EkA_2;
	ldi R16,<L25
	ldi R17,>L25
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R4,_EkA_2+2
	lds R5,_EkA_2+2+1
	lds R2,_EkA_2
	lds R3,_EkA_2+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall mpy32f
	sts _EkA_2+1,R17
	sts _EkA_2,R16
	sts _EkA_2+2+1,R19
	sts _EkA_2+2,R18
	.dbline 113
;    u=EkA+EkA_1+EkA_2;
	lds R4,_EkA+2
	lds R5,_EkA+2+1
	lds R2,_EkA
	lds R3,_EkA+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	lds R4,_EkA_1+2
	lds R5,_EkA_1+2+1
	lds R2,_EkA_1
	lds R3,_EkA_1+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall add32fs
	lds R4,_EkA_2+2
	lds R5,_EkA_2+2+1
	lds R2,_EkA_2
	lds R3,_EkA_2+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall add32f
	mov R30,R28
	mov R31,R29
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 114
;    z=u/U1*0x03FF;
	ldi R16,<L26
	ldi R17,>L26
	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 115
;    temp1=GetOutputComReg1A();
	rcall _GetOutputComReg1A
	mov R10,R16
	mov R11,R17
	.dbline 116
;    temp1=temp1+z;
	add R10,R12
	adc R11,R13
	.dbline 117
;    SetOutputComReg1A(temp1);
	mov R16,R10
	mov R17,R11
	rcall _SetOutputComReg1A
	.dbline 118
;    EkA_2=EkA_1;
	lds R4,_EkA_1+2
	lds R5,_EkA_1+2+1
	lds R2,_EkA_1
	lds R3,_EkA_1+1
	sts _EkA_2+1,R3
	sts _EkA_2,R2
	sts _EkA_2+2+1,R5
	sts _EkA_2+2,R4
	.dbline 119
;    EkA_1=EkA;}
	lds R4,_EkA+2
	lds R5,_EkA+2+1
	lds R2,_EkA
	lds R3,_EkA+1
	sts _EkA_1+1,R3
	sts _EkA_1,R2
	sts _EkA_1+2+1,R5
	sts _EkA_1+2,R4
	.dbline 119
L22:
	.dbline 120
;  TCNT1=0x00;
	clr R2
	clr R3
	out 0x2d,R3
	out 0x2c,R2
	.dbline -2
	.dbline 121
; }
L20:
	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 PIDB _PIDB fV
;              z -> R12,R13
;              u -> y+0
;              y -> R10
;          temp1 -> R10,R11
	.even
_PIDB::
	rcall push_gset4
	sbiw R28,4
	.dbline -1
	.dbline 125
; 
; //******************************PID调节:右电机***********************//
; void PIDB()
; {char  y;
	.dbline 129
;  float u;                    //电压差值
;  int   z;
;  int temp1;
;  y = TCNT2;
	in R10,0x24
	.dbline 130
;  EkB=y-desireV;
	lds R2,_desireV
	lds R3,_desireV+1
	mov R16,R10
	clr R17
	sub R16,R2
	sbc R17,R3
	rcall int2fp
	sts _EkB+1,R17
	sts _EkB,R16
	sts _EkB+2+1,R19
	sts _EkB+2,R18
	.dbline 131
;  if(EkB==(0-desireV))      //当Ek大于某一值时直接加最大//
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R2,_desireV
	lds R3,_desireV+1
	clr R16
	clr R17
	sub R16,R2
	sbc R17,R3
	rcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	rcall cmp32f
	brne L29
	.dbline 132
;   {temp1=0x0000;
	.dbline 132
	clr R10
	clr R11
	.dbline 133
;    SetOutputComReg1B(temp1);}
	mov R16,R10
	mov R17,R11
	rcall _SetOutputComReg1B
	.dbline 133
	rjmp L30
L29:
	.dbline 135
;  else
;   {EkB =Ab*EkB;
	.dbline 135
	ldi R16,<L31
	ldi R17,>L31
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R4,_EkB+2
	lds R5,_EkB+2+1
	lds R2,_EkB
	lds R3,_EkB+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall mpy32f
	sts _EkB+1,R17
	sts _EkB,R16
	sts _EkB+2+1,R19
	sts _EkB+2,R18
	.dbline 136
;    EkB_1=Bb*EkB_1;
	ldi R16,<L32
	ldi R17,>L32
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R4,_EkB_1+2
	lds R5,_EkB_1+2+1
	lds R2,_EkB_1
	lds R3,_EkB_1+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall mpy32f
	sts _EkB_1+1,R17
	sts _EkB_1,R16
	sts _EkB_1+2+1,R19
	sts _EkB_1+2,R18
	.dbline 137
;    EkB_2=Cb*EkB_2;
	ldi R16,<L25
	ldi R17,>L25
	rcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R4,_EkB_2+2
	lds R5,_EkB_2+2+1
	lds R2,_EkB_2
	lds R3,_EkB_2+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall mpy32f
	sts _EkB_2+1,R17
	sts _EkB_2,R16
	sts _EkB_2+2+1,R19
	sts _EkB_2+2,R18
	.dbline 138
;    u=EkB+EkB_1+EkB_2;
	lds R4,_EkB+2
	lds R5,_EkB+2+1
	lds R2,_EkB
	lds R3,_EkB+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	lds R4,_EkB_1+2
	lds R5,_EkB_1+2+1
	lds R2,_EkB_1
	lds R3,_EkB_1+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall add32fs
	lds R4,_EkB_2+2
	lds R5,_EkB_2+2+1
	lds R2,_EkB_2
	lds R3,_EkB_2+1
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	rcall add32f
	mov R30,R28
	mov R31,R29
	std z+0,R16
	std z+1,R17
	std z+2,R18
	std z+3,R19
	.dbline 139
;    z=u/U1*0x03FF;
	ldi R16,<L26
	ldi R17,>L26