* Stephen Tobias * Main Version 8 * Walking Test Program3 * Walking Refinement * Turning Sub Finish * Obsticle Avoidance * Timer Servo Controler Enabled * Switches Tasks * Samples Sensors * Incriments RTI Counter * (Defer Sub also) * (Sensor Sample Sub) * And Writes to LED * Memory Map * $0000 - $03FF Registers * $0800 - $0BFF 1 KByte RAM * $0D00 - $0FFF EEPROM * $0xxx Maped I/O * $0000 - $FFFF External RAM * Memory Usage * $4000 - $BFFF (32 k) Map * $0800 - $0BFF Main, Sensor, Obsticle Proc * $C000 - $FCFF Additional Program Space * $3FFF and down Stack Spaces * $2000 and up ISRs, Subs, Inits *************************************************** * EQUs *************************************************** * Top of Stack SPTop EQU $0C00 * Size of Each Processe's Stack * Max value is $FF (255) SPSize EQU $8F * Max Processes MaxProc EQU $04 * RTI Vector RTIVec EQU $FFF0 * Constants Thresh EQU $78 Period EQU $2FA3 EOS EQU $04 CR EQU $0D LF EQU $0A Char1 EQU $31 Char2 EQU $32 Char3 EQU $33 Char4 EQU $34 Char5 EQU $35 Char6 EQU $36 CharA EQU $41 CharB EQU $42 CharC EQU $43 CharD EQU $44 CharM EQU $4D BIT0 EQU %00000001 BIT1 EQU %00000010 BIT2 EQU %00000100 BIT3 EQU %00001000 BIT4 EQU %00010000 BIT5 EQU %00100000 BIT6 EQU %01000000 BIT7 EQU %10000000 INV0 EQU %11111110 INV1 EQU %11111101 INV2 EQU %11111011 INV3 EQU %11110111 INV4 EQU %11101111 INV5 EQU %11011111 INV6 EQU %10111111 INV7 EQU %01111111 * Timer Middle, Left & Right TMRMid EQU $2A0 TMRLft EQU $4A0 TMRRt EQU $0A0 * SWI Operatoins SWISNew EQU $01 SWIDefr EQU $02 SWISet EQU $03 * Registers RTICTL EQU $14 RTIFLG EQU $15 ATDCTL2 EQU $62 ATDCTL4 EQU $64 ATDCTL5 EQU $65 MODE EQU $0B MISC EQU $13 PEAR EQU $0A INITEE EQU $12 INTCR EQU $1E TIOS EQU $80 CFORC EQU $81 TCNT EQU $84 TSCR EQU $86 TCTL1 EQU $88 TCTL2 EQU $89 TCTL3 EQU $8A TCTL4 EQU $8B TMSK1 EQU $8C TMSK2 EQU $8D TFLG1 EQU $8E TFLG2 EQU $8F TC0 EQU $90 TC1 EQU $92 TC2 EQU $94 TC3 EQU $96 TC4 EQU $98 TC5 EQU $9A TC6 EQU $9C TC7 EQU $9E PWPER0 EQU $4C PWPER1 EQU $4D PWPER2 EQU $4E PWPER3 EQU $4F PWDTY0 EQU $50 PWDTY1 EQU $51 PWDTY2 EQU $52 PWDTY3 EQU $53 ATDSTAT EQU $67 ADR0H EQU $70 ADR1H EQU $72 ADR2H EQU $74 ADR3H EQU $76 ADR4H EQU $78 ADR5H EQU $7A ADR6H EQU $7C ADR7H EQU $7E * Subs, not in this .ASM OutChar EQU $FD79 OutStr EQU $FD84 InChar EQU $FD94 GetDbt EQU $FDBB ASCII2 EQU $FDAA MCOPY EQU $FE14 INITPWM EQU $FE1C *************************************************** * Main *************************************************** ORG $800 JMP Main * PsedoVectors, so I don't have to keep * changing the Vectors in the EEPROM VecRTI JMP RTIISR VecSWI JMP SWIISR VecOC1 JMP OC1ISR VecOC2 JMP OC2ISR VecOC3 JMP OC3ISR VecOC4 JMP OC4ISR VecOC5 JMP OC5ISR VecOC6 JMP OC6ISR Main LDS #SPTop * Move EEPROM LDAA #%11110001 STAA INITEE * Initalise JSR InitRTI JSR InitAD JSR InitPWM JSR InitOC * Set Servos to standing position LDD #$0330 STD Prd1 LDD #$0200 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0280 STD Prd4 LDD #$02A0 STD Prd5 LDD #$02A0 STD Prd6 LDAA #$0B STAA PWDTY0 LDAA #$08 STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Enable Interupts CLI TFR CCR,A STAA StdCCR * Start Sensor sampling Process LDX #SenProc JSR NewProc * Start Obsticle Avoidance Process LDX #ObsProc JSR NewProc * Wait a little up front Start LDD #$6000 JSR WaitTmr JSR WaitTmr JSR WaitTmr Here LDD #$3000 JSR WaitTmr LDAA ObstVar CMPA #$01 BEQ MTrnLf CMPA #$02 BEQ MTrnRt CMPA #$03 BEQ MTrnRt JSR StepLft BRA Here MTrnRt JSR TurnRgt BRA Here MTrnLf JSR TurnLft BRA Here * Read Sensors Process SenProc JSR SenSamp JSR Defer BRA SenProc * Obsticle Avoidance ObsProc CLR ObstVar LDX #AtoDRes * Left Sensor Results LDAA 2,X * Right Sensor Results LDAB 3,X * Compare A to thresh hold * Look at Left Sensor CMPA #Thresh BLT ObsSkp0 LDAA #$01 STAA ObstVar * Compare B to thresh hold * Look at Right Sensor ObsSkp0 CMPB #Thresh BLT ObsSkp1 LDAB ObstVar ADDB #$02 STAB ObstVar ObsSkp1 JSR Defer BRA ObsProc *************************************************** * RMBs & FCB/FDB *************************************************** * Obsticle Avoidance Recomendation ObstVar RMB 1 * # of times RTI has executed RTICntr RMB 2 * RTI Has been defered? * if not 0, continue * Also used to how temp data * in defer sub RTIDefr RMB 1 * Process Number ProcNum RMB 1 * Process Stacks (2 bytes ea.) ProcSP RMB MaxProc RMB MaxProc ProcNSP FDB $0C00 FDB $0BC0 FDB $0B80 FDB $0B40 * # of Proccesses running ProcAct RMB 1 * A to D Results AtoDRes RMB 8 * Digital Inputs (2 of them) DgInput RMB 2 * SWI Operation SWIOp RMB 1 * Return Locatoin from SWProc SWRtn RMB 2 * A standard CCR StdCCR RMB 1 TempSP RMB 2 Prd1 RMB 2 Prd2 RMB 2 Prd3 RMB 2 Prd4 RMB 2 Prd5 RMB 2 Prd6 RMB 2 *********************************************** ORG $2000 *********************************************** * Initalise Timer *********************************************** InitOC PSHD * Select all output compare LDAA #$FF STAA TIOS * Enable timer & fast flag clear LDAA #%10010000 STAA TSCR * Set output levels LDAA #%10101010 STAA TCTL1 LDAA #%10101010 STAA TCTL2 * Divide by 16 LDAA #%00000100 STAA TMSK2 * Enable Interupt * Only the middle 6 LDAA #%01111110 STAA TMSK1 * Set Timer Servos to middle LDD #TMRMid STD Prd1 STD Prd2 STD Prd3 STD Prd4 STD Prd5 STD Prd6 PULD RTS *************************************************** * Init the RTI & MultiTasking System *************************************************** InitRTI PSHA PSHX PSHY * Enable RTI interupt * RTI Rate 4.096 ms LDAA #%10000011 STAA RTICTL * Setup JumpVector LDX #RTIISR STX RTIVEC * Set Main as current Process (# 0) CLR ProcNum * Set to 1 Proccesses running now LDAA #$01 STAA ProcAct * Clear Process SP table LDX #ProcSP LDAA #MaxProc LDY #$0000 CLRProc STY 0,X INX INX DBNE A,CLRProc * Put a value in 0's entry * To prevent a starting proccess from * using Main's slot TSX STX ProcSP * Clear RTI counter STY RTICntr * Clear SWI Op & New CLR SWIOp PULY PULX PULA RTS *************************************************** * Init the A to D system *************************************************** InitAD PSHD PSHX LDAA #%11000000 STAA ATDCTL2 LDAA #%00000001 STAA ATDCTL4 LDAA #%01010000 STAA ATDCTL5 LDX #AtoDRes LDAA #$07 InitAD0 CLR A,X DBNE A,InitAD0 PULX PULD RTS *************************************************** * Sample A to D *************************************************** SampAD PSHD PSHX LDAA #%01010000 STAA ATDCTL5 SampAD0 BRCLR ATDSTAT,BIT7,SampAD0 SampAD1 BRCLR ATDSTAT,BIT0,SampAD1 LDX #AtoDRes LDAB ADR0H STAB 0,X LDAB ADR1H STAB 1,X LDAB ADR2H STAB 2,X LDAB ADR3H STAB 3,X LDAB ADR4H STAB 4,X LDAB ADR5H STAB 5,X LDAB ADR6H STAB 6,X LDAB ADR7H STAB 7,X PULX PULD RTS *************************************************** * Sensor Sampling Sub *************************************************** SenSamp PSHD PSHX PSHY JSR SampAD * Read Digital Inputs * I'll write that later PULY PULX PULD RTS *********************************************** * TurnLft * Turn robot to its Left *********************************************** TurnLft PSHD * Wait a littlebit LDD #$3000 JSR WaitTmr * Push and turn LDD #$02B0 STD Prd1 LDD #$0200 STD Prd2 LDD #$0200 STD Prd3 LDD #$0280 STD Prd4 LDAA #$0F STD PWDTY1 LDAA #$08 STAA PWDTY2 LDAA #$0D STAA PWDTY3 * Wait a littlebit LDD #$2000 JSR WaitTmr * New Tail position LDAA #$02 JSR MTail * Wait a littlebit LDD #$0400 JSR WaitTmr * Set Servos to standing position * LLeg up LDD #$0380 STD Prd1 LDD #$0200 STD Prd2 LDD #$0300 STD Prd3 LDD #$0280 STD Prd4 LDAA #$0B STD PWDTY1 LDAA #$08 STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Wait a littlebit LDD #$1000 JSR WaitTmr * Set Servos to standing position LDD #$0330 STD Prd1 LDD #$0200 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0280 STD Prd4 LDD #$02A0 STD Prd5 LDD #$02A0 STD Prd6 LDAA #$0B STAA PWDTY0 LDAA #$08 STAA PWDTY2 LDAA #$0B STAA PWDTY3 PULD RTS *********************************************** * TurnRgt * Turn robot to its Right *********************************************** TurnRgt PSHD * Wait a littlebit LDD #$3000 JSR WaitTmr * Push and turn LDD #$02F0 STD Prd1 LDD #$02A0 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0320 STD Prd4 LDD #$0240 STD Prd5 LDAA #$06 STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Wait a littlebit LDD #$2000 JSR WaitTmr * New Tail position LDAA #$12 JSR MTail * Wait a littlebit LDD #$0400 JSR WaitTmr * Set Servos to standing position * LLeg up LDD #$0330 STD Prd1 LDD #$01A0 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0220 STD Prd4 LDD #$02A0 STD Prd5 LDAA #$08 STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Wait a littlebit LDD #$1000 JSR WaitTmr * Set Servos to standing position LDD #$0330 STD Prd1 LDD #$0200 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0280 STD Prd4 LDD #$02A0 STD Prd5 LDD #$02A0 STD Prd6 LDAA #$0B STAA PWDTY0 LDAA #$08 STAA PWDTY2 LDAA #$0B STAA PWDTY3 PULD RTS *********************************************** * StepLft * Take a step w/Left foot *********************************************** StepLft PSHD * Wait a littlebit LDD #$3000 JSR WaitTmr LDAA #$0E STAA PWDTY0 * Wait a littlebit LDD #$1000 JSR WaitTmr * Set Servos to Upright Standing LDD #$02A0 STD Prd1 LDD #$02A0 STD Prd2 LDD #$02A0 STD Prd3 LDD #$02A0 STD Prd4 LDAA #$11 STAA PWDTY0 LDAA #$0A STAA PWDTY2 LDAA #$0A STAA PWDTY3 * Wait a littlebit LDD #$2000 JSR WaitTmr * Set Servos to LLeg forward LDD #$02A0 STD Prd1 LDD #$0280 STD Prd2 LDD #$02A0 STD Prd3 LDD #$0220 STD Prd4 LDAA #$0C STAA PWDTY2 LDAA #$0A STAA PWDTY3 * Wait a littlebit LDD #$1000 JSR WaitTmr * Set Servos to LLeg forward Squat LDD #$0330 STD Prd1 LDD #$0280 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0220 STD Prd4 LDAA #$0C STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Wait a littlebit LDD #$1000 JSR WaitTmr * New Tail position LDAA #$0B JSR MTail * Wait a littlebit LDD #$0800 JSR WaitTmr * Pre LLeg shift LDD #$0240 STD Prd1 LDD #$02A0 STD Prd2 LDD #$01D0 STD Prd3 LDD #$02B0 STD Prd4 LDAA #$0A STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Wait a littlebit LDD #$1800 JSR WaitTmr * Set Servos to Shift to LLeg LDD #$0240 STD Prd1 LDD #$0280 STD Prd2 LDD #$01A0 STD Prd3 LDD #$0290 STD Prd4 LDAA #$0A STAA PWDTY2 LDAA #$0C STAA PWDTY3 * Wait a littlebit LDD #$1000 JSR WaitTmr * New Tail position ($02) LDAA #$02 JSR MTail * Wait a littlebit LDD #$1000 JSR WaitTmr * Set Servos to Shift to LLeg Up LDD #$0240 STD Prd1 LDD #$02C0 STD Prd2 LDD #$0200 STD Prd3 LDD #$02B0 STD Prd4 LDAA #$0A STAA PWDTY2 LDAA #$0B STAA PWDTY3 * Wait a littlebit LDD #$1800 JSR WaitTmr * New Tail position ($02) * LDAA #$02 * JSR MTail * Wait a littlebit LDD #$1000 JSR WaitTmr * Move RT leg foreward LDD #$02D0 STD Prd1 LDD #$02C0 STD Prd2 LDD #$02A0 STD Prd3 LDD #$02B0 STD Prd4 LDAA #$0A STAA PWDTY2 LDAA #$0A STAA PWDTY3 * Wait a littlebit LDD #$1000 JSR WaitTmr * Set Servos to Squating Standing LDD #$0330 STD Prd1 LDD #$0200 STD Prd2 LDD #$02B0 STD Prd3 LDD #$0280 STD Prd4 LDAA #$08 STAA PWDTY2 LDAA #$0B STAA PWDTY3 * New Tail position (Center) LDAA #$0B JSR MTail * Return PULD RTS *************************************************** * MTail * Moves Tail to position in A * Tail movements should be done here to * ensure they are done slowly enough to * prevent sliding around *************************************************** MTail PSHA * Needs to Add or Subtract? CMPA PWDTY0 BGT MTailRt * Adjust Tail location MTailLf DEC PWDTY0 * Wait a Short while PSHD LDD #$0600 JSR WaitTmr PULD CMPA PWDTY0 BNE MTailLf * Branch to End BRA MTailEd MTailRt INC PWDTY0 * Wait a Short while PSHD LDD #$0600 JSR WaitTmr PULD CMPA PWDTY0 BNE MTailRt MTailEd PULA STAA PWDTY0 RTS *************************************************** * WaitTmr * Waits for a while *************************************************** WaitTmr PSHD WaitTm0 JSR WaitLit DBNE D,WaitTm0 PULD RTS WaitLit PSHA LDAA #$FF WaitLi0 DBNE A,WaitLi0 PULA RTS *************************************************** * NewProc, Starts a new proccess * its Starting address is in X *************************************************** NewProc PSHD PSHX PSHY * Verify a process can be started LDAA ProcAct CMPA MaxProc BEQ NewEnd SEI * Save current SP STS TempSP * Indicate a new process is running INC ProcAct TAB * Get New SP LSLB LDY #ProcNSP LDS B,Y * Setup Stack PSHX PSHX PSHX PSHX LDAA StdCCR PSHA * Save SP LDY #ProcSP STS B,Y * Restore SP LDS TempSP CLI NewEnd PULY PULX PULD RTS *************************************************** * SWProc * Switches Processes *************************************************** SWProc PULX STX SWRtn *************************************************** * Save Current SP * Get SP Table LDX #ProcSP * Load Proccess number LDAA ProcNum * B is the offset TAB LSLB * Save proccesses SP STS B,X *************************************************** * Get new process to run * Incriment ProcNum LAgain INCA CMPA #MaxProc BEQ ClrPID * B is offset DClrPID TAB LSLB * Load possible SP LDAB B,X BEQ LAgain *************************************************** * Found a new process STAA ProcNum LSLA * Get New SP LDS A,X LDX SWRtn PSHX RTS ClrPID CLRA BRA DClrPID *************************************************** * Defer, used to defer a process *************************************************** Defer SEI LDAA #SWIDefr STAA SWIOp SWI CLI RTS *************************************************** * Outputs a string when interupts * are enabled *************************************************** OutStrI PSHA PSHX Here01I LDAA 0,X CMPA #EOS BEQ DoneStr JSR OutCharI INX BRA Here01I DoneStr PULX PULA RTS *************************************************** * Outputs a char when interupts * are enabled *************************************************** OutCharI SEI JSR OutChar CLI RTS *************************************************** * Outputs Data in ASCII format (byte) * Data to be output in A *************************************************** OutData PSHD * Save A in B TFR A,B * Shift High 4 bits into lower bits LSRA LSRA LSRA LSRA * Remove upper 1/2 of A ANDA #%00001111 * Asume it is a number ADDA #$30 CMPA #$3A BLT OutDat0 * It is a hex A,B ... ADDA #$07 * Send out Char OutDat0 JSR OutCharI * Restore A TFR B,A * Mask it just in case ANDA #%00001111 * Asume it is a number ADDA #$30 CMPA #$3A BLT OutDat1 * It is a hex A,B ... ADDA #$07 * Send out Char OutDat1 JSR OutCharI PULD RTS *************************************************** * Outputs a Word in ASCII format (16 bits) * Data to be output in D *************************************************** OutWord PSHD * Send out A first BSR OutData * Restore D PULD PSHD * Then B TFR B,A BSR OutData PULD RTS *************************************************** * RTI interupt service routene *************************************************** * Clear Flag RTIISR LDAA #%10000000 STAA RTIFLG * Branch if defered last LDAA RTIDefr BNE Defered * Switch Proccesses JSR SWProc Defered CLR RTIDefr * Incriment RTI counter LDD RTICntr ADDD #$01 STD RTICntr RTI *************************************************** * SWI interupt service routene * Used for defer *************************************************** SWIISR LDAA SWIOp BEQ SWIEnd CMPA #SWIDefr BEQ SWIOp2 SWIEnd RTI * A defer has been called * Switch processes SWIOp2 JSR SWProc * Indicate this precess was defered to * It will get more time LDAA #$01 STAA RTIDefr RTI *********************************************** * OC1ISR *********************************************** OC1ISR BRSET TCTL2,BIT2,OC1Set * Enable OC to high next time BSET TCTL2,BIT2 * Add in Low time LDD TC1 ADDD #Period SUBD Prd1 STD TC1 BRA OC1End * Clear OC for next time OC1Set BCLR TCTL2,BIT2 * Add in High time LDD TC1 ADDD Prd1 STD TC1 OC1End RTI *********************************************** * OC2ISR *********************************************** OC2ISR BRSET TCTL2,BIT4,OC2Set * Enable OC to high next time BSET TCTL2,BIT4 * Add in Low time LDD TC2 ADDD #Period SUBD Prd2 STD TC2 BRA OC2End * Clear OC for next time OC2Set BCLR TCTL2,BIT4 * Add in High time LDD TC2 ADDD Prd2 STD TC2 OC2End RTI *********************************************** * OC3ISR *********************************************** OC3ISR BRSET TCTL2,BIT6,OC3Set * Enable OC to high next time BSET TCTL2,BIT6 * Add in Low time LDD TC3 ADDD #Period SUBD Prd3 STD TC3 BRA OC3End * Clear OC for next time OC3Set BCLR TCTL2,BIT6 * Add in High time LDD TC3 ADDD Prd3 STD TC3 OC3End RTI *********************************************** * OC4ISR *********************************************** OC4ISR BRSET TCTL1,BIT0,OC4Set * Enable OC to high next time BSET TCTL1,BIT0 * Add in Low time LDD TC4 ADDD #Period SUBD Prd4 STD TC4 BRA OC4End * Clear OC for next time OC4Set BCLR TCTL1,BIT0 * Add in High time LDD TC4 ADDD Prd4 STD TC4 OC4End RTI *********************************************** * OC5ISR *********************************************** OC5ISR BRSET TCTL1,BIT2,OC5Set * Enable OC to high next time BSET TCTL1,BIT2 * Add in Low time LDD TC5 ADDD #Period SUBD Prd5 STD TC5 BRA OC5End * Clear OC for next time OC5Set BCLR TCTL1,BIT2 * Add in High time LDD TC5 ADDD Prd5 STD TC5 OC5End RTI *********************************************** * OC6ISR *********************************************** OC6ISR BRSET TCTL1,BIT4,OC6Set * Enable OC to high next time BSET TCTL1,BIT4 * Add in Low time LDD TC6 ADDD #Period SUBD Prd6 STD TC6 BRA OC6End * Clear OC for next time OC6Set BCLR TCTL1,BIT4 * Add in High time LDD TC6 ADDD Prd6 STD TC6 OC6End RTI