SCRUB COLD HEX \ 13 SEPT 2004 \ Tilt Sensor \ Programed for ISOPOD V2 Isomax v0.61 \ Memsic MXD2020EL PWM 1g Dual Axis Accelerometer \ updates at 100 hz or every 10 ms \ \ Inputs to ISOPOD V2 \ X-Out -> TB0 \ Y-Out -> TB1 \ Details of PWM \ Period of PWM is ~10ms, freq is 100 Hz 97-103 hz (9.7ms - 10.03 ms) \ 50% duty cycle = ~5ms = 0g \ prescaler IPBus divide by 8 for 26.2144 ms = FFFF at HALFSPEEDCPU \ 10ms = 25,000 counts = period of PWM \ 1g = 20% of scale or 5,000 counts \ +1g = 20,000 counts \ 0g = 25,000 counts \ -1g = 30,000 counts \ \ A = acceleration \ Ax = acceleration on X axis \ Ay = acceleration on Y axis \ T1 = time measurement of high side of PWM pulse \ T2 = time measurement of period of PWM pulse \ T2 is obtained by adding high side and low side \ A = ((T1/T2) - 0.5)/0.20 equation from datasheet \ basically at 50% duty cycle, A = 0 \ at 20% of duty cycle, A = 1g or -1g \ tilt angle is calculated by \ angle = ATAN(Ax/Ay) for +90 deg to -90 deg \ to get full 360 deg measurement, you need to do quadrent correction \ on the ATAN function - see Memsic App Note AN-00mx-007.pdf page 3 \ \ No temerature correction being done here. If needed, you can see \ Memsic App Note AN-00mx-002.pdf \ \ Timer B0 \ This timer takes the Xout signal and measures the high side of the pulse \ Timer B1 \ This timer takes the Yout signal and measures the high side of the pulse \ Timer B2 \ This timer takes the Xout signal and measures the low side of the pulse \ Timer B3 \ This timer takes the Yout signal and measures the low side of the pulse \ -- CTRL Register -- \ 011 Gated Count Mode \ 1011 - prescaler IPBus divide by 8 for 26.2144 ms = FFFF at HALFSPEEDCPU \ 00 - Counter #0 Input Pin - for Xout \ 01 - Counter #1 Input Pin - for Yout \ 0 - count repeatedly \ 0 - Roll over \ 0 - Count up \ 0 - no co-channel \ 000 - output mode \ 0111011000000000 = 7600 - TB0.CTRL 0D26 \ 0111011010000000 = 7680 - TB1.CTRL 0D2E \ 0111011000000000 = 7600 - TB2.CTRL 0D36 \ 0111011010000000 = 7680 - TB3.CTRL 0D3E \ -- SCR Register -- \ 0 - Timer compare flag \ 0 - TCFIE \ 0 - TOF \ 0 - TOFIE \ 0 - Input edge flag - may need to clear this \ 0 - IEFIE \ 0 - IPS - count when input pin is high \ 1 - IPS - invert input - count when input pin is low \ 0 - INPUT \ 10 - Capture Mode - Load on falling edge with IPS=0 \ 10 - Capture Mode - Load on rising edge with IPS=1 \ 0 - MSTR \ 0 - EEOF \ 0 - VAL \ 0 - FORCE \ 0 - OPS \ 0 - OEN \ 0000000010000000 = 0080 - TB0.SCR 0D27 \ 0000000010000000 = 0080 - TB1.SCR 0D2F \ 0000001010000000 = 0280 - TB2.SCR 0D37 \ 0000001010000000 = 0280 - TB3.SCR 0D3F 0D22 CONSTANT TB0.CAP EEWORD 0D2A CONSTANT TB1.CAP EEWORD 0D32 CONSTANT TB2.CAP EEWORD 0D3A CONSTANT TB3.CAP EEWORD 0D26 CONSTANT TB0.CTRL EEWORD 0D2E CONSTANT TB1.CTRL EEWORD 0D36 CONSTANT TB2.CTRL EEWORD 0D3E CONSTANT TB3.CTRL EEWORD 0D25 CONSTANT TB0.CNTR EEWORD 0D2D CONSTANT TB1.CNTR EEWORD 0D35 CONSTANT TB2.CNTR EEWORD 0D3D CONSTANT TB3.CNTR EEWORD 0D27 CONSTANT TB0.SCR EEWORD 0D2F CONSTANT TB1.SCR EEWORD 0D37 CONSTANT TB2.SCR EEWORD 0D3F CONSTANT TB3.SCR EEWORD DECIMAL PI F2* FCONSTANT 2PI EEWORD 360.0E0 2PI F/ FCONSTANT DR EEWORD 1 S>F 2 S>F F/ FCONSTANT HALF EEWORD 1 S>F 5 S>F F/ FCONSTANT 20PERCENT EEWORD 10 S>F FCONSTANT FTEN EEWORD 10 CONSTANT TEN EEWORD HEX \ Initialize Timer B Modlue to do the gated counting. : INIT-TIMERB \ Init CTRL Register 7600 TB0.CTRL ! 7680 TB1.CTRL ! 7600 TB2.CTRL ! 7680 TB3.CTRL ! \ Init SCR Register 0080 TB0.SCR ! 0080 TB1.SCR ! 0280 TB2.SCR ! 0280 TB3.SCR ! \ Init CAP - Zero Capture registers 0000 TB0.CNTR ! 0000 TB1.CNTR ! 0000 TB2.CNTR ! 0000 TB3.CNTR ! ; EEWORD \ ------------ TILTX MACINE ------------------- MACHINE TILTX EEWORD ON-MACHINE TILTX APPEND-STATE X-AWAIT-LOW EEWORD APPEND-STATE X-AWAIT-HIGH EEWORD \ ---------------- X-AWAIT-LOW ----------- IN-STATE X-AWAIT-LOW CONDITION \ Wait for INPUT pin to go low \ by checking SCR INPUT bit 8 TB0.SCR @ 0100 AND 0CAUSES YELLED TOGGLE 7600 TB0.CTRL ! 0000 TB0.CNTR ! \ Zero counter for next time 0080 TB0.SCR ! THEN-STATE X-AWAIT-HIGH NEXT-TIME IN-EE \ ---------------- X-AWAIT-HIGH ----------- IN-STATE X-AWAIT-HIGH CONDITION \ Wait for INPUT pin to go high \ by checking SCR INPUT bit 8 TB0.SCR @ 0100 AND 0= NOT CAUSES YELLED TOGGLE 7600 TB2.CTRL ! 0000 TB2.CNTR ! \ Zero counter for next time 0280 TB2.SCR ! THEN-STATE X-AWAIT-LOW NEXT-TIME IN-EE \ ------------ TILTY MACINE ------------------- MACHINE TILTY EEWORD ON-MACHINE TILTY APPEND-STATE Y-AWAIT-LOW EEWORD APPEND-STATE Y-AWAIT-HIGH EEWORD \ ---------------- Y-AWAIT-LOW ----------- IN-STATE Y-AWAIT-LOW CONDITION \ Wait for INPUT pin to go low \ by checking SCR INPUT bit 8 TB1.SCR @ 0100 AND 0= CAUSES REDLED TOGGLE 7680 TB1.CTRL ! 0000 TB1.CNTR ! \ Zero Counter for next time 0080 TB1.SCR ! THEN-STATE Y-AWAIT-HIGH NEXT-TIME IN-EE \ ---------------- Y-AWAIT-HIGH ----------- IN-STATE Y-AWAIT-HIGH CONDITION \ Wait for INPUT pin to go high \ by checking SCR INPUT bit 8 TB1.SCR @ 0100 AND 0= NOT CAUSES REDLED TOGGLE 7680 TB3.CTRL ! 0000 TB3.CNTR ! \ Zero counter for next time 0280 TB3.SCR ! THEN-STATE Y-AWAIT-LOW NEXT-TIME IN-EE \ -------- Machine chain ----------- MACHINE-CHAIN ALL-STATES TILTX TILTY END-MACHINE-CHAIN EEWORD DECIMAL : RUN-DUAL-TILT HALFSPEEDCPU INIT-TIMERB X-AWAIT-LOW SET-STATE Y-AWAIT-LOW SET-STATE EVERY 2000 CYCLES SCHEDULE-RUNS ALL-STATES \ Need to run fairly fast to ensure you don't miss any captures ; EEWORD HEX : R>D DR F* ; EEWORD \ RADIANS TO DEGREES : GET-FANGLE ( -- f ) \ returns floating point angle from -90 deg to + 90 deg \ needs modification to get full 360 measurement TB1.CAP @ S>F TB1.CAP @ TB3.CAP @ + S>F F/ \ T1 divided by T2 HALF F- 20PERCENT F/ \ subtract 0.5 and divide by .2 to get Ay TB0.CAP @ S>F TB0.CAP @ TB2.CAP @ + S>F F/ \ T1 divided by T2 HALF F- 20PERCENT F/ \ subtract 0.5 and divide by .2 to get Ax F/ \ DIVIDE Ax/Ay FATAN \ ATAN function to get angle in radians R>D \ Convert to degrees ; EEWORD \ Get the tilt angle as an integer : GET-ANGLE ( -- n ) GET-FANGLE F>D DROP \ Convert float to Double, drop to convert to single ; EEWORD \ to use, start machine by executing RUN-DUAL-TILT and whenever you need to \ you can use GET-FANGLE to get the tilt angle as a floating point number. \ or use GET-ANGLE to get an integer value instead