***************************************************** * * * eoh_l_r.s ( EOH RECEIVE Load Version ) * * * * xxx_y_z.s * * * * xxx = eoh - engineering open house version * * tlc - TLC Robot Competition version * * y = l - LOAD version...via serial port * * b - BOARD version...via EEPROMS * * z = t - TRANSMITTER * * r - RECEIVER * * * * -Author : Mariusz Zaczek (zaczek@uiuc.edu) * * -Date : 01/11/2001 * * * ***************************************************** opt nolist ***************************************************** * * * This code is used for the RECEIVER side of the * * robot control system. * * * * The packet received is used to control the * * motors, weapons, etc. * * * * The items to control include: * * 1) Left motor * * PWM via TPU channel 0 * * DIRECTION via PORTQS bit 0 (pin MISO 48) * * * * 2) Right Motor * * PWM via TPU channel 1 * * DIRECTION via PORTQS bit 1 (pin MOSI 47) * * * * 3) Main Arm using Pittman Motor * * PWM via TPU channel 2 * * DIRECTION via PORTQS bit 2 (pin sck 50) * * Use LMD18200 driver chip * * * * 4) Lightbulb Unscrewing using Pittman Motor * * PWM via TPU channel 3 * * Direction is hardwired to one direction * * Use LMD18200 driver chip * * * * 5) Release/Retract using Pittman Motor * * PWM via TPU channel 4 * * DIRECTION via PORTQS bit 3 (pin PCS0 49) * * Use LMD18200 driver chip * * * ***************************************************** *** Port usage: * ***************************************************** * * * IMPORTANT NOTE: * * See page 4 of: "An Introduction to the MC6832" * * PULL DATA PINS 8 AND 9 LOW using the 74hct244n * * tri-state octal buffer. Do not use the DS or * * R/W pins as shown in the circuit of the manual.* * We want to use the DS pin as part of the input * * buttons but if the circuit is used as is then * * the pin can't be used for buttons inputs. So, * * instead we can just connect the RESET pin to * * the enable of the 74hct244n and then put low * * the two necessary pins of the buffer which * * will then pull low the DATAx pins. * * DATA8 = PORTE I/O when pulled low * * DATA9 = PORTF I/O when pulled low * * * * Port E ( output ) * * M68332 PIN * * ------------------------------------------- * * 0 DSACK0 (49) * * 1 DSACK1 (50) * * 2 AVEC (51) * * 3 RMC (52) * * 4 DS (53) * * 5 AS (54) * * 6 SIZ0 (55) * * 7 SIZ1 (56) * * * * Port F ( not assigned ) * * M68332 PIN * * ------------------------------------------- * * * * Port QS ( output ) 7 pins only * * BIT (pin) Description * * ------------------------------------------- * * 0 (MISO 48) - Motor #1 Direction * * 1 (MOSI 47) - Motor #2 Direction * * 2 (SCK 50) - Main arm direction * * 3 (PCS0 49) - Lightbulb PUSH out direction * * 4 (PCS1 52) - Pneumatic Arm On/Off * * 5 (PCS2 51) - * * 6 (PCS3 54) - * * 7 - NOT USED * * * ***************************************************** *** Register usage: * ***************************************************** * * * D0 - all purpose * * D1 - 8 on/off toggle inputs * * D2 - Left Potentiometer * * D3 - Right Potentiometer * * D4 - more on/off toggle inputs * * D5 - Error Counter * * D6 - Checksum * * D7 - * * * ***************************************************** *** OUTPUT: * ***************************************************** * * * M68332 PIN Purpose * * ------------------------------------------- * * TP0 (46) Left Motor PWM * * TP1 (45) Right Motor PWM * * MISO (48) Left Motor Direction * * MOSI (47) Right Motor Direction * * * ***************************************************** *** Data Packet Skeleton * ***************************************************** * * * * * .... * * * * * * * * * ***************************************************** *** Program Execution * ***************************************************** * * * - Initialize the system * * - Initialize the TPU for PWM outputs * * - Initialize the SCI to be a receiver * * - Initialize PORTQS for output * * - Set PORT E to be OUTPUT port * * - Main Loop * * - Read the data from the SCI * * - Control the various equipment * * - End Main Loop * * * ***************************************************** * * Equates * FUNCNUM_2 equ $0009 * 7) No, 6) No, 5) No, 4) PWM FUNCNUM_1 equ $9999 * 3) PWM, 2) PWM, 1) PWM, 0) PWM LETTER_A equ %01000001 * HEX $41 - Header letter Servo_Period equ $28f6 * 20 ms Servo_Max equ $037b * 1.7 ms Servo_Min equ $016f * 0.7 ms Servo_Mid equ $029d * Mid value ... 1.2 ms Servo_Diff equ $020c * Diff between max & min = 1.0ms PittmanHigh equ $0c00 * 75% of max speed($1000) PittmanPer equ $1000 * Period of Pittman motors PittmanStop equ $0000 * 0% PWM for Pittman motors - STOPPED JoyMin equ $55 * JoyMax equ $ab * Max_Error equ $0005 * Counter = 5 MotorPeriod equ $1000 * Period of PWM for motors (4096) MotorStop equ $0000 * 0% PWM - STOPPED MOTOR ( 0) Motor05 equ $00cd * 5% PWM - Max Forward Speed ( 205) Motor45 equ $0732 * 45% PWM - Deadband (1842) Motor50 equ $0800 * 50% PWM - MOTOR STOPPED (2048) Motor44 equ $08cc * 55% PWM - Deadband (2252) Motor95 equ $0f33 * 95% PWM - Max Reverse Speed (3891) MinDeadBand equ $0073 * 73h = 115d (0-255) MaxDeadBand equ $008d * 8dh = 141d (0-255) * LEFT SIDE MOTOR CHANNEL_0 equ $FFFF00 * CH 0 - PWM (control reg) HIGH_TIME_0 equ $FFFF04 * ( high time ) PERIOD_0 equ $FFFF06 * ( period ) * RIGHT SIDE MOTOR CHANNEL_1 equ $FFFF10 * CH 1 - PWM (control reg) HIGH_TIME_1 equ $FFFF14 * ( high time ) PERIOD_1 equ $FFFF16 * ( period ) * MAIN ARM MOTOR CHANNEL_2 equ $FFFF20 * CH 2 - PWM (control reg) HIGH_TIME_2 equ $FFFF24 * ( high time ) PERIOD_2 equ $FFFF26 * ( period ) * LIGHTBULB UNSCREM MOTOR CHANNEL_3 equ $FFFF30 * CH 3 - PWM (control reg) HIGH_TIME_3 equ $FFFF34 * ( high time ) PERIOD_3 equ $FFFF36 * ( period ) * LIGHBULB RELEASE/RETRACT MOTOR CHANNEL_4 equ $FFFF40 * CH 4 - PWM (control reg) HIGH_TIME_4 equ $FFFF44 * ( high time ) PERIOD_4 equ $FFFF46 * ( period ) org $9000 Start include equ332.asm move.w #$2700,SR jsr InitSys * Initialize System jsr TPUInit * Initialize TPU jsr SCIInit * Initialize SCI jsr PortQSInit * Initialize Port QS (Output) jsr PortEInit * Port E (Output) * * Main_Loop * * - Main program body. * Main_Loop jsr GetData * get data, test checksum jsr Control * Set PWM functions bra Main_Loop * Loop continuously * * SafeRobot * * - Routine to SAFE the robot... * - This includes: disabling the motor drivers * turning off weapons * turning off all other active motors * SafeRobot * Stop all motors ... 0% PWM move.w #MotorStop,(HIGH_TIME_0).L * 0% High Time = LEFT Motor STOPPED move.w #MotorStop,(HIGH_TIME_1).L * 0% High Time = Right Motor STOPPED move.w #PittmanStop,(HIGH_TIME_2).L * 0% High Time = Main Arm STOPPED move.w #PittmanStop,(HIGH_TIME_3).L * 0% High Time = Unscrew STOPPED move.w #PittmanStop,(HIGH_TIME_4).L * 0% High Time = Retract STOPPED * Disable Pneumatic Ram bclr #4,PORTQS rts * End ( SafeRobot ) * * SetPWM * * - Sets the original control reg., periods and High times * SetPWM clr.l D0 clr.l D1 * Clears all pertinent registers clr.l D2 clr.l D3 clr.l D4 clr.l D5 * Clear Error Counter clr.l D6 clr.l D7 * LEFT Motor, Set PWM to 50% = STOP move.w #$0092,(CHANNEL_0).L * PWM (channel 0) - Left Motor move.w #MotorStop,(HIGH_TIME_0).L * 0% High Time = STOPPED move.w #MotorPeriod,(PERIOD_0).L * PERIOD * RIGHT Motor, Set PWM to 50% = STOP move.w #$0092,(CHANNEL_1).L * PWM (channel 1) - Right Motor move.w #MotorStop,(HIGH_TIME_1).L * 0% High Time = STOPPED move.w #MotorPeriod,(PERIOD_1).L * PERIOD * MAIN ARM Motor, Set PWM to 0% = STOP move.w #$0092,(CHANNEL_2).L * PWM (channel 2) - Main Arm Motor move.w #PittmanStop,(HIGH_TIME_2).L * 0% High Time = STOPPED move.w #PittmanPer,(PERIOD_2).L * PERIOD * UNSCREW Motor, Set PWM to 0% = STOP move.w #$0092,(CHANNEL_3).L * PWM (channel 3) - UNSCREW Motor move.w #PittmanStop,(HIGH_TIME_3).L * 0% High Time = STOPPED move.w #PittmanPer,(PERIOD_3).L * PERIOD * RELEASE/RETRACT Motor, Set PWM to 0% = STOP move.w #$0092,(CHANNEL_4).L * PWM (channel 4) - RELEASE Motor move.w #PittmanStop,(HIGH_TIME_4).L * 0% High Time = STOPPED move.w #PittmanPer,(PERIOD_4).L * PERIOD rts * End ( SetPWM ) * * GetNextByte * * - Reads a byte of data * GetNextByte move.w (SCSR).L,D0 * Get SCSR andi.w #$0040,D0 * Test to see if SCSR has received data beq GetNextByte * if not then keep testing... move.w (SCDR).L,D0 * if Yes then read data andi.w #$00ff,D0 * Clears 2nd byte of word..leaves 1st rts * End ( GetNextByte ) * * GetData * * - Byte 1: Header (Letter A) [ D0 ] register * Byte 3: 8 on/off signals [ D1 ] * Byte 4: Potentiometer #1 [ D2 ] * Byte 5: Potentiometer #2 [ D3 ] * * GetData clr.l D0 clr.l D1 clr.l D2 clr.l D3 clr.l D4 clr.l D6 clr.l D7 GetHeader jsr GetNextByte cmp.w #LETTER_A,D0 * If byte is letter A, beginning of header bne GetHeader * else return to reading data..wait for A GetHeaderTwo jsr GetNextByte cmp.w #LETTER_A,D0 * If 2ND byte is letter A, beginning of header bne GetHeader * else return to reading data..FROM BEGINNING GetFirst jsr GetNextByte * Get 8 i/o inputs move.w D0,D1 GetSecond jsr GetNextByte * Get LEFT motor value move.w D0,D2 GetThird jsr GetNextByte * Get RIGHT motor value move.w D0,D3 GetChecksum jsr GetNextByte * Get CHECKSUM for error checking move.w D0,D6 * Take each byte of data and XOR all values, storing in D0 Checksum clr.l D0 eor.b D1,D0 eor.b D2,D0 eor.b D3,D0 * Check the CHECKSUM,D6 with D0 to make sure data received was good. cmp.b D0,D6 jmp ChecksumPassed * IF (checksum check FAILS) then * update the error counter and try to read data again from beginning. * * IF ( Error counter >= Max Error Counter ) then * safe the robot... * ENDIF * ELSE (checksum is OK) then * continue on and update all motor PWMs, etc. * ENDIF bne ChecksumFailed * Checksum failed...increase error count jmp ChecksumPassed * Checksum passed so continue on * Checksum failed so test Error counter, D5 ChecksumFailed add.l #1,D5 * Add one to Error counter cmp.l #Max_Error,D5 * Test to see if Error counter is at max bge SafeRobot * If counter is >= Max, then SAFE the robot jmp GetData ChecksumPassed clr.l D5 * Clear counter since GOOD packet read GetDataEnd rts * End ( GetData ) * * Control * * - Control everything * Control * * MotorControl * MotorControl Motor_1 cmp.w #MinDeadBand,D2 * Check for Min Deadband bge CheckDeadband_1 jmp Motor_1_REV CheckDeadband_1 cmp.w #MaxDeadBand,D2 * Check for Max Deadband ble Motor_1_deadband jmp Motor_1_FWD Motor_1_deadband bset #0,PORTQS * Set Motor #1 for FWD move.w #MotorStop,HIGH_TIME_0 * Then set PWM to 0% jmp Motor_Break Motor_1_FWD mulu.w #MotorPeriod,D2 * Convert position to PWM high time value by divu.w #$ff,D2 * taking [(0-255 position) * PERIOD] / 256 cmp.w #Motor95,D2 blt Motor_1_FWD_set * If HIGH TIME is < than 95% then set motor move.w #Motor95,D2 * else make this only 95% PWM Motor_1_FWD_set move.w D2,HIGH_TIME_0 * Set HIGH TIME value bset #0,PORTQS * FORWARD direction jmp Motor_Break Motor_1_REV mulu.w #MotorPeriod,D2 * Convert position to PWM high time value by divu.w #$ff,D2 * taking [(0-255 position) * PERIOD] / 256 cmp.w #Motor05,D2 bgt Motor_1_REV_set * If HIGH TIME is > than 5% then set motor move.w #Motor05,D2 * else make this only 5% PWM Motor_1_REV_set move.w D2,HIGH_TIME_0 * Set HIGH TIME value bclr #0,PORTQS * REVERSE direction jmp Motor_Break Motor_Break Motor_2 cmp.w #MinDeadBand,D3 * Check for Min Deadband bge CheckDeadband_2 jmp Motor_2_REV CheckDeadband_2 cmp.w #MaxDeadBand,D3 * Check for Max Deadband ble Motor_2_deadband jmp Motor_2_FWD Motor_2_deadband bset #1,PORTQS * Set Motor #2 for FWD move.w #MotorStop,HIGH_TIME_1 * Then set PWM to 0% jmp MotorControlEnd Motor_2_FWD mulu.w #MotorPeriod,D3 * Convert position to PWM high time value by divu.w #$ff,D3 * taking [(0-255 position) * PERIOD] / 256 cmp.w #Motor95,D3 blt Motor_2_FWD_set * If HIGH TIME is < than 95% then set motor move.w #Motor95,D3 * else make this only 95% PWM Motor_2_FWD_set move.w D3,HIGH_TIME_1 * Set HIGH TIME value bset #1,PORTQS * FORWARD direction jmp MotorControlEnd Motor_2_REV mulu.w #MotorPeriod,D3 * Convert position to PWM high time value by divu.w #$ff,D3 * taking [(0-255 position) * PERIOD] / 256 cmp.w #Motor05,D3 bgt Motor_2_REV_set * If HIGH TIME is > than 5% then set motor move.w #Motor05,D3 * else make this only 5% PWM Motor_2_REV_set move.w D3,HIGH_TIME_1 * Set HIGH TIME value bclr #1,PORTQS * REVERSE direction jmp MotorControlEnd MotorControlEnd * * ARM Motor * Arm clr.l D0 move.w D1,D0 andi.w #%00000010,D0 bne ArmUp clr.l D0 move.w D1,D0 andi.w #%00000100,D0 bne ArmDown move.w #PittmanStop,HIGH_TIME_2 * Stopped jmp ArmEnd ArmUp bset #2,PORTQS jmp ArmSpeed ArmDown bclr #2,PORTQS ArmSpeed move.w #PittmanHigh,HIGH_TIME_2 ArmEnd * * Lightbulb Motor * Lightbulb clr.l D0 move.w D1,D0 andi.w #%00001000,D0 bne LightbulbSpeed move.w #PittmanStop,HIGH_TIME_3 jmp LightbulbEnd LightbulbSpeed move.w #PittmanHigh,HIGH_TIME_3 LightbulbEnd * * Release/Retract Motor * Screw clr.l D0 move.w D1,D0 andi.w #%00010000,D0 bne Release clr.l D0 move.w D1,D0 andi.w #%00100000,D0 bne Retract move.w #PittmanStop,HIGH_TIME_4 jmp ScrewEnd Release bset #3,PORTQS jmp ScrewSpeed Retract bclr #3,PORTQS ScrewSpeed move.w #PittmanHigh,HIGH_TIME_4 ScrewEnd * * Pneumatic Ram * Ram clr.l D0 move.w D1,D0 andi.w #%00010000,D0 bne RamOff bset #4,PORTQS jmp RamEnd RamOff bclr #4,PORTQS RamEnd ControlEnd rts * End ( ControlMotors ) * * InitSys * * - Initializes system...primarily for UART right now * InitSys move.b #$7F,(SYNCR).L clr.b (SYPCR).L rts * End ( InitSys ) * * PortQSInit * * - Enable as Output * PortQSInit move.b #$00,PORTQS move.b #$80,PQSPAR move.b #$ff,DDRQS rts * End ( PortQSInit ) * * PortEInit * * - Enable as Output * PortEInit move.b #$00,PORTE0 * Clear move.b #$00,PEPAR * Set for I/O move.b #$ff,DDRE * Output rts * End ( PortEInit ) * * TPUInit * * - Initialize the TPU for channels 0-1 to be PWM * TPUInit * Set channel 0-1 to be PWM move.w #FUNCNUM_1,(CFSR3).L move.w #FUNCNUM_2,(CFSR2).L * Emu mode off, sup on, sysclk/32 ---> 1 TCR1 = 238ns*8 move.w #$0080,(TPUMCR).L * Set Host seq. reg. to NO PARITY move.w #$0000,(HSQR1).L * Call _SetPWM_ function to ... * ... set the PWM periods and control Regs jsr SetPWM * Set Host Service Request Register #1 for ch 0,1,2,3,4 * to 01 = 2, for initialization * ($02aa = %0000001010101010) move.w #$02aa,(HSRR1).L * Set Channel priority register #1 for ch 0,1,2,3,4 to HIGH = 11 * ($03ff = %0000001111111111) move.w #$03ff,(CPR1).L rts * End ( TPUInit ) * * SCIInit * * - Initialize Serial Communications Interface (SCI) * SCIInit move.w #$0037,(SCCR0).L * Set the SCI baud rate to 9600 move.w #$0004,(SCCR1).L * enable the receiver rts * End ( SCIInit )