智能车制作

标题: 小车转向不稳定，请教问题说在 [打印本页]

作者: chenxudong16 时间: 2011-4-28 09:02
标题: 小车转向不稳定，请教问题说在
现在小车转向不稳定，有时候会反打方向

现在将源代码公布，请高人指点

***************************************************************************************
***************************************************************************************
*Title:    The circles of the light  on and off
*MCU: M68HC908SR12CB                                                    头信息
*Author: ly
*Date: 2010.7.10
***************************************************************************************
***************************************************************************************
*
*
***************************************************************************************
***************************************************************************************
* System Register Pre-Defined                                     程序定义和硬件接口
***************************************************************************************
***************************************************************************************
*
********************************************************
* The Program Discription                   程序描述
********************************************************

*
********************************************************
* Hardware Connect                         硬件连接
********************************************************
*
*
*

***************************************************************************************
***************************************************************************************
* System Register Pre-Defined                                           寄存器定义
***************************************************************************************
***************************************************************************************
PTA EQU $0000    ; Port A Data register
PTB EQU $0001    ; Port B data register
PTC EQU $0002    ; Port C data register
PTD EQU $0003    ; Port D data register
DDRA EQU $0004    ; Port A data direction register
DDRB EQU $0005    ; Port B data direction register
DDRC EQU $0006    ; Port C data direction register
DDRD EQU $0007    ; Port D data direction register

LEDA EQU $000C    ; Port-A LED Control Register
LEDC EQU $000D    ; Port-C LED Control Register

AMCR EQU $000E    ; Analog Module Control Register
AMGCR  EQU $000F    ; Analog Module Gain Control Register
AMSCR  EQU $0010    ; Analog Module Status & Control Register

SCC1 EQU $0013    ; SCI Control Register 1
SCC2 EQU $0014    ; SCI Control Register 2
SCC3 EQU $0015    ; SCI Control Register 3
SCS1 EQU $0016    ; SCI Status Register 1
SCS2 EQU $0017    ; SCI Status Register 2
SCDR EQU $0018    ; SCI Data Register
SCBR EQU $0019    ; SCI Baud Rate Register

KBSCR  EQU $001A    ; Keyboard Status & Control register
KBIER  EQU $001B    ; Keyboard interrupt enable register

IRQ1    EQU $001E ; IRQ1 Status & Control Register
IRQ2    EQU $001C ; IRQ2 Status & Control Register

CONFIG1  EQU $001F ; Configuration Register 1
CONFIG2  EQU $001D ; Configuration Register 2

T1SC    EQU $0020 ; Timer 1 Status & Control Register
T1CNTH EQU $0021 ; Timer 1 Counter Register High
T1CNTL EQU $0022 ; Timer 1 Counter Register Low
T1MODH EQU $0023 ; Timer 1 Counter Modulo Register High
T1MODL EQU $0024 ; Timer 1 Counter Modulo Register Low
T1SC0 EQU $0025 ; Timer 1 Channel 0 Status & Control Register
T1CH0H EQU $0026 ; Timer 1 Channel 0 Register High
T1CH0L EQU $0027 ; Timer 1 Channel 0 Register Low
T1SC1 EQU $0028 ; Timer 1 Channel 1 Status & Control Register
T1CH1H EQU $0029 ; Timer 1 Channel 1 Register High
T1CH1L EQU $002A ; Timer 1 Channel 1 Register Low
T2SC    EQU $002B ; Timer 2 Status & Control Register
T2CNTH EQU $002C ; Timer 2 Counter Register High
T2CNTL EQU $002D ; Timer 2 Counter Register Low
T2MODH EQU $002E ; Timer 2 Counter Modulo Register High
T2MODL EQU $002F ; Timer 2 Counter Modulo Register Low
T2SC0 EQU $0030 ; Timer 2 Channel 0 Status & Control Register
T2CH0H EQU $0031 ; Timer 2 Channel 0 Register High
T2CH0L EQU $0032 ; Timer 2 Channel 0 Register Low
T2SC1 EQU $0033 ; Timer 2 Channel 1 Status & Control Register
T2CH1H EQU $0034 ; Timer 2 Channel 1 Register High
T2CH1L EQU $0035 ; Timer 2 Channel 1 Register Low

PCTL    EQU $0036 ; PLL Control Register
PBWC    EQU $0037 ; PLL Bandwidth Control Register
PMSH    EQU $0038 ; PLL Multiplier Select Register High
PMSL    EQU $0039 ; PLL Multiplier Select Register Low
PMRS    EQU $003A ; PLL VCO Range Select
PMDS    EQU $003B ; PLL Reference Divider Select Register

TBCR    EQU $0046 ; Timebase Control Register

MMADR EQU $0048 ; MMIIC Address Register
MMCR1 EQU $0049 ; MMIIC Control Register 1
MMCR2 EQU $004A ; MMIIC Control Register 2
MMSR    EQU $004B ; MMIIC Status Register
MMDTR EQU $004C ; MMIIC Data Transmit Register
MDDRR EQU $004D ; MMIIC Data Receive Register
MMCRDR EQU $004E ; MMIIC CRC Data Register
MMFDR EQU $004F ; MMIIC Frequency Divider Register

PWMCR EQU $0051 ; PWM Control Register
PWMCCR EQU $0052 ; PWM Clock Control Register
PWMDR0 EQU $0053 ; PWM Data Register 0
PWMDR1 EQU $0054 ; PWM Data Register 1
PWMDR2 EQU $0055 ; PWM Data Register 2
PWMPCR EQU $0056 ; PWM Phase Control Register

ADSCR EQU $0057 ; ADC Status & Control Register
ADICLK EQU $0058 ; ADC Clock Control Register
ADRH0 EQU $0059 ; ADC Data Register High 0
ADRL0 EQU $005A ; ADC Data Register Low 0
ADRL1 EQU $005B ; ADC Data Register Low 1
ADRL2 EQU $005C ; ADC Data Register Low 2
ADRL3 EQU $005D ; ADC Data Register Low 3
ADASCR EQU $005E ; ADC Auto-scan Control Register

SBSR    EQU $FE00 ; SIM Break Status Register
SRSR    EQU $FE01 ; SIM Reset Status Register
SBFCR EQU $FE03 ; SIM Break Flag Control Register

INT1    EQU $FE04 ; Interrupt Status Register 1
INT2    EQU $FE05 ; Interrupt Status Register 2
INT3    EQU $FE06 ; Interrupt Status Register 3

FLCR    EQU $FE08 ; FLASH Control Register
FLBPR EQU $FE09 ; FLASH Block Protect Register

BRKH    EQU $FE0C ; Break Address Register High
BRKL    EQU $FE0D ; Break Address Register Low
BRKSCR EQU $FE0E ; Break Status & Control Register

LVISR EQU $FE0F ; Low-Voltage Inhibit Status Register

MOR    EQU $FF80 ; Mask Option Register

COPCTL EQU $FFFF ; COP Control Register

RAMstart EQU  $0060
RAMEnd EQU  $025F
FlashStart EQU  $C000
UserStart EQU  $C036
COCOBit EQU 7

*
***************************************************************************************
***************************************************************************************
* Variable Section                                                          用户变量
***************************************************************************************
***************************************************************************************
ORG RAMstart
DUOJIFX RMB 1
CDATA RMB 1
RETURN RMB 1
DATA1 RMB 1
DATA2 RMB 1
DATA3 RMB 1
DATA4 RMB 1
DATA5 RMB 1

***************************************************************************************
***************************************************************************************
*                                                             中断初始化
***************************************************************************************
***************************************************************************************

***************************************************************************************
***************************************************************************************
* System Interruput Initialize                                           中断向量表
***************************************************************************************
***************************************************************************************
ORG FlashStart
TIMBSE:    JMP  Dummy
ANALOG:    JMP  Dummy
ADCINT:    JMP  Dummy
KEYBOD:    JMP  Dummy
SCITRN:    JMP  Dummy
SCIREC:    JMP  Dummy
SCIERR:    JMP  Dummy
MMIIC:    JMP  Dummy
TM2INT:    JMP  Dummy
TM2CH1:    JMP  Dummy
TM2CH0:    JMP  Dummy
TM1INT:    JMP  Dummy
TM1CH1:    JMP  Dummy
TM1CH0:    JMP  Dummy
PLLINT:    JMP  Dummy
IQ2INT:    JMP  Dummy
IQ1INT:    JMP  Dummy
SWIINT:    JMP  Dummy

***************************************************************************************
***************************************************************************************
* Initialization                                                             初始化
***************************************************************************************
***************************************************************************************
ORG UserStart
Startup:
LDHX #RAMEnd       ; initialize the stack pointer
TXS
CLI                   ; enable interrupts

********************************************************
* SCI initialization
********************************************************
MOV #%00010010,SCBR          ;波特率=9600HZ
MOV #%01000000,SCC1 ;8位模式
MOV #%00001100,SCC2
MOV #%00000000,SCC3
LDHX #$00
;初始化
MOV #$00,DUOJIFX
MOV #$00,CDATA
MOV #$00,DATA1
MOV #$00,DATA2
MOV #$00,DATA3
MOV #$00,DATA4
MOV #$00,DATA5
MOV #$00,RETURN

MOV #%01110000,ADICLK ;AD采样初始化
BCLR 7,DUOJIFX
BCLR 6,DUOJIFX

MOV #%00110011,T2SC             ;8000周期定周期  电机初始化
BCLR 5,T2SC
MOV #$00,T2MODH;                      |
MOV #$38,T2MODL;------------------------

MOV #%00010110,T2SC0
MOV #$00,T2CH0H
      BCLR 4,T2SC0
MOV #$38,T2CH0L
      BSET 4,T2SC0
BSET 4,DDRB
BCLR 5,DDRB

MOV #%00110110,T1SC             ;1000周期定周期
BCLR 5,T1SC
MOV #$03,T1MODH;                      |
MOV #$B6,T1MODL;------------------------

MOV #%00010110,T1SC0
MOV #$00,T1CH0H
      BCLR 4,T1SC0
MOV #$97,T1CH0L
      BSET 4,T1SC0
BSET 6,DDRA
;向右转角最大89  中间97  向左转角最大A6 T1CH0L是占空比控制寄存器
***************************************************************************************
***************************************************************************************
* mainloop(主循环)                                                          主循环
***************************************************************************************
***************************************************************************************
MAINLOOP:
JSR CHARACTERISTIC
LDA CDATA
JSR SCI_TX
CMP #0
BEQ FanH
TAX
LDA DUOJICTAB,X
STA T1CH0L
JMP MAINLOOP

FanH:
BRA MAINLOOP
***************************************************************************************
**********特征值处理子程序*************************************************************
CHARACTERISTIC:
JSR DATENERFASSUNG
JSR BIJIAO
LDA RETURN
JSR SCI_TX
CMP #3
BEQ CASE1
CMP #6
BEQ CASE2
CMP #12
BEQ CASE3
CMP #7
BEQ CASE4
CMP #14
BEQ CASE5
CMP #15
BEQ CASE6
JMP CHARACTERISTIC
CASE1:
LDA DATA1
STA CDATA
JMP NEXT_C

CASE2:
LDA DATA2
STA CDATA
JMP NEXT_C

CASE3:
LDA DATA3
STA CDATA
JMP NEXT_C
CASE4:
LDA DATA1
STA CDATA
JMP NEXT_C

CASE5:
LDA DATA2
STA CDATA
JMP NEXT_C

CASE6:
LDA DATA1
STA CDATA
JMP NEXT_C
NEXT_C:
RTS
***************************************************************************************
***********场数据采集子程序************************************************************
DATENERFASSUNG:
JSR ADCOM_1
LDA DUOJIFX
STA DATA1

JSR ADCOM_1
LDA DUOJIFX
STA DATA2

JSR ADCOM_1
LDA DUOJIFX ;说明：分别采集5场数据
STA DATA3

JSR ADCOM_1
LDA DUOJIFX
STA DATA4

JSR ADCOM_1
LDA DUOJIFX
STA DATA5

RTS

***************************************************************************************
******比较子程序***********************************************************************
******出口：RETURN*************************************************

BIJIAO:
JSR BIJIAO_1
JSR BIJIAO_2
JSR BIJIAO_3
JSR BIJIAO_4
RTS

BIJIAO_1:
LDA DATA1
;JSR SCI_TX
CMP DATA2
BEQ B_SET_1
JMP B_CLR_1
B_SET_1:
BSET 0,RETURN
RTS
B_CLR_1:
BCLR 0,RETURN ;说明：分别比较1和2 2和3 3和4 4和5的值,判断是否相等，相等则置对应位为1
RTS

BIJIAO_2:
LDA DATA2
CMP DATA3
BEQ B_SET_2
JMP B_CLR_2
B_SET_2:
BSET 1,RETURN
RTS
B_CLR_2:
BCLR 1,RETURN
RTS

BIJIAO_3:
LDA DATA3
CMP DATA4
BEQ B_SET_3
JMP B_CLR_3
B_SET_3:
BSET 2,RETURN
RTS
B_CLR_3:
BCLR 2,RETURN
RTS

BIJIAO_4:
LDA DATA4
CMP DATA5
BEQ B_SET_4
JMP B_CLR_4
B_SET_4:
BSET 3,RETURN
RTS
B_CLR_4:
BCLR 3,RETURN
RTS

***************************************************************************************
*********************AD数据采集子程序**************************************************
***********入口 ADCOM_1  出口 send DUOJIFX是数据寄存器*******************************

ADCOM_1:
LDA #%0000110 ;采集1号管的数据，并判断
JSR admid
CMP #$A0
BLS SET_1
JMP CLR_1
ADCOM_2:
LDA #%0000111 ;采集2号管的数据，并判断
JSR admid
CMP #$C0
BLS SET_2
JMP CLR_2
ADCOM_3:
LDA #%0000010 ;采集3号管的数据，并判断
JSR admid
CMP #$C0
BLS SET_3
JMP CLR_3
ADCOM_4:
LDA #%0000011 ;采集4号管的数据，并判断
JSR admid
CMP #$C0
BLS SET_4
JMP CLR_4
ADCOM_5:
LDA #%0000101 ;采集5号管的数据，并判断
JSR admid
CMP #$C0
BLS SET_5
JMP CLR_5
ADCOM_6:
LDA #%0000100 ;采集6号管的数据，并判断
JSR admid
CMP #$C0
BLS SET_6
JMP CLR_6
SEND:
RTS
***************************************************************************************
SET_1:
BSET 5,DUOJIFX ;1号管判断程序
JMP ADCOM_2
CLR_1:
BCLR 5,DUOJIFX
JMP ADCOM_2
****************************************************************************************
SET_2:
BSET 4,DUOJIFX ;2号管判断程序
JMP ADCOM_3
CLR_2:
BCLR 4,DUOJIFX
JMP ADCOM_3
****************************************************************************************
SET_3:
BSET 3,DUOJIFX ;3号管判断程序
JMP ADCOM_4
CLR_3:
BCLR 3,DUOJIFX
JMP ADCOM_4
****************************************************************************************
SET_4:
BSET 2,DUOJIFX ;号管判断程序
JMP ADCOM_5
CLR_4:
BCLR 2,DUOJIFX
JMP ADCOM_5
****************************************************************************************
SET_5:
BSET 1,DUOJIFX ;5号管判断程序
JMP ADCOM_6
CLR_5:
BCLR 1,DUOJIFX
JMP ADCOM_6
****************************************************************************************
SET_6:
BSET 0,DUOJIFX ;6号管判断程序
JMP SEND
CLR_6:
BCLR 0,DUOJIFX

JMP SEND
****************************************************************************************
advalue:
   AIS    #-1
   AND    #%00011111
   STA    1,SP             ;暂存通道号
;选取通道
   LDA    ADSCR
   AND    #%11100000
   ORA    1,SP
   STA    ADSCR
   BRCLR COCOBit,ADSCR,* ;取A/D转换结果
   LDA    ADRL0
   AIS    #1
RTS
*admid:1路A/D转换子程序(中值滤波)----------------------*
*功能描述:对某通道进行三次采样,中值滤波结果放入A    *
*入口:A=通道号                                  *
*出口:A=该通道中值滤波后的A/D转换结果             *
*堆栈深度:2+5=7                                     *
*-----------------------------------------------------*/
admid:
   PSHX                      ;保护X
   AIS    #-4             ;在堆栈区开辟临时变量
   STA    4,SP             ;暂存通道号
;取三次A/D转换结果
   LDA    4,SP
   JSR    advalue
   STA    1,SP
   LDA    4,SP
   JSR    advalue
   STA    2,SP
   LDA    4,SP
   JSR    advalue
   STA    3,SP
;从三次A/D转换结果中取中值
   ;取出堆栈前2个数比较,小的放在(SP+1),大的放在(SP+2)
   LDA    1,SP
   CMP    2,SP
   BLS    admid1       ;若(SP+1)<=(SP+2),转admid1
   TAX                   ;否则(SP+1)与(SP+2)互换
   LDA    2,SP
   STA    1,SP
   STX    2,SP
admid1:
;取出(SP+3)中的数,与(SP+2)比较
   LDA    3,SP
   CMP    2,SP
   BHS    admid2       ;若(SP+3)>(SP+2),转admid2
   CMP    1,SP       ;否则与(SP+1)比较
   BHI    admid3
   LDA    1,SP
   BRA    admid_Exit
admid2
   LDA    2,SP
   BRA    admid_Exit
admid3
   LDA    3,SP
admid_Exit
   AIS    #4          ;释放临时变量
   PULX                   ;恢复X
RTS
********************************************************
*adave:1路A/D转换子程序(均值滤波)----------------------*
*功能描述:求某通道X次均值滤波结果,并放入A中          *
*入口:A=通道号 X=平均次数(1--255)                *
*出口:A=该通道中值滤波后的A/D转换结果             *
*堆栈深度:2+5=7                                     *
*-----------------------------------------------------*/
adave:
   AIS    #-4          ;在堆栈区开辟临时变量
   STX    4,SP       ;暂存平均次数
   STA    3,SP       ;暂存通道号
   LDA    #0          ;初始化X次和值
   STA    2,SP
   STA    1,SP
;求X次的和
adave_1:
   LDA    3,SP       ;取通道号
   JSR    admid
   ADD    1,SP       ;相加
   STA    1,SP
   CLRA
   ADC    2,SP
   STA    2,SP
   DBNZX adave_1
;求平均值
   LDA    2,SP       ;高位->H
   PSHA
   PULH
   LDA    1,SP       ;低位->A
   LDX    4,SP       ;除数->X
   DIV                   ;(H:A)/X->A 忽略余数
   AIS    #4          ;释放临时变量
   RTS

******************************************************************************
DUOJICTAB:
FCB $97
FCB $A6 ;00000001 1
FCB $A0 ;00000010 2
FCB $A2 ;00000011 3
FCB $99 ;00000100 4
FCB $97 ;空位预留 5
FCB $9C ;00000110 6
FCB $97 ;空位预留 7
FCB $95 ;00001000 8
FCB $97 ;空位预留 9
FCB $97 ;空位预留 10
FCB $97 ;空位预留 11
FCB $97 ;00001100 12 中间
FCB $97 ;空位预留 13
FCB $97 ;空位预留 14
FCB $97 ;空位预留 15
FCB $90 ;00010000 16
FCB $97 ;空位预留 17
FCB $97 ;空位预留 18
FCB $97 ;空位预留 19
FCB $97 ;空位预留 20
FCB $97 ;空位预留 21
FCB $97 ;空位预留 22
FCB $97 ;空位预留 23
FCB $94 ;00011000 24
FCB $97 ;空位预留 25
FCB $97 ;空位预留 26
FCB $97 ;空位预留 27
FCB $97 ;空位预留 28
FCB $97 ;空位预留 29
FCB $8A ;空位预留 30
FCB $8A ;空位预留 31
FCB $8B ;00100000 32
FCB $89 ;空位预留 33
FCB $97 ;空位预留 34
FCB $97 ;空位预留 35
FCB $97 ;空位预留 36
FCB $97 ;空位预留 37
FCB $97 ;空位预留 38
FCB $97 ;空位预留 39
FCB $97 ;空位预留 40
FCB $97 ;空位预留 41
FCB $97 ;空位预留 42
FCB $97 ;空位预留 43
FCB $97 ;空位预留 44
FCB $97 ;空位预留 45
FCB $97 ;空位预留 46
FCB $97 ;空位预留 47
FCB $8B ;00110000 48
FCB $97 ;00110001 49
FCB $97 ;00110010 50
FCB $97 ;00110011 51
FCB $97 ;00110100 52
FCB $97 ;00110101 53
FCB $97 ;00110110 54
FCB $97 ;00110111 55
FCB $97 ;00111000 56
FCB $97 ;00111001 57
FCB $97 ;00111010 58
FCB $97 ;00111011 59
FCB $97 ;00111100 60
FCB $97 ;00111101 61
FCB $97 ;00111110 62
FCB $97 ;00111111 63
FCB $97 ;00111111 63
FCB $97 ;00111111 63
FCB $97 ;00111111 63
FCB $97 ;00111111 63
********************************************************
BIJIAOTAB:

********************************************************
*                   延时子程序                      *
********************************************************
*------------------------------------------------------*
*文件说明:包含2个延时子程序，一个ms级延时,一个us级延时 *
*------------------------------------------------------*
*Delay_ms:毫秒级延时子程序-----------------------------*
*功  能:毫秒级延时子程序, 延时T=500*HX个时钟周期    *
*入  口:HX=T/500    ;(HX的取值范围#1--#65535)       *
*调用例

DHX #1000 ;延时T=500*1000=50000个时钟周期  *
*    JSR Delay_ms                                  *
*备  注:在总线频率为f=7.3728MHz时,每周期T=0.1356us    *
*    最大延时:65535*500*0.1356us=4.44s             *
*------------------------------------------------------*
Delay_ms:
   LDA    #96       ;2个时钟周期
   BSR    Delay_us    ;490个时钟周期
   AIX    #-1       ;2个时钟周期
   CPHX    #0          ;3个时钟周期
   BNE    Delay_ms    ;3个时钟周期
   RTS                   ;4个时钟周期(忽略)
*Delay_us:微秒级延时子程序-----------------------------*
*功  能：微秒级延时子程序,延时T=(10+5*A)个时钟周期    *
*入  口：寄存器A=(T-10)/5 (A的取值范围#1--#255)    *
*备  注:由总线频率计算每个周期时间.                   *
*    设总线频率为f=7.3728MHz,                      *
*    每个周期为T0=1/f≈0.1356us                   *
*    若用BSR调用：T=10+5*A的来源                   *
*                   LDA(2),BSR(4),循环体(5*A),RTS(4)  *
*------------------------------------------------------*
Delay_us:
   NOP                ;1个时钟周期
   NOP                ;1个时钟周期
   DBNZA  Delay_us    ;3个时钟周期
RTS                   ;4个时钟周期

********************************************************
* SCI发送一个字节数据
********************************************************
SCI_TX:
BRCLR 7,SCS1,*
STA SCDR
RTS
********************************************************
* SCI接收一个字节数据
********************************************************
SCI_RX:
BRCLR 5,SCS1,*
LDA SCDR
RTS
********************************************************
* Empty Interruput
********************************************************
Dummy:
NOP
RTI

作者: qwertyuio 时间: 2011-4-28 17:24
这是传说中的汇编吗？

作者: chenxudong16 时间: 2011-4-28 22:00
回复 2# qwertyuio

是汇编，应为汇编比C运行起来要快一些，所以用汇编

作者: 赵古今 时间: 2011-5-2 13:23
太牛了

作者: xiao19890501 时间: 2011-5-15 23:07
靠终于看见用汇编的了鸡冻呀。

作者: LJG 时间: 2012-3-18 23:16

作者: zc090115 时间: 2012-3-19 18:22
用得着吗，这都什么年代了，现在的编译软件已经做得很好了，在执行效率上，汇编并不能占到多少便宜，不够，用汇编写，还是值得鼓励哈，勇气可嘉

作者: 宁静致远 时间: 2012-3-21 00:14
真是大神啊

作者: qiyixi 时间: 2012-3-22 16:04
强大的汇编啊~~~

作者: 断翅at雄鹰 时间: 2012-9-25 20:06

欢迎光临智能车制作 (http://111.231.132.190/)