;TEST-under test or not important *NOTE that the variable can only be used in different PROGRAM LOOPS *CHECK NOTES *- check all testing procedures to be remove *///////////////////////NOTE possible error cause///////////////////////////// * ERROR POSSIBLE SOFTWARE REASON *slave cannot recieve data - didn't insert the JSR CONTROLLEVEL at RUNLEVEL *use of CPX - things can't work *///////////////////////////////////////////////////////////////////////////// *NOTE that there is a change in physical & logical ports of MASTER MCU (PORTB & PORTC IS SWAP) ********* why need to put BSR SETDELAY again when one is at the ACC_CHANGE PORTA EQU $00 ; PA7 CLK1 (R) PB7 status port PORTB EQU $02 ; 6 CHOP1(R) 6 " PORTC EQU $01 ; 5 DIR(R) 5 " PORTD EQU $03 ; 4 QUICK STOP 4 " DDRA EQU $04 ; 3 TRANSMIT ERROR(TEST) 3 " DDRB EQU $06 ; 2 2 " DDRC EQU $05 ; 1 1 " TSR EQU $13 ; 0 0 " TCR EQU $12 ; PC7 Read LEVEL PD7 !without_reverse brake or with reverse brake ICAPHI EQU $14 ; 6 " 6 XXX (Cannot be use!) ICAPLO EQU $15 ; 5 " 5 !SERIAL COM 's status or SPEED ALTIMERHI EQU $1A ; 4 " 4 speed !MAX or 9600 ALTIMERLO EQU $1B ; 3 " 3 !without_reverse or reverse(when turning) OCMPHI EQU $16 ; 2 " 2 start to !STOP or MAX OCMPLO EQU $17 ; 1 " 1 XXX (For serial com) ; 0 " 0 XXX (For serial com) BAUDRATE EQU $0D ; RECIEVED DATA FORMAT (test: code to be check again) SCCR1 EQU $0E ; 7 !forward,backward mode SCCR2 EQU $0F ; 6 !left,right (for forward only) !maxspeed,backward(for backward mode only) SCSR EQU $10 ; 5 !left,right (for backward only) SCDAT EQU $11 ; 5-0 range (if not in used) ******************************************************************* * VARIABLES * ******************************************************************* *CONSTANT TERMS..........NOTES........................ *Variable LEVEL SPEED CYCLE must be done together *For CYCLE please X 2 yourself (-_-_-_-_- one cycle = two delay period = CYCLE) *At final speed or final frequency cycle=$00 --max delay before change of chopping * cycle=$01 --min delay before change of chopping *beware of slave's memory ; ORG $0020 ;Only allows 48 bytes ;max only $3E [6 bits' levels - 1(which is use for on spot turning code)] ;LEVEL FCB $3E ;no. of acc-1 start at 200 Hz ORG $200 *set 9 50Hz-2000Hz * FDB $7D,$7F,$80,$82,$84,$85 * FDB $87,$89,$8B,$8D,$8F,$91,$93, CYCLEDELAY FDB $95,$98,$9A FDB $9C,$9F,$A1,$A4,$A7,$A9,$AC,$AF,$B3,$B6 FDB $B9,$BD,$C0,$C4,$C8,$CC,$D0,$D5,$D9,$DE FDB $E3,$E9,$EE,$F4,$FA,$100,$107,$10E,$116,$11E FDB $126,$12F,$139,$142,$14D,$159,$165,$172,$181,$190 FDB $1A1,$1B3,$1C7,$1DC,$1F4,$20E,$22C,$24C,$271,$29B FDB $2CA,$301,$341,$38D,$3E8,$457,$4E2,$683,$9C4,$1388 FCB $FF * FCB $22,$22,$22,$20,$20,$20 ;ACC=76($4C) future but max acc is only 63 or 2 power of 6 * FCB $20,$1E,$1E,$1E,$1E,$1E,$1C,$1C,$1C,$1C * FCB $1C,$1A,$1A,$1A,$1A,$1A,$18,$18,$18,$18 *CYCLE FCB $16,$16,$16,$16,$16,$14,$14,$14,$14,$14 * FCB $12,$12,$12,$12,$12,$C,$C,$C,$A,$A * FCB $A,$A,$A,$8,$8,$8,$8,$8,$8,$8 * FCB $6,$6,$6,$6,$6,$6,$6,$4,$4,$4 * FCB $4,$4,$4,$4,$2,$2,$2,$4,$2,$2 ;ACC=50($32) current * FCB $44,$44,$42,$42,$40,$40 * FCB $3E,$3E,$3E,$3C,$3C,$3A,$3A, CYCLE FCB $3A,$38,$38 FCB $36,$36,$34,$34,$34,$32,$32,$30,$30,$2E FCB $2E,$2E,$2C,$2C,$2A,$2A,$28,$28,$28,$26 FCB $26,$24,$24,$22,$22,$18,$16,$16,$16,$14 FCB $14,$14,$14,$12,$12,$10,$10,$10,$E,$E FCB $E,$E,$C,$C,$C,$A,$A,$A,$A,$8 FCB $8,$8,$6,$6,$6,$6,$4,$8,$6,$1 FCB $FF * FCB $58,$58,$56,$56,$54,$54 * FCB $52,$52,$50,$4E,$4E,$4C,$4C, CYCLEPOOR FCB $4A,$4A,$48 FCB $48,$46,$44,$44,$42,$42,$40,$40,$3E,$3E FCB $3C,$3A,$3A,$38,$38,$36,$36,$34,$34,$32 FCB $30,$30,$2E,$2E,$2C,$1E,$1E,$1C,$1C,$1C FCB $1A,$1A,$18,$16,$16,$16,$14,$14,$14,$12 FCB $12,$12,$10,$10,$E,$E,$E,$C,$C,$C FCB $A,$A,$8,$8,$8,$6,$6,$A,$8,$4 FCB $FF ORG $0050 LEVEL RMB $1 *BIT1-LEFT BIT0-RIGHT DIRCHSTATUS RMB $1 ;to indicate change of direction SLOWER? RMB $1 ;want to slow down motor? left or right(for curve) REVERSEBRAKE RMB $1 ;to indicate reverse braking action WHICHPROFILE? RMB $1 ;to select optimise or delicated profile DIFF_RATE RMB $1 ;level rate difference by command(for curve) STOPMOTORLEVEL RMB $1 ;level which motor can stop($FF for no stopping) STOPMOTORNOW RMB $1 ;command to stop motor immediately DIRSTATUS RMB $1 ;to store new/current motor direction status *DIRSTATUS (msb)variable2(lsb)variable1 ;LMOTORSTEP RMB $1 ;number of step left motor stepped ;RMOTORSTEP RMB $1 ;number of step right motor stepped *Serial Communication COMMAND RMB $5 ;to store command from main MCU SCI_INT RMB $1 ;to tell if interrupt is activate & store no. of command SCI_FLOW RMB $1 ;to control the flow for SORTCOMMAND PRE_SDATA RMB $1 ;to save up previous data send for checking ;PRE_DATA must not be all 1's or $FF see initialise *PORTS NAME MODEPORT EQU PORTD RLEVEL1 EQU PORTC ;rlevel1 fcb $ff STATUSPORT EQU PORTB ;statusport rmb $1 MOTORPORT EQU PORTA MAXSPEED RMB $1 ;maximum speed allowed MAXLEVEL1 RMB $1 ;control maximum speed MAXLEVEL2 RMB $1 ;control maximum speed LEVEL2 RMB $1 ;for displaying current level CYCLE1 RMB $1 ; *temperory storage TEMPA RMB $1 ;protect ACC TEMPX RMB $1 ;protect X TEMP1 RMB $1 ;cannot be use for holding value for long TEMP2 RMB $1 ;cannot be use for holding value for long ******************************************************************* * INITIALIZATION * ******************************************************************* ORG $0400 INITIAL CLI ;got interrupt **communication ; LDA #%00110001 ; BRSET 4,MODEPORT,CHBAUDRATE ; LDA #%00000000 ; CHBAUDRATE ; STA BAUDRATE ; LDA #%00000000 ; STA SCCR1 ; LDA #%00101100 ;can be improve to make MCU sleep STA SCCR2 ; **timer ; LDA #%01000000 ;enable timer interrupt STA TCR ; ;disable timer when motor stops ;enable timer if not motor stop at WAITRUNDELAY ;set OCIE (output compare interrupt enable) ;note if OCIE interrupt set the TSR will not work **ports ; LDA #%11111111 ; STA DDRA ;OUT ;control output STA DDRB ;OUT ;status output ; LDA #%00000000 ; STA DDRC ;in ;slave's level reading ;MOTORPORT below ; STA STATUSPORT ; **variable ; ;MAXLEVEL1, MAXLEVEL2 below ;LEVEL2 intial at STACC ;CYCLE1 ;DIRSTATUS below STA DIRCHSTATUS ;clearing memories STA SLOWER? ; STA REVERSEBRAKE ; STA DIFF_RATE ; ;STOPMOTORLEVEL below ;STOPMOTORNOW below ; STA LMOTORSTEP ; ; STA RMOTORSTEP ; ; STA WHICHPROFILE? ; ; STA COMMAND ; STA SCI_INT ; ;PRE_DATA below ; STA TEMPA ; STA TEMPX ; STA TEMP1 ; STA TEMP2 ; ; LDA LEVEL ;stop brclr 2,MODEPORT,STARTATMIN ;to select to stop or travel to maxspeed LDA #!0 ;maxlevel STARTATMIN ; STA STOPMOTORNOW ; STA MAXLEVEL1 ; STA MAXLEVEL2 ; ; LDA $FF ; STA PRE_SDATA ;data sending code must not contain $FF ; LDA #!0 ; STA MAXSPEED ;allow maximum speed ; LDA #%10100000 ;starting with forward status STA DIRSTATUS ; ; LDA #%00100000 ; STA MOTORPORT ;initialise MOTORPORT direction ; lda #!15 ; sta stopmotorlevel ; ; JSR PROFILE_UPLOADING JSR STACC ; JMP WAITRUNDELAY ; ****************************************************************************** PROFILE_UPLOADING CHECKCODE1 LDA RLEVEL1 ;check slave status CMP #%10101010 ;is slave ready code1 BNE CHECKCODE1 ;if slave MCU not ready CLRX ;use for indexing DATA_UPLOAD_NEXT DATA_UPLOAD LDA CYCLE,X ;data to be upload BRCLR 7,SCSR,DATA_UPLOAD STA SCDAT ;send data INCX ;point to next value to be send CMP #$FF ;check for end of block BNE DATA_UPLOAD_NEXT;load next data CHECKCODE2 LDA RLEVEL1 ;check slave status CMP #%01010101 ;is slave ready code2 BNE CHECKCODE2 ;if slave MCU not ready DECX DECX STX LEVEL ;record LEVEL INCX INCX CLRX ;use for indexing DATA_2UPLOAD_NEXT DATA_2UPLOAD LDA CYCLEPOOR,X ;data to be upload BRCLR 7,SCSR,DATA_2UPLOAD STA SCDAT ;send data INCX ;point to next value to be send CMP #$FF ;check for end of block BNE DATA_2UPLOAD_NEXT;load next data TRANSMIT_ERROR_CHECK JSR DELAY ;slave reaction is slow;TEST LDA LEVEL CMP RLEVEL1 ;check if slave received the data by BEQ TRANSMIT_FINISH JMP ERROR ;comparing no. of LEVEL receive TRANSMIT_FINISH RTS ****************************************************************************** ********************************************************************** ********************LEVEL CONTROL************************************* ;NOTE: carefully use variables used in subrountine *input variables: ;STOPMOTORNOW,SLOWER?,RLEVEL1,LEVEL2,MAXSPEED,DIFF_RATE,LEVEL ;Below Variable updated for every LEVEL *output variables: ;MAXLEVEL1,MAXLEVEL2,DIRSTATUS *control variables: ;reversebrake,TEMP2 *functions use ;UPDATEDIRECTION(TEMP1,MOTORPORT,DIRSTATUS,DIRCHSTATUS) ;note that temp1 is use in rountine ;SENDDATA(DIRSTATUS,WHICHPROFILE?,MAXLEVEL1,PRE_SDATA) ********************************************************************** CONTROLLEVEL ;on spot turn is control by DIRCHSTATUS LDA DIRSTATUS ;make note of original motor direction because there STA TEMP2 ;will be change of DIRSTATUS for control purposes ;note the use of TEMP2 instead of TEMP1(TEMP1 use by UPDATEDIRECTION) LDA STOPMOTORNOW ;stop come first BEQ NOTEREDUCECODE ; JMP STOPPINGMOTOR ; NOTEREDUCECODE ; reverse_update ; LDA RLEVEL1 ;REVERSEBRAKE is use for STOPPINGMOTOR ADD LEVEL2 ;if not REVERSEBRAKE is to be updated COMA ;convert max speed (00000000) to a large value LSRA ; LSRA ;take an approximate reading from LSRA ;motor's speed & calculate & update a LSRA ; STA reversebrake ;suitable reversebrake value ; LDA SLOWER? ; BNE SLOWERMOTOR ; SAME_SPEED ; LDA RLEVEL1 ; ADD LEVEL2 ; ASRA ;take the average speed CMP RLEVEL1 ;compare if the two level are the same BEQ STORE_MAX_SPEED ;use average speed if level not the same CMP LEVEL2 ;estimate if two speed are near BNE STORE1_SAME_SPEED ; STORE_MAX_SPEED ; LDA MAXSPEED ;if the same, allow maximum speed STORE1_SAME_SPEED ; JMP STORE_SAME_SPEED ; *-----------------------------------; SLOWERMOTOR ; LDA SLOWER? ; LSLA ; BEQ SLOW_RIGHT ; SLOW_LEFT ; LDA LEVEL2 ;with reference to the right motor speed ADD DIFF_RATE ; STA MAXLEVEL1 ;store calculated left's speed CMP LEVEL ;NOTE THAT SUB IS NOT USE HERE BECAUSE ;check if calculation is greater than minimum speed ;SUB IS NOT THE SAME AS CMP (bugS) BNE SLOW_ONLYLEFT ;if MAXLEVEL2 is same as stopcode use one DEC MAXLEVEL1 ;level away to avoid that region BRA SLOW_ONLY_LEFT ;NOTE that if master hang at motor hang MASTER is hanged SLOW_ONLYLEFT ; BLO SLOW_ONLY_LEFT ;if value is greater direction is to be change LDA LEVEL ;formula MAXLEVEL1=LEVEL-(LEVEL2+DIFF_RATE-LEVEL) LSLA ; =2*LEVEL-LEVEL2-DIFF_RATE SUB MAXLEVEL1 ;NOTE that MAXLEVEL1 is now LEVEL2+DIFF_RATE STA MAXLEVEL1 ;store new calculated value because speed is LDA DIRSTATUS ;slow till direction is to be change EOR #%10000000 ;toggle the slow motor to the other direction ; brset 3,MODEPORT,SLOWONLY_LEFT ;to select with or without reserse when turning lda level ;use slowest speed deca ;but remember to avoid stop code region sta maxlevel1 ;pass it to the control variable SLOW_ONLY_LEFT ;set direction ready to forward LDA TEMP2 ;restore original direction if rate is not out of range SLOWONLY_LEFT ;note that TEMP2 contains original direction STA DIRSTATUS ;UPDATEDIRECTION is below LDA MAXSPEED ;load the maximum speed allowed STA MAXLEVEL2 ;allow max speed for the right motor BRA ENDCONTROLLEVEL;check ; SLOW_RIGHT ; LDA RLEVEL1 ;with reference to the left motor speed ADD DIFF_RATE ; STA MAXLEVEL2 ;store calculated right's speed CMP LEVEL ;NOTE THAT SUB IS NOT USE HERE BECAUSE ;check if calculation is greater than minimum speed ;SUB IS NOT THE SAME AS CMP (bugS) BNE SLOW_ONLYRIGH ;if MAXLEVEL2 is same as stopcode use one DEC MAXLEVEL2 ;level away to avoid that region BRA SLOW_ONLY_RIGH ;NOTE that if master hang at motor hang MASTER is hanged SLOW_ONLYRIGH ; BLO SLOW_ONLY_RIGH ;if value is greater direction is to be change LDA LEVEL ;formula MAXLEVEL2=LEVEL-(RLEVEL1+DIFF_RATE-LEVEL) LSLA ; =2*LEVEL-RLEVEL1-DIFF_RATE SUB MAXLEVEL2 ;NOTE that MAXLEVEL2 is now RLEVEL1+DIFF_RATE STA MAXLEVEL2 ;store new calculated value because speed is LDA DIRSTATUS ;slow till direction is to be change EOR #%00100000 ;toggle the slow motor to the other direction ; brset 3,MODEPORT,SLOWONLY_RIGH ;to select with or without reserse when turning lda level ;use slowest speed deca ;but remember to avoid stop code region sta maxlevel2 ;pass it to the control variable SLOW_ONLY_RIGH ;set direction ready to forward LDA TEMP2 ;restore original direction if rate is not out of range SLOWONLY_RIGH ;note that TEMP2 contains original direction STA DIRSTATUS ;UPDATEDIRECTION is below LDA MAXSPEED ;load the maximum speed allowed STA MAXLEVEL1 ;allow max speed for the left motor BRA ENDCONTROLLEVEL;check *-----------------------------------; STOPPINGMOTOR ; brset 7,MODEPORT,STOPMOTORREVERSE LDA LEVEL ;load stop code DECA ;to avoid stopcode but need speed close to stop CMP RLEVEL1 ; BNE STORE_SAME_SPEED ;can only stop when CMP LEVEL2 ; both motor's speed BNE STORE_SAME_SPEED ;is close to stop BRA STOPMOTORCODE ; STOPMOTORREVERSE ; dec reversebrake ; bmi STOPMOTORCODE ;stop if duration of reverse is up lda dirstatus ;continue to run motor if reversebrake eor #%10100000 ;do a reverse direction run sta dirstatus ;UPDATEDIRECTION is below lda maxspeed ;let it run to maximum speed bra STORE_SAME_SPEED ;is a positive STOPMOTORCODE ; LDA LEVEL ;set stopping code STORE_SAME_SPEED ;meant for motor having the same speed STA MAXLEVEL1 ;update calculated maxlevel STA MAXLEVEL2 ; ; ENDCONTROLLEVEL ; ; JSR UPDATEDIRECTION ; ; LDX #!1 ; CHECKMAX LDA MAXSPEED ;check CMP MAXLEVEL1,X ;prevent value over BLS CHECKMIN ;must be between MAXSPEED