*********************************************** * * * load_re.s * * * * - Author: Mariusz Zaczek (zaczek@uiuc.edu) * * - Date: 02/27/2000 * * * *********************************************** opt nolist *********************************************** * * * This code is used for the RECEIVER side * * of the robot control system. * * * * Port QS is used to control motor directions* * bit 7 - not used * * 6 - Elbow direction * * 5 - Arm direction * * 4 - Base direction * * 3 - Blades #2 * * 2 - Blades #1 * * 1 - Motor #2 * * 0 - Motor #1 * * * *********************************************** * * Equates * FUNCNUM_2 equ $9999 * 7) No, 6) PWM, 5) PWM, 4) PWM FUNCNUM_1 equ $9999 * 3) PWM, 2) PWM, 1) PWM, 0) No LETTER_A equ %01000001 * HEX $41 - Header letter LETTER_B equ %01000010 * HEX $42 - BAD mode LETTER_C equ %01000011 * HEX $43 - NICE mode 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 JoyMin equ $55 JoyMax equ $ab * LEFT SIDE MOTOR SERVO CHANNEL_0 equ $FFFF00 * CH 0 - PWM (control reg) HIGH_TIME_0 equ $FFFF04 * ( high time ) PERIOD_0 equ $FFFF06 * ( period ) * Servo to select arm CHANNEL_1 equ $FFFF10 * CH 1 - PWM (control reg) HIGH_TIME_1 equ $FFFF14 * ( high time ) PERIOD_1 equ $FFFF16 * ( period ) * Base Swivel (HAT button left/right) CHANNEL_2 equ $FFFF20 * CH 2 - PWM (control reg) HIGH_TIME_2 equ $FFFF24 * ( high time ) PERIOD_2 equ $FFFF26 * ( period ) * Base Arm (HAT button up/down) CHANNEL_3 equ $FFFF30 * CH 3 - PWM (control reg) HIGH_TIME_3 equ $FFFF34 * ( high time ) PERIOD_3 equ $FFFF36 * ( period ) * Arm Link #2 (BIG buttons left/right) CHANNEL_4 equ $FFFF40 * CH 4 - PWM (control reg) HIGH_TIME_4 equ $FFFF44 * ( high time ) PERIOD_4 equ $FFFF46 * ( period ) * Gripper (3rd Pot) CHANNEL_5 equ $FFFF50 * CH 5 - PWM (control reg) HIGH_TIME_5 equ $FFFF54 * ( high time ) PERIOD_5 equ $FFFF56 * ( period ) * Iris (Trigger button) CHANNEL_6 equ $FFFF60 * CH 6 - PWM (control reg) HIGH_TIME_6 equ $FFFF64 * ( high time ) PERIOD_6 equ $FFFF66 * ( period ) * RIGHT SIDE MOTOR SERVO CHANNEL_7 equ $FFFF70 * CH 7 - PWM (control reg) HIGH_TIME_7 equ $FFFF74 * ( high time ) PERIOD_7 equ $FFFF76 * ( 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) * Both main motors are set for FWD direction (1) and all pittman * motors are set for FWD direction. move.b #%01110011,PORTQS * * Main_Loop * * - Main program body. * Main_Loop jsr GetData * get data, test checksum jsr Control * Set PWM functions bra Main_Loop * Loop * * 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 clr.l D6 clr.l D7 move.w #$0092,(CHANNEL_0).L * PWM (channel 0) - Left Motor move.w #Servo_Mid,(HIGH_TIME_0).L * 50% High Time move.w #Servo_Period,(PERIOD_0).L * PERIOD = 20 ms move.w #$0092,(CHANNEL_1).L * PWM (channel 1) - Select Servo move.w #Servo_Max,(HIGH_TIME_1).L * 100% High Time - full closed move.w #Servo_Period,(PERIOD_1).L * PERIOD = 20 ms move.w #$0092,(CHANNEL_2).L * PWM (channel 2) - Base swivel move.w #$0000,(HIGH_TIME_2).L * 0% High Time - stopped move.w #PittmanPer,(PERIOD_2).L * PERIOD = $1000 TICR move.w #$0092,(CHANNEL_3).L * PWM (channel 3) - Base Arm move.w #$0000,(HIGH_TIME_3).L * 0% High Time - stopped move.w #PittmanPer,(PERIOD_3).L * PERIOD = $1000 TICR move.w #$0092,(CHANNEL_4).L * PWM (channel 4) - Arm #2 move.w #$0000,(HIGH_TIME_4).L * 0% High Time - stopped move.w #PittmanPer,(PERIOD_4).L * PERIOD = $1000 TICR move.w #$0092,(CHANNEL_5).L * PWM (channel 5) - Gripper Servo move.w #Servo_Max,(HIGH_TIME_5).L * 100% High Time - full open move.w #Servo_Period,(PERIOD_5).L * PERIOD = 20 ms move.w #$0092,(CHANNEL_6).L * PWM (channel 6) - IRIS Servo move.w #Servo_Max,(HIGH_TIME_6).L * 100% High Time - full closed move.w #Servo_Period,(PERIOD_6).L * PERIOD = 20 ms move.w #$0092,(CHANNEL_7).L * PWM (channel 5) - Motor Right move.w #Servo_Mid,(HIGH_TIME_7).L * 50% High Time move.w #Servo_Period,(PERIOD_7).L * PERIOD = 20 ms 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 2: Mode (BAD / NICE) [ D7 ] * Byte 2: 8 on/off signals [ D1 ] * Byte 3: Potentiometer #1 [ D2 ] * Byte 4: Potentiometer #2 [ D3 ] * Byte 5: Potentiometer #3 [ D4 ] * GetData clr.l D0 clr.l D1 clr.l D2 clr.l D3 clr.l D4 clr.l D5 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 bne GetHeader GetMode jsr GetNextByte move.w D0,D7 GetFirst jsr GetNextByte move.w D0,D1 GetSecond jsr GetNextByte move.w D0,D2 GetThird jsr GetNextByte move.w D0,D3 GetFourth jsr GetNextByte move.w D0,D4 GetChecksum jsr GetNextByte move.w D0,D6 Checksum clr.l D0 eor.b D1,D0 eor.b D2,D0 eor.b D3,D0 eor.b D4,D0 eor.b D7,D0 cmp.b D0,D6 bne GetData GetDataEnd rts * End ( GetData ) * * Control * * - Control everything * * Control cmp.w #LETTER_B,D7 beq BadMode * if D7 is letter B then "BAD" mode * *********************** * ****** NICE mode ****** * *********************** NiceMode * * IRIS * Iris clr.l D0 move.w D1,D0 andi.w #%00000001,D0 beq IrisClosed IrisOpen move.w #Servo_Max,HIGH_TIME_6 jmp IrisEnd IrisClosed move.w #Servo_Min,HIGH_TIME_6 IrisEnd * * ARM * 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 #$0000,HIGH_TIME_3 * Stopped jmp ArmEnd ArmUp bset #5,PORTQS jmp ArmSpeed ArmDown bclr #5,PORTQS ArmSpeed move.w #PittmanHigh,HIGH_TIME_3 ArmEnd * * BASE * Base clr.l D0 move.w D1,D0 andi.w #%00001000,D0 bne BaseLeft clr.l D0 move.w D1,D0 andi.w #%00010000,D0 bne BaseRight move.w #$0000,HIGH_TIME_2 jmp BaseEnd BaseLeft bset #4,PORTQS jmp BaseSpeed BaseRight bclr #4,PORTQS BaseSpeed move.w #PittmanHigh,HIGH_TIME_2 BaseEnd * * ELBOW * Elbow clr.l D0 move.w D1,D0 andi.w #%00100000,D0 bne ElbowUp clr.l D0 move.w D1,D0 andi.w #%01000000,D0 bne ElbowDown move.w #$0000,HIGH_TIME_4 jmp ElbowEnd ElbowUp bset #6,PORTQS jmp ElbowSpeed ElbowDown bclr #6,PORTQS ElbowSpeed move.w #PittmanHigh,HIGH_TIME_4 ElbowEnd * * GRIPPER * Gripper mulu.w #Servo_Diff,D4 divu.w #$00ff,D4 add.w #Servo_Min,D4 move.w D4,HIGH_TIME_5 GripperEnd jmp AllModes * *********************** * ****** BAD mode ****** * *********************** BadMode Blades * * FrontBlades * FrontBlades clr.l D0 move.w D1,D0 andi.w #%00000001,D0 beq FrontBladesOff FrontBladesOn bset #2,PORTQS jmp BackBlades FrontBladesOff bclr #2,PORTQS * * BackBlades * BackBlades clr.l D0 move.w D1,D0 andi.w #%01000000,D0 bne BackBladesOn move.w D1,D0 andi.w #%00100000,D0 beq BackBladesOff BackBladesOn bset #3,PORTQS jmp BladesEnd BackBladesOff bclr #3,PORTQS BladesEnd * *********************** * ****** All modes ****** * *********************** AllModes * * SelectArm * SelectArm clr.l D0 move.w D1,D0 andi.w #%10000000,D0 beq ArmTwo ArmOne move.w #Servo_Max,HIGH_TIME_1 jmp SelectArmEnd ArmTwo move.w #Servo_Min,HIGH_TIME_1 SelectArmEnd * * MotorControl * MotorControl cmp.w #JoyMin,D2 ble JoyLeft cmp.w #JoyMax,D2 bge JoyRight cmp.w #JoyMin,D3 ble JoyBottom cmp.w #JoyMax,D3 bge JoyTop jmp JoyNeutral JoyLeft cmp.w #JoyMin,D3 ble JoyNeutral cmp.w #JoyMax,D3 bge JoyNeutral * move.w #Servo_Min,HIGH_TIME_0 move.w #$020f,HIGH_TIME_0 * move.w #Servo_Max,HIGH_TIME_7 move.w #$035a,HIGH_TIME_7 jmp MotorControlEnd JoyRight cmp.w #JoyMin,D3 ble JoyNeutral cmp.w #JoyMax,D3 bge JoyNeutral * move.w #Servo_Max,HIGH_TIME_0 move.w #$0343,HIGH_TIME_0 * move.w #Servo_Min,HIGH_TIME_7 move.w #$01ab,HIGH_TIME_7 jmp MotorControlEnd JoyTop cmp.w #JoyMin,D2 ble JoyNeutral cmp.w #JoyMax,D2 bge JoyNeutral * move.w #Servo_Max,HIGH_TIME_0 move.w #$0343,HIGH_TIME_0 * move.w #Servo_Max,HIGH_TIME_7 move.w #$035a,HIGH_TIME_7 jmp MotorControlEnd JoyBottom cmp.w #JoyMin,D2 ble JoyNeutral cmp.w #JoyMax,D2 bge JoyNeutral * move.w #Servo_Min,HIGH_TIME_0 move.w #$020f,HIGH_TIME_0 * move.w #Servo_Min,HIGH_TIME_7 move.w #$01ab,HIGH_TIME_7 jmp MotorControlEnd JoyNeutral move.w #Servo_Mid,HIGH_TIME_0 move.w #Servo_Mid,HIGH_TIME_7 MotorControlEnd 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-6 to be PWM * TPUInit * Set channel 0-6 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 * ($00a0 = %10100000) move.w #$aaaa,(HSRR1).L * ($00f0 = %11110000) move.w #$ffff,(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 )