***************************************************** * * * tlc_l_r.s ( TLC RECEIVER 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 : 02/17/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) Propane supply valve (normally closed) * * On/Off controlled via PORTQS bit 2 * * * * 4) Igniter * * On/Off controlled via PORTQS bit 3 * * * ***************************************************** *** Port usage: * ***************************************************** * * * IMPORTANT NOTE: * * ( Not necessary when NOT using ports E & F ) * * See page 4 of: "An Introduction to the MC68322" * * 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 ( no assigned ) * * 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) - Supply valve * * 3 (PCS0 49) - Igniter * * 4 (PCS1 52) - * * 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 - Used as TEMPORARY value in Moton Control* * * ***************************************************** *** 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 * * - Main Loop * * - Read the data from the SCI * * - Control the various equipment * * - End Main Loop * * * ***************************************************** * * Equates * FUNCNUM_2 equ $0000 * 7) No, 6) No, 5) No, 4) No FUNCNUM_1 equ $0099 * 3) No, 2) No, 1) PWM, 0) PWM LETTER_A equ %01000001 * HEX $41 - Header letter * NOT USED for this code * 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 Max_Error equ $00ff * Counter = 256 TEMPORARY MotorPeriod equ $1000 * Period of PWM for motors (4096) MotorStop equ $0000 * 0% PWM - STOPPED MOTOR ( 0) * * The speed controllers we are using have a 20% to 60% range for PWM * control because the control is 1-3 VDC. Where 1V (20%PWM) is * STOPPED and 3v (60%PWM) if FULL SPEED. * To account for oscillations I am increasing the 40% area to 45% * and setting the STOP to be 18% rather than 20%. * Forty equ $0733 * 45% of Period..should be 40% Twenty equ $02e1 * 18% of Period..should be 20% MinDeadBand equ $0073 * 73h = 115d (0-255) {MID=128} MaxDeadBand equ $008d * 8dh = 141d (0-255) {MID=128} MidValue equ $0080 * 80h = 128d ... MID value Temp equ $cd Sixty equ $99a * 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 ) 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) * * 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 = STOPPED move.w #MotorStop,(HIGH_TIME_1).L * 0% High Time = STOPPED * Close Supply Valve bclr #2,PORTQS * Turn off igniter bclr #3,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 rts * End ( SetPWM ) * * GetNextByte * * - Reads a byte of data * GetNextByte clr.l D0 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... clr.l D0 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 bne GoBackToData * 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 GoBackToData jmp GetData ChecksumPassed clr.l D5 * Clear counter since GOOD packet read GetDataEnd rts * End ( GetData ) * * Control * * - Control everything * Control * * SafetyCheck * SafetyCheck andi.w #$0001,D1 * Check if start bit is still on beq SafeRobot * If NOT then safe the robot * * MotorControl * MotorControl mulu.l #MotorPeriod,D2 divu.l #$ff,D2 move.w D2,HIGH_TIME_0 * mulu.l #MotorPeriod,D3 * divu.l #$ff,D3 * move.w D3,HIGH_TIME_1 move.w #Twenty,HIGH_TIME_1 * jmp MotorControlEnd MotorControlEnd * * Supply Valve - using Normally Closed Clippard Valve * * SupplyValve clr.l D0 move.w D1,D0 andi.w #%00000100,D0 bne SupplyValveOff bset #2,PORTQS jmp SupplyValveEnd SupplyValveOff bclr #2,PORTQS SupplyValveEnd * * Igniter (piezoelectric) * * Igniter clr.l D0 move.w D1,D0 andi.w #%00001000,D0 bne IgniterOff bset #3,PORTQS jmp IgniterEnd IgniterOff bclr #3,PORTQS IgniterEnd ControlEnd rts * End ( ControlMotors ) * * InitSys * * - Initializes system * 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 * to 01 = 2, for initialization * ($000a = %0000000000001010) move.w #$0a,(HSRR1).L * Set Channel priority register #1 for ch 0,1 to HIGH = 11 * ($000f = %0000000000001111) move.w #$000f,(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 )