* COMPILER CONSTANTS * ***********************************************************************************

Transcription

1 Copyright 2005 by Aaron Enes Adam Cardi Matt Wagner ASSEMBLY LANGUAGE CODE FOR THE TEETERING BUFFALO OR THE SELF BALANCING ROBOT OR THE REMOTE CONTROL SEGWAY COMPILER CONSTANTS PROG EQU $E000 CONST EQU $1040 STACK EQU $0047 TCNT EQU $100E TOC5 EQU $101E TOC3 EQU $101A PORTA EQU $1000 PORTD EQU $1008 DDRD EQU $1009 TIC3 EQU $1014 TCTL1 EQU $1020 TCTL2 EQU $1021 TMSK1 EQU $1022 TFLG1 EQU $1023 TMSK2 EQU $1024 TFLG2 EQU $1025 PACTL EQU $1026 ADCTL EQU $1030 OPTION EQU $1039 LED_ON EQU $80 LED_OFF EQU $00 SWITCH EQU $02 OC5_VEC EQU $FFE0 OC3_VEC EQU $FFE4 IC3_VEC EQU $FFEA RST_VEC EQU $FFFE OC5_ISR EQU $F000 OC3_ISR EQU $F100 IC3_ISR EQU $F200 ACCEL EQU $1031 ACCEL_F EQU $1034 GYRO EQU $1032 GYRO_F EQU $1033 PROGRAM SPACE SPACE FOR DYNAMIC PROGRAM VARIABLES START OF USER STACK HC11 REGISTERS USED TO TURN AND OFF AN INDICATOR LED FOR DEBUGGINS USED TO MONITOR RESET SWITCH THESE ARE THE VECTOR LOCATIONS FOR THE OC5,3 INTERUPTS SET THESE ACCORDING TO ROMON ENABLED/DISABLED INTERRUPT SERVICE ROUTINE (ISR) FOR OC5 ISR FOR OC3 ISR FOR IC3 EQUATES FOR ACCEL AND GYRO INPUTS. ACCEL TRIGGERS INT REGION _F DESIGNATES THE MORE FILTERED INPUTS DIR EQU $1008 LOCATION OF OUTPUT PORT USED FOR SETTING MOTOR DIRECTIONS REV_L EQU % USED TO CLEAR DIR BITS TO CHANGE DIRECTION REV_R EQU % FORW_L EQU % USED TO SET DIR BITS TO CHANGE DIRECTION FORW_R EQU % HERE ARE THE COMPILER EQUATES USED FOR CONSTANTS THAT AFFECT BALANCING AND PROGRAM OPERATION PERIOD EQU $05FA PERIOD OF PWM SIGNAL IN CLOCK CYCLES. (SET FOR 1KHZ) AINT_NULL EQU $3000 NULL VALUE OF ACCELEROMETER INTEGRATION THRESH EQU $FF 06 USED TO IMPLEMENT THE 2 GAIN REGIONS

2 RSET_ERROR EQU $09 THRESHOLD FOR INTEGRATION AND RAMPUP/DOWN ACC_GAIN EQU $0006 GAIN FOR ACCEL GYR_GAIN EQU $0002 GAIN FOR GYRO GYR_FGAIN EQU $0005 GAIN FOR GYRO_F AINTGAIN EQU $0009 USING FDIV, SO INCREASE TO DECREASE EFFECTIVE GAIN DUTY_MAX EQU $F0 DUTY_MIN EQU $05 MAXIMUM AND MINIMUM ALLOWABLE DUTY CYCLES OUTSIDE THIS RANGE THE OC INTERRUPTS OCCUR TOO OFTEN TURN_G EQU $0012 TURNING GAIN, USING FDIV DELTA EQU $08 INT_MAX EQU $3300 INT_MIN EQU $2D00 PWMTH EQU $0007 STEP SIZE FOR RAMPING UP AND RAMPING DOWN THE ACCEL INT MAX AND MIN ALLOWED FOR ACCEL INT MAX ALLOWED CHANGE OF DUTY CYCLE. USED FOR DIGITAL SMOOTHING STEER_NULL EQU $0BF2 VALUE OF T.I.C. WHEN X-MITTER INPUT IS NULL P_TIMEMIN EQU $0830 MAX AND MIN ALLOWED VALUES FOR T.I.C. P_TIMEMAX EQU $0F50 USED TO FILTER OUT ERRONEOUS STEERING SIGNAL INPUTS MEMORY SPACE FOR DYNAMIC PROGRAM VARIABLES ORG CONST OC3_HIGH RMB 2 OC3_LOW RMB 2 OC5_HIGH RMB 2 OC5_LOW RMB 2 DUTY1 RMB 1 DUTY2 RMB 1 STORES THE HIGH AND LOW TIMES FOR THE DUTY CYCLE CALCULATIONS DUTY CYCLE FOR EACH MOTOR FLOATING POINT VARIABLES MANT1 RMB 2 OP1 EXP1 RMB 1 MANT2 RMB 2 OP2 EXP2 RMB 1 TEMP1 RMB 2 used by routines TEMP2 RMB 2 Y RMB 2 substitute Y register QUOT RMB 2 used by FDIV SENSOR VARIABLES FOR CALCULATIONS GYRO_VAL RMB 2 ACCEL_VAL RMB 2 GYRO_NULL RMB 2 ACCEL_NULL RMB 2 GYRO_FNULL RMB 2 A_GAIN RMB 2 G_GAIN RMB 2 GNULL_CALC RMB 2 ANULL_CALC RMB 2 PWM RMB 2 PWM_L RMB 2 PWM_R RMB 2 PREVDTY1 RMB 1 PREVDTY2 RMB 1 PWM1 RMB 2 PWM2 RMB 2 MANTISSA RMB 2

3 EXPONENT RMB 1 MANT_L RMB 2 MANT_R RMB 2 ABS_ACCEL RMB 1 ABS_GYRO RMB 1 ACC_INT RMB 2 1 BYTE DEVIATION OF ACCEL AND GYRO FROM NULL NUMERICAL INTEGRATION OF ACCEL GINT_TEMP RMB 2 GINTN_TEMP RMB 2 AINT_TEMP RMB 2 AINTN_TEMP RMB 2 GYRO_TEMP RMB 2 GYRON_TEMP RMB 2 MODE RMB 1 VARIABLE TO TEST STATE OF STEERING PWM INPUT T_0 RMB 2 TIME OF RISING EDGE OF STEERING INPUT T_1 RMB 2 TIME OF FALLING EDGE OF STEERING INPUT P_TIME RMB 2 STORES PULSE LENGTH OF STEERING INPUT END CONFIGURE EEPROM VECTORS CONFIGURE OC5 ORG #OC5_VEC FDB $F000 END CONFIGURE OC3 ORG #OC3_VEC FDB $F100 END CONFIGURE IC3 ORG #IC3_VEC FDB $F200 END CONFIGURE RESET ORG #RST_VEC FDB $E000 END PROGRAM SPACE ORG PROG CONFIGURATION SETTINGS CONFIGURE A/D REGISTER LDS #STACK LOAD STACK INTO RAM LDAA #$90 ADPU = 1; DLY = 1 STAA OPTION LDAA #$30 SCAN = 1; MULT = 1 STAA ADCTL LDX #$FFFF AD_SET NOP DEX CPX #$00 BNE AD_SET DELAY BEFORE LOADING FIRST VALUE TO ALLOW ADC TO SETTLE CONFIGURE LED

4 LDAA #LED_ON STAA PACTL CONFIGURE MASKS AND FLAGS FOR OC5 AND OC3 LDAA #% CONFIGURES OC5/3 TO GO HIGH ON SUCCESSFUL COMAPRE STAA TCTL1 LDAA #% STAA TCTL2 LDAA #% STAA TFLG1 STAA TMSK1 LDD #$0000 STD TOC3 STD TOC5 SETS IC3 ON PA1 TO CAPTURE ON RISING EDGE CONFIGURE MASKS AND FLAGS FLAG OC5/3 MASK OC5/3 LOAD INITIAL COMPARE TO OC3 AND OC5 CONFIGURE DDRD LDAA #% DIRECTION FOR PWM CONTROLLER. PD3: DIR1, PD4: DIR2 STAA DDRD SET GAIN FOR ACCEL AND GYRO LDD #ACC_GAIN STD A_GAIN LDD #GYR_GAIN STD G_GAIN LDD #GINTGAIN STD GINT_GAIN MISC. SETUP STEPS LDD #AINT_NULL STD ACC_INT LDAA #$FF STAA PREVDTY1 STAA PREVDTY2 INITIALIZE ACCEL INTEGRAL INITIALIZE PREVIOUS DUTY CYCLES LDD #$3000 STD PWM1 LDAA #$00 STAA MODE IF MODE=00, NEXT IC WILL TRIGGER ON RISING CONFIGURE OUTSIDE OF MAIN LOOP LDD #STEER_NULL STD P_TIME CLI JMP RESET RESET NULL GYRO/ACCEL VALUES RESET LDAA #LED_ON STAA PORTA LDD #AINT_NULL STD ACC_INT CLRA LDAB ACCEL NO WEIGHT: 78, WEIGHT: 6E STD ACCEL_NULL CLRA LDAB GYRO STD GYRO_NULL LDAB GYRO_F STD GYRO_FNULL

5 LDAB ACCEL_F #$95 STD ANULL_CALC LDAA #LED_OFF STAA PORTA MAIN PROGRAM LOOP LOOP NOP LDAA PORTA BITA #SWITCH BNE RESET CHECK IF RESET PRESSED CALC DEVIATIONS FROM NULL LDAB ACCEL_NULL+1 CMPB ACCEL BMI SUBT_2 SUBB ACCEL STAB ABS_ACCEL BRA NEXT_ SUBT_2 LDAB ACCEL SUBB ACCEL_NULL+1 STAB ABS_ACCEL NEXT_ LDAB GYRO_NULL+1 CMPB GYRO BMI SUBT_1 SUBB GYRO STAB ABS_GYRO BRA DONE_ABS SUBT_1 LDAB GYRO SUBB GYRO_NULL+1 STAB ABS_GYRO DONE_ABS NOP LOAD CURRENT A/D VALUES CLRA LDAB ABS_GYRO CMPB #THRESH BMI SLOWVAL STAY_FAST CLRA LDAB ACCEL_F STD ACCEL_VAL LDAB GYRO STD GYRO_VAL LDD #GYR_GAIN STD G_GAIN LDD #GINTGAIN STD GINT_GAIN LDD #ACC_GAIN STD A_GAIN LDD GYRO_NULL STD GNULL_CALC

6 SLOWVAL CLRA BRA FASTVAL LDAB ACCEL_F STD ACCEL_VAL LDAB GYRO_F STD GYRO_VAL LDD #GYR_FGAIN STD G_GAIN LDD #GINTGAIN STD GINT_GAIN LDD #ACC_GAIN STD A_GAIN LDD GYRO_FNULL STD GNULL_CALC FASTVAL NOP INTEGRATE ACCEL, RESET DRIFT WHEN ALLOWED LDAA ABS_ACCEL ONLY INTEGRATE WHEN OUTSIDE OF THRESHOLD CMPA #RSET_ERROR BMI RAMP LDAA #LED_OFF STAA PORTA BRA INTEGRATE RAMP LDAA #LED_ON STAA PORTA LDD #AINT_NULL CPD ACC_INT BMI RAMPDOWN LDD ACC_INT ADDD #DELTA STD ACC_INT CPD #$2FE0 BMI MINMAX LDD #AINT_NULL STD ACC_INT BRA MINMAX THE RAMPING PROVIDES A WAY OF DAMPING THE INTEGRAL TERM RAMPDOWN LDD ACC_INT SUBD #DELTA STD ACC_INT CPD #$3020 BPL MINMAX LDD #AINT_NULL STD ACC_INT BRA MINMAX INTEGRATE CLRA LDAB ACCEL ADDD ACC_INT SUBD ACCEL_NULL STD ACC_INT MINMAX LDD ACC_INT CPD #INT_MAX BMI OKHIGH LDD #INT_MAX STD ACC_INT PERFORM INTEGRATION TEST FOR MIN/ MAX VALS

7 OKHIGH LDD ACC_INT CPD #INT_MIN BPL NO_INT LDD #INT_MIN STD ACC_INT NO_INT NOP END OF INTEGRATION COMPUTE PWM WITH (ACCEL-ACCEL_NULL)A_GAIN + (GYRO-GYRO_NULL)GYRO_GAIN LDD GYRO_VAL JSR INTFLOAT1 GYRO () GYRO_GAIN, STORE IN GYRO_TEMP LDD G_GAIN JSR FMUL JSR FLOATINT1 STD GYRO_TEMP LDD GNULL_CALC JSR INTFLOAT1 GYRO_NULL () GYRO_GAIN, STORE IN GYRON_TEMP LDD G_GAIN JSR FMUL JSR FLOATINT1 STD GYRON_TEMP LDD #AINT_NULL JSR INTFLOAT1 (ACCINTNULL-ACCINT)/ACCINTGAIN LDD #AINTGAIN JSR FDIV JSR FLOATINT1 STD AINTN_TEMP LDD ACC_INT JSR INTFLOAT1 LDD #AINTGAIN JSR FDIV JSR FLOATINT1 STD AINT_TEMP LDD ANULL_CALC JSR INTFLOAT1 (ACCEL-ACCEL_NULL)(A_GAIN) LDD ACCEL_VAL JSR FSUB LDD A_GAIN JSR FMUL LDD GYRO_TEMP NOW ADD GYRO AND INT TEMP TERMS

8 JSR FADD LDD GYRON_TEMP JSR FSUB LDD AINT_TEMP JSR FSUB LDD AINTN_TEMP JSR FADD JMP CONT59 SMOOTHING OF PWM SIGNAL TO PREVENT JITTERING LDD MANT1 STD MANTISSA LDD PWM1 STD PWM2 LDD #AINT_NULL JSR FADD JSR FLOATINT1 STD PWM STD PWM1 CPD PWM2 BEQ CONT57 BLS DECREASE LDD PWM1 SUBD PWM2 CPD #PWMTH BMI CONT58 BEQ CONT58 LDD PWM2 ADDD #PWMTH STD PWM1 BRA CONT57 DECREASE LDD PWM2 SUBD PWM1 CPD #PWMTH BMI CONT58 BEQ CONT58 LDD PWM2 SUBD #PWMTH STD PWM1 CONT57 LDD PWM1 JSR INTFLOAT1 LDD #$3000 JSR FSUB CONT58 LDD PWM JSR INTFLOAT1 LDD #$3000 JSR FSUB END SMOOTHING

9 MODIFICATION OF PWM FOR STEERING CONT59 NOP LDD MANT1 STD MANTISSA LDAA EXP1 STAA EXPONENT LEFT MOTOR LDD P_TIME JSR INTFLOAT1 LDD #STEER_NULL JSR FSUB LDD #TURN_G JSR FDIV LDD MANTISSA STD MANT2 LDD EXPONENT STD EXP2 JSR FADD LDD MANT1 STD MANT_L JSR FLOATINT1 STD PWM_L LDD P_TIME JSR INTFLOAT1 LDD #STEER_NULL RIGHT MOTOR JSR FSUB LDD #TURN_G JSR FDIV LDD MANTISSA STD MANT2 LDD EXPONENT STD EXP2 JSR FSUB LDD MANT1 STD MANT_R JSR FLOATINT1 STD PWM_R SET DIRECTION PINS BY CHECKING THE SIGN OF THE PWM CALCULATIONS ST_RDIR LDD MANT_L BITA #% BEQ POSI_L LDX #DIR BCLR $0,X #REV_L BRA UME

10 POSI_L NOP LDAA DIR ORAA #FORW_L STAA DIR UME LDD MANT_R BITA #% BEQ POSI_R LDX #DIR BCLR $0,X #REV_R BRA CONT867 POSI_R NOP LDAA DIR ORAA #FORW_R STAA DIR CONT867 NOP CHECK FOR FLOATING POINT OVERFLOW ABOVE FF LEFT MOTOR LDD PWM_L CMPA #$00 BEQ OVER_OK LDAB #DUTY_MAX STAB DUTY1 CHECK FOR OVERFLOW INTO ACC. A LOAD MAX DUTY IF OVERFLOW DETECTED OVER_OK STAB DUTY1 RIGHT MOTOR LDD PWM_R CMPA #$00 BEQ OVER_ROK LDAB #DUTY_MAX STAB DUTY2 CHECK FOR OVERFLOW INTO ACC. A LOAD MAX DUTY IF OVERFLOW DETECTED OVER_ROK STAB DUTY2 CHECK IF DUTY IS WITHIN DUTY_MIN AND DUTY_MAX LDAA DUTY1 CMPA #DUTY_MIN BLS D1_LESS CMPA #DUTY_MAX BHS D1_MORE BRA D1_OK D1_LESS LDAA #DUTY_MIN STAA DUTY1 BRA D1_OK D1_MORE LDAA #DUTY_MAX STAA DUTY1 D1_OK LDAA DUTY2 CMPA #DUTY_MIN BLS D2_LESS CMPA #DUTY_MAX BHS D2_MORE BRA D2_OK D2_LESS LDAA #DUTY_MIN STAA DUTY2 BRA D2_OK D2_MORE LDAA #DUTY_MAX

11 D2_OK NOP STAA DUTY2 CONVERT DUTY CYCLE TO HIGH/LOW TIME MOTOR 1 / OC5 LDAA DUTY1 LOAD DUTY CYCLE FROM 00(MIN) TO FF(MAX) LDB #$06 SCALING FACTOR FOR 1hZ CYCLE TIME MUL HIGH TIME IS IN D STD OC5_HIGH STORE HIGH TIME LDD #PERIOD PERIOD OF PWM SIGNAL SUBD OC5_HIGH STD OC5_LOW STORE LOW TIME MOTOR 2 / OC3 LDAA DUTY2 LOAD DUTY CYCLE FROM 00(MIN) TO FF(MAX) LDB #$06 MUL STD OC3_HIGH STORE HIGH TIME LDD #PERIOD SUBD OC3_HIGH STD OC3_LOW SCALING FACTOR FOR 1hZ CYCLE TIME HIGH TIME IS IN D STORE LOW TIME JMP LOOP RETURN TO TOP OF PROGRAM FLOATING POINT PACKAGE BELOW INTERUPT SERVICE ROUTINES AT BOTTOM OF CODE Floating Point Package 22/1/86 written by R.Soja, Motorola; modified for MC6801 by Tom Rogers All floating point routines use two, 3-byte operands located in RAM at OP1,OP2 Each is organised as: 7 bit exponent + 1 sign bit (2's complement) 15 bit mantissa + 1 sign bit (15 bit positive notation) Sign bit is always is always MSBit Routines implemented: 1. Addition MAD (FADD) OP1+OP2 2. Subtraction MSB (FSUB) OP1-OP2 3. Division MDV (FDIV) OP1/OP2 4. Multiplication MML (FMUL) OP1OP2 On exit from routine, OP1 contains result, OP2 is destroyed Two conversion routines are included: FLOATINT1 converts FP number in OP1 to unsigned integer in ACCD INTFLOAT1 converts unsigned integer in ACCD to FP number in OP1 An integer rounding subroutine (ROUND) is also included. INTFLOAT2 PUTS ACCD INTO OP2 FLOATINT2 PUTS OP2 INTO ACCD Note that NO ORG staements are included, as this package is intended to be appended as a subroutine package to a main calling program. RAM variables: FP routines: MAD EQU OP1 + OP2 => OP1

12 FADD EQU BSR ALIGN BVS FADDEX LDAA MANT1 ANDA #$80 JSR XGDX LDAA MANT1 EORA MANT2 BMI SUBMANT BSR GETABS ADDD MANT2 BPL FADD1 INC EXP1 BVC FADD2 DEC EXP1 LDD #$7FFF BRA FADD1 FADD2 LSRD FADD1 STX MANT2 STD MANT1 JSR NORM FADDEX SUBMANT BSR GETABS SUBD MANT2 BSR CONVFP BRA FADD1 GETABS LDAA MANT2 ANDA #$7F STAA MANT2 LDD MANT1 ANDA #$7F If exponent difference too great, return. Put sign bit of mantissa 1 in X reg. If signs are same then add positive parts of mantissas. if MSBit of result set, then result has overflowed, so increment exponent, while limiting value to upper bound. Save sign bit and result, prior to normalising it. Return to calling program segment. If signs are different, then subtract positive parts of mantissas. Change 2s compl result to floating point format and store result (X contains corrected sign bit) Clear sign bits in MANT2 and MANT1 in ACCD MSB EQU OP1 - OP2 => OP1 FSUB EQU LDAA MANT2 ADDA #$80 Negate sign of mantissa 2 STAA MANT2 BRA FADD ALIGN CLRA LDAB EXP1 SUBB EXP2 BPL POS BVS ALIGNEX NEGB JSR XGDX LDD MANT1 BSR ALIGN1 STD MANT1 LDAA EXP2 and perform addition If EXP1<EXP2 then If exponent difference within range, get absolute value of difference into X. This will be the # of bits to shift. Update result exponent STAA EXP1 POS BVC POS1 If exp diff too great then LDD MANT2 STD MANT1 LDAA EXP2 STAA EXP1 SEV copy OP2 to OP1 restore flag for calling routine and return. POS1 JSR XGDX Move number of bits to shift into X reg CPX #0 If no shift required BEQ ALIGNEX then return LDD MANT2 BSR ALIGN1 else align mantissa 2 with mantissa 1 STD MANT2 ALIGNEX ALIGN1 BMI ALIGN3 ALIGN2 LSRD (Result exponent stays the same). If sign bit is clear then align ACCD

13 DEX BNE ALIGN2 ALIGN3 ANDA #$7F BSR ALIGN2 ORAA #$80 CONVFP EQU JSR XGDX STX MANT2 ADDA MANT2 ANDA #$80 JSR XGDX TSTA BPL CONVFPEX COMA NEGB BCS CONVFPEX else remove sign bit before aligning. Restore sign bit and return Put result in X, sign bit in ACCD Store 2s compl result. Correct sign bit and mask it. Restore result to ACCD, corrected sign to X Update CC reg Only convert negative numbers 1s complement hi byte Convert lo byte to 2s complement. INCA Convert hi byte to 2s complement if ACCB=0 CONVFPEX NORM EQU!ACCD = unsigned value to be normalised.!mant2= sign bit of value. BEQ NORMEX Zero cannot be normalised! NORM1 DEC EXP1 BVS NORM2 Terminate on underflow (i.e. EXP1 < $80) LSLD BPL NORM1 LSRD NORM2 INC EXP1 NORMEX ORAA MANT2 NORM3 CMPA #$80 BNE NORMEX1 CMPB #$00 BNE NORMEX1 CLRA NORMEX1 STD MANT1 Wait until MSB set Clear sign bit and adjust exponent Update sign bit and remove sign from -0 mantissa. Store normalised result. MDV EQU OP1 / OP2 => OP1 FDIV EQU LDD MANT2 BEQ MAXRES Trap divide by 0 LDAA MANT1 EORA MANT2 BMI FDIV1 CLR Y CLR Y+1 BRA FDIV2 FDIV1 LDX #$8000 STX Y FDIV2 LDAB EXP1 SUBB EXP2 BVC FDIV3 If signs are same then result sign is positive ( Y has result sign.) else result sign is negative. If V bit set then its an under/overflow, so STAA MANT1! update result sign BCC MINRES! If C bit clear then force result to min limit BRA MAXRES! else force result to max limit, retaining sign FDIV3 STAB EXP1 Save result exponent LDX #16 Initialize shift counter JSR GETABS +ive part of MANT1 in D, +ive part of MANT2 in it COMPARE SUBD MANT2 Dividend minus divisor BCS RESTORE Borrow so need to restore dividend SEC else no borrow so set carry BRA SHIFT and go shift into quotient. RESTORE ADDD MANT2 Add divisor back to dividend CLC prep to shift a 0 SHIFT ROL QUOT+1 Shift 0 or 1 into quotient ROL QUOT LSLD Shift dividend left DEX All division done?

14 BNE COMPARE not yet LDD QUOT else done so get quotient BPL FDIV4 If quot sign bit already clear, wrap up LSRD else open up sign bit INC EXP1 adjusting exponent. FDIV4 ADDD Y Update sign bit BRA NORM3 Ck. for -0, store result and return MAXRES STAA TEMP1 Store A. LDAA MANT1 Maximise MSbyte of mantissa, retaining sign. ORAA #$7F STAA MANT1 LDAA TEMP1 Restore A. LDD #$FF7F Maximise LSbyte of mantissa, and exponent. STD MANT1+1 MINRES CLRA Result = 0 CLRB STD MANT1 CLR EXP1 MML EQU OP1 OP2 => OP1 FMUL EQU LDAB EXP1 First, add exponents. ADDB EXP2 BVC FMUL1 If V bit is set then its an under/overflow, so STAA MANT1! update result sign BCS MINRES! If C bit set then force result to min limit BRA MAXRES! else force result to max limit, retaining sign FMUL1 STAB EXP1 Store result exponent. LDAA MANT1 Evaluate result sign, and EORA MANT2 put it in X reg JSR XGDX STAA TEMP1 Store A. LDAA MANT1 Make both operands positive ANDA #$7F STAA MANT1 LDAA MANT2 ANDA #$7F STAA MANT2 LDAA TEMP1 Restore A. BSR CONVFPI Convert MANT1,MANT2 to unsigned integer format!and return with result sign + MANT1 in Y reg LDAA MANT1 multiply MSbytes of mantissas. LDAB MANT2 MUL JSR XGDX Save 1st partial result in X LDAA MANT1+1 Cross multiply. LDAB MANT2 MUL ADCA #0 Round up and TAB CLRA restore weighting of partial result (in ACCD) STX MANT1 Add 1st and 2nd partial results ADDD MANT1 JSR XGDX Store updated partial result. STD TEMP1 Store D. LDD Y Restore MANT1 STD MANT1 LDD TEMP1 Restore D. LDAA MANT1 Cross multiply again. LDAB MANT2+1 MUL ADCA #0 Round up again. TAB CLRA Correctly weighted 3rd partial result now in ACCD STX MANT1 so add it to stored partial result. ADDD MANT1 BSR CONVIFP Convert integer in ACCD to FP format,and store.

15 Multiplication of LS bytes is unnecessary, as the result will always overflow if both non-zero. Return to calling program. CONVFPI EQU Enter with +ive MANT1,MANT2: result sign in X reg LSL MANT1+1 De-normalise both operands. ROL MANT1 LSL MANT2+1 ROL MANT2 LDD MANT1 CPX #0 Add result sign bit to ACCD BPL FPI1 ORAB #1 FPI1 JSR XGDY and save result sign+operand 1 in Y. CONVIFP EQU LDX #0 STX MANT2 STD MANT1 Temporarily store result. JSR XGDY Move result sign bit LSRD ROR MANT2 to MANT2 before LDD MANT1 LSRD JSR NORM normalising result mantissa. IFP2 Return with result stored in MANT1. FLOATINT1 EQU CLRB LDAA EXP1 If exponent <= 0 BGT FLTINT1 then clear ACCD and return. CLRA FLTINT1 CLRA else LSL MANT1+1 adjust mantissa, destroying sign bit ROL MANT1 FLTINT2 LSL MANT1+1 then move mantissa into ACCD ROL MANT1 ROLB ROLA DEC EXP1 BNE FLTINT2 until exponent = 0 ROUND TST MANT1 Call IMMEDIATELY after FLOATINT if to be used. BPL ROUNDEX MANT1 has "leftover precision." ASL MANT1 If MSB of MANT1 is set, fraction >=.5 BPL ROUNDEX If next MSB set, fraction >=.75 ADDD #1 so increment D & return ROUNDEX else just return. INTFLOAT1 EQU On entry, unsigned integer in ACCD LDX #0 Initialise result mantissa STX MANT1 CLR EXP1 and exponent. INTFLT1 LSRD Move integer to F.P. mantissa ROR MANT1 ROR MANT1+1 INC EXP1 adjusting exponent with every shift. CMPA #0 BNE INTFLT1 CMPB #0 BNE INTFLT1 When no more bits in ACCD ROR MANT1 clear sign bit as integer was always >=0 ROR MANT1+1 XGDX STD TEMP1 Store D STX TEMP2 and X. LDD TEMP2 Load D with X

16 LDX TEMP1 and X with D. XGDXEX XGDY STD TEMP1 D in TEMP1 LDD Y Move Y STD TEMP2 to TEMP2. LDD TEMP1 Move D/TEMP1 STD Y to Y. LDD TEMP2 Put Y into D. XGDYEX INTFLOAT2 EQU On entry, unsigned integer in ACCD LDX #0 Initialise result mantissa STX MANT2 CLR EXP2 and exponent. INTFLT2 LSRD ROR MANT2 ROR MANT2+1 INC EXP2 CMPA #0 BNE INTFLT2 CMPB #0 FLOATINT2 EQU CLRB Move integer to F.P. mantissa adjusting exponent with every shift. BNE INTFLT2 When no more bits in ACCD ROR MANT2 clear sign bit as integer was always >=0 ROR MANT2+1 LDAA EXP2 If exponent <= 0 BGT FLTINT12 then clear ACCD and return. CLRA FLTINT12 CLRA else LSL MANT2+1 ROL MANT2 FLTINT22 LSL MANT2+1 adjust mantissa, destroying sign bit then move mantissa into ACCD ROL MANT2 ROLB ROLA DEC EXP2 BNE FLTINT22 until exponent = 0 END END OF FLOATING POINT ROUTINES BELOW ARE THE INTERRUPT SERVICE ROUTINES OC5 INTERRUPT SERVICE SUBROUTINE FOR MOTOR 1 ORG OC5_ISR OC5 INTERRUPT SERVICE SUBROUTINE LDAA #% BITA TCTL1 BEQ LOW LDD TCNT ADDD OC5_HIGH STD TOC5 BRA HIGH TEST FOR CURRENT STATE OF PWM (HIGH OR LOW) LOAD CURRENT VAL OF COMPARE, ADD HIGH TIME LOW LDD TCNT LOAD CURRENT VAL OF COMPARE, ADD LOW TIME ADDD OC5_LOW STD TOC5

17 HIGH LDAA TCTL1 EORA #% STAA TCTL1 LDX #TFLG1 BCLR $0,X #% INVERT HIGH/LOW BIT OF OC5 RESET OC5 FLAG RTI END OC3 INTERRUPT SERVICE SUBROUTINE FOR MOTOR 2 ORG OC3_ISR LDAA #% BITA TCTL1 BEQ LOW2 LDD TCNT ADDD OC3_HIGH STD TOC3 BRA HIGH2 LOW2 LDD TCNT ADDD OC3_LOW STD TOC3 HIGH2 LDAA TCTL1 EORA #% STAA TCTL1 LDX #TFLG1 BCLR $0,X #% TEST FOR CURRENT STATE (HIGH OR LOW LOAD CURRENT VAL OF COMPARE, ADD HIGH TIME LOAD CURRENT VAL OF COMPARE, ADD LOW TIME INVERT HIGH/LOW BIT OF OC3 RESET FLAG RTI END IC3 SERVICE ROUTINE. USED TO DECODE PULSE WIDTH FROM RC RECEIVER ORG IC3_ISR LDAA MODE CMPA #$00 BEQ R_EDGE LDD TIC3 SUBD T_0 BPL DONE LDD #$FFFF SUBD T_0 ADDD TIC3 DONE CPD #P_TIMEMIN BPL MAXSTR LDD #STEER_NULL BRA SET MAXSTR CPD #P_TIMEMAX BMI SET LDD #STEER_NULL CHECK STATE OF INPUT SIGNAL BRANCH IF RISING EDGE DO THIS IF FALLING EDGE IF P_TIME < P_TIMEMIN, THEN DONOT CHANGE P_TIME CHECK IF STEERING IS WITHING ALLOWED RANGE SET STD P_TIME LDAA MODE DECA STAA MODE LDAA #% STAA TCTL2 NEXT TIME ISR IS CALLED THE PULSE WILL BE A RISING EDGE. SETS IC3 ON PA1 TO CAPTURE ON RISING EDGE

18 LDX #TFLG1 BCLR $0,X #% CLEAR IC3 FLAG RTI R_EDGE LDD TIC3 STD T_0 LDAA MODE INCA STAA MODE LDAA #% STAA TCTL2 STORE TIME OF INPUT CAPTURE EVENT NEXT TIME ISR IS CALLED THE PULSE WILL BE A FALLING EDGE SETS IC3 ON PA2 TO CAPTURE ON FALLING EDGE LDX #TFLG1 BCLR $0,X #% CLEAR IC3 FLAG RTI END