; ; Bret Victor: bret@ugcs.caltech.edu ; GRASP lab: University of Pennsylvania ; August, 2000 ; ; DAX rotation sensor controller ; Source code for SX18AC microcontroller ; version 1.0 ; ; Port map: Port A: bit 3: output: ADC /CS ; bit 2: output: LED 2 ; bit 1: output: LED 1 ; bit 0: output: LED 0 ; Port B: bit 7: in/out: ADC data ; bit 6: output: ADC clock ; bit 5: output: RS232 xmit ; bit 4: input: RS232 receive ; bit 3: output: integrator reset 2 ; bit 2: output: integrator reset 1 ; bit 1: output: PWM 2 ; bit 0: output: PWM 1 ; ; Comment out the following line to assemble with SASM instead of SX-Key SX_KEY ;;;;;;;;;;;;;;;;; ;; ;; device options ;; IFDEF SX_KEY device sx18l,turbo,stackx_optionx freq 22_118_000 ELSE device pins18,banks8,oschs3,turbo,optionx ENDIF id 'DAX v1.0' reset ResetEntry ;;;;;;;;;;;;;;; ;; ;; port mapping ;; adc_CS equ ra.3 ; output: ADC chip select (active low) led_2 equ ra.2 ; output: pretty light #2 (active low) led_1 equ ra.1 ; output: pretty light #1 (active low) led_0 equ ra.0 ; output: pretty light #0 (active low) adc_Data equ rb.7 ; in/out: ADC serial data line adc_Clk equ rb.6 ; output: ADC serial clock line serial_Tx equ rb.5 ; output: RS232 transmit line serial_Rx equ rb.4 ; output: RS232 receive line reset_1 equ rb.3 ; output: reset integrator #2 reset_2 equ rb.2 ; output: reset integrator #1 pwm_2 equ rb.1 ; output: PWM #2 pwm_1 equ rb.0 ; output: PWM #1 adc_Clk_mask equ $40 reset_1_mask equ $08 reset_2_mask equ $04 pwm_2_mask equ $02 pwm_1_mask equ $01 PortA_ResetValue equ %1111 PortAdir_ResetValue equ %0000 PortB_ResetValue equ %00000000 PortBdir_ResetValue equ %10010000 PortBdir_DataOutValue equ %00010000 PortBdir_DataInValue equ %10010000 ;;;;;;;;;;;;;;;;; ;; ;; memory mapping ;; InterruptPage equ $0 ; interrupt handler, reset entry, and event loop StringPage equ $1 ; strings SerialPage equ $2 ; serial and UI routines UiPage equ $3 ; UI and decimal conversion routines AdcPwmPage equ $4 ; delay, ADC, and PWM routines, gyro vectors GyroPage equ $5 ; gyro routines AnalysisPage equ $6 ; currently unused AnalysisPage2 equ $7 ; currently unused ;;;;;;;;;; ;; ;; equates ;; VersionMajor equ $01 ; major software version VersionMinor equ $00 ; minor software version InterruptRate equ 144 ; 22.118 Mhz clock with 1/2 prescalar: ; interrupt every 13 us, or 8 x 9600 Khz SerialDivider equ 8 ; every 8 interrupts, send a bit SerialDivider2 equ 12 ; 1.5 x SerialDivider PwmPhaseLag equ 9 ; time between rising edges in staggered mode PwmMinValue equ 9 ; smallest possible PWM value PwmMinModeAB equ 18 ; smallest value that modes A and B can handle PwmMaxTrim equ 8 ; max trim value (equal to trim off), must be 8 EventLoopDelay equ 3 ; time between samples, divided by 3.3 ms GyroDriftTime equ 5 ; minimum number of samples to be considered a drift GyroCourseTimeout equ 127 ; max time to wait for drift in CalibrateCourse GyroFineTimeout equ 127 ; max time to wait for drift in CalibrateFine GyroLowThreshold equ 10 ; value under which the integrator recenters GyroHighThreshold equ 240 ; value over which the integrator recenters GyroFineIterations equ 13 ; max number of times to do CalibrateFine loop UiDefaultAutoRead equ 64 ; default autoread delay UiAck equ $80 ; code for generic OK in non-verbose mode UiAck1 equ $81 ; code for gyro 1 OK in non-verbose mode UiAck2 equ $82 ; code for gyro 2 OK in non-verbose mode UiNak equ $f0 ; code for unrecognized command in non-verbose mode UiChannel1 equ $01 ; code for gyro 1 reading in non-verbose mode UiChannel2 equ $02 ; code for gyro 2 reading in non-verbose mode ;;;;;;;;;;;; ;; ;; variables ;; ; globals org $8 stashi ds 1 ; stash variable for interrupt routine stash1 ds 1 ; stash variable for main code (higher level) stash2 ds 1 ; stash variable for main code (subroutines) statusbits ds 1 ; global status bits mastercounter ds 1 ; increments every 13 us eventloop_timer ds 1 ; decrements every 3.3 ms pwm_pointer ds 1 ; pointer to the current pwm phase info record ; bank 1 org $10 BankSerial = $ serial_rxgetptr ds 1 ; offset in buffer to read the next byte serial_rxputptr ds 1 ; offset in buffer to put the next byte serial_rxlength ds 1 ; how many bytes are in the buffer serial_txgetptr ds 1 ; offset in buffer to read the next byte serial_txputptr ds 1 ; offset in buffer to put the next byte serial_txlength ds 1 ; how many bytes are in the buffer serial_txbyte ds 1 ; the byte that is being transmitted serial_txcount ds 1 ; how many bits we have left to transmit serial_txdivide ds 1 ; how many more interrupt periods to wait serial_rxbyte ds 1 ; the byte that is being received serial_rxcount ds 1 ; how many bits we have left to receive serial_rxdivide ds 1 ; how many more interrupt periods to wait serial_stash ds 1 ; stash register for calculations serial_stash2 ds 1 ; stash register for calculations ; bank 3 org $30 BankRxBuf = $ ; bank 5 org $50 BankTxBuf = $ ; bank 7 org $70 BankDecimal = $ BankUI = $ decimal_numhi ds 1 ; hibyte in the division decimal_numlo ds 1 ; lobyte in the division decimal_stash ds 1 ; stash byte decimal_subresult ds 1 ; temporary result in the division ui_autoreadcounter ds 1 ; loop countdown for autoread delay ui_autoreaddelay ds 1 ; delay value for autoread ui_stash ds 1 ; just a stash variable ; bank 9 org $90 BankGyro1 = $ gyro_statusbits ds 1 ; gyro-specific status bits gyro_resetmask ds 1 ; either reset_1_mask or reset_2_mask gyro_sample ds 1 ; latest raw input sample gyro_outputhi ds 1 ; hibyte of latest processed output value gyro_outputlo ds 1 ; lobyte of latest processed output value gyro_positionhi ds 1 ; hibyte of position offset gyro_positionlo ds 1 ; lobyte of position offset gyro_calmask ds 1 ; bitmask in calibrateCourse gyro_pwmvalue ds 1 ; PWM_SetValue in calibrateCourse gyro_calcounter ; counts down calibrateFine iterations gyro_drifttimer ds 1 ; decs on each sample but floors at $01 gyro_oldaverage ds 1 ; used in drift correction gyro_driftcounter ds 1 ; used in drift correction gyro_history1 ds 1 ; previous three samples, for averaging gyro_history2 ds 1 ; in drift correction. gyro_history3 ds 1 gyro_average ds 1 ; bank B org $b0 ; this simply mirrors bank 9, so all gyro BankGyro2 = $ ; variables can be referred to with the ; same name, in either bank. ; bank D org $d0 BankAnalysis = $ ; put your analysis variables here! ; bank F org $f0 BankPWM = $ pwm_startptr ds 1 ; ptr to first phase info record (mask1 or mask2) pwm_trimptr ds 1 ; ptr to trim routine for the current PWM mode pwm_negvalue1 ds 1 ; PWM1 value, negated pwm_trim1 ds 1 ; counter for trimming PWM1 pwm_negvalue2 ds 1 ; PWM2 value, negated pwm_trim2 ds 1 ; counter for trimming PWM2 pwm_trimcounter ds 1 ; counter for doing the trims org $f8 pwm_mask1 ds 1 ; variables to store bit flipping masks pwm_period1 ds 1 ; and period lengths for each phase of the pwm_mask2 ds 1 ; dual PWM output. These must be from pwm_period2 ds 1 ; $f8 to $ff in RAM, so I can pull some pwm_mask3 ds 1 ; clever tricks in the interrupt routine. pwm_period3 ds 1 pwm_mask4 ds 1 pwm_period4 ds 1 ;;;;;;;;;;;;;; ;; ;; status bits ;; ; global status bits serialTxActive equ statusbits.0 ; true if a byte is being transmitted serialRxActive equ statusbits.1 ; true if a byte is being received serialRxAvail equ statusbits.2 ; true if the receive buf is not empty gyroWhichGyro equ statusbits.3 ; 0 = gyro1 is active, 1 = gyro2 is active uiAutoReadOn equ statusbits.4 ; true if autoread is turned on for either channel uiVerbose equ statusbits.5 ; true if verbose mode is on uiDebug equ statusbits.6 ; true if debug mode is on statusbits_resetvalue equ %0000000 ; gyro_statusbits status bits gyrostatusPwmHigh equ gyro_statusbits.0 ; true if the pwm value is above halfway gyrostatusIntegrate equ gyro_statusbits.1 ; true if reading integrator, false if amplifier gyrostatusDriftUp equ gyro_statusbits.2 ; true if drifting up, false if drifting down gyrostatusGotJitter equ gyro_statusbits.3 ; true if there was movement opposite the drift gyrostatusAutoRead equ gyro_statusbits.4 ; true if autoread is on for this channel gyrostatus_resetvalue equ %00000 ;;;;;;;;; ;; ;; macros ;; ;; ;; comparison macros ;; ; _sweq, _swne, _swgt, _swlte myreg (modify w) ; skip next instruction if w is equal to, not equal to, greater than, ; or less than or equal to the specified register, respectively. _sweq MACRO 1 xor w, \1 ; see if they are equal sz ; if so, skip next instruction ENDM _swne MACRO 1 xor w, \1 ; see if they are equal snz ; if not, skip next instruction ENDM _swgt MACRO 1 mov w, \1-w ; w = reg - w snc ; c = 0 if w > reg ENDM _swlte MACRO 1 mov w, \1-w ; w = reg - w sc ; c = 1 if w <= reg ENDM ; _srgtel, _srltl, _srgtl, _srltel (modify w) ; skip next instruction if register is >=, <, >, <= the specified literal. _srgtel MACRO 2 mov w, #\2 _swlte \1 ENDM _srltl MACRO 2 mov w, #\2 _swgt \1 ENDM _srgtl MACRO 2 mov w, #(\2)-1 _swlte \1 ENDM _srltel MACRO 2 mov w, #(\2)-1 _swgt \1 ENDM ;; ;; serial port macros ;; ; _incptr mybufptr (modifies w) ; increments mybufptr, wrapping to 0 if greater than 15. _incptr MACRO 1 mov w, ++\1 ; increment buffer pointer, and w, #$0f ; wrapping appropriately mov \1, w ENDM ; _getbuf mybufptr, BankBuffer (modifies w, fsr) ; gets a byte from the specified buffer and leaves it in w. ; The bank is left as BankSerial. _getbuf MACRO 2 mov w, #\2 ; get the bank add w, \1 ; add the "get pointer" mov fsr, w ; get ready for indirect addressing mov w, indf ; get our byte from the buffer bank BankSerial ; switch back to our bank ENDM ; _putbuf mybufptr, BankBuffer, globalvar (modifies w, fsr) ; puts the byte in globalvar into the specified buffer. globalvar must ; be a global variable (duh). The bank is left as BankSerial. _putbuf MACRO 3 mov w, #\2 ; get the bank add w, \1 ; add in the "put pointer" mov fsr, w ; get ready for indirect addressing mov w, \3 ; get the received byte mov indf, w ; put it in its place bank BankSerial ; switch back to our bank ENDM ;; ;; pwm macros ;; ; _pwm_waitforsync (modifies w) ; waits until a slow-phase interrupt has just finished and the trim counter ; has just been reset. At this time, another interrupt won't occur for at ; least a little while and the PWM periods are set to their non-trimmed ; values, so it is temporarily safe to modify the PWM variables. Bank must ; be set to BankPWM. _pwm_waitforsync MACRO :wait1 movsz w, --pwm_trimcounter ; is the trim counter 1? jmp :wait1 ; if not, wait for it :wait8 sb pwm_trimcounter.3 ; is the trim counter 8? jmp :wait8 ; if not, wait for it ENDM ;; ;; gyro macros ;; ; _gyro_restorebank (modifies fsr) ; switches the bank to either BankGyro1 or BankGyro2, ; depending on the gyroWhichGyro bit. _gyro_restorebank MACRO bank BankGyro1 ; switch back to the right gyro bank snb gyroWhichGyro ; are we doing gyro1 or gyro2? setb fsr.5 ; if gyro2, set the appropriate bank bit ENDM ; _gyro_reseton, _gyro_resetoff (modify w) ; turn on or off the appropriate integrator reset switch. Bank must be ; set to BankGyroN. _gyro_reseton MACRO mov w, gyro_resetmask or rb, w ENDM _gyro_resetoff MACRO mov w, /gyro_resetmask and rb, w ENDM ;;;;;;;;;;;;;;;; ;; ;; ;; Page 0 ;; ;; ;; ;;;;;;;;;;;;;;;; org $0 ;;;;;;;;;;;;;;;;; ;; ;; Interrupt code ;; ; The interrupt routine does three things. It generates two independent ; high-frequency PWM waveforms, increments two 3-byte counters, and ; implements a UART. The nature of the PWM generator imposes some very ; strict timing requirements, which this interrupt routine meets, but just ; barely. If a few instructions were to be added here, the software could ; fail in certain cases. ; ; Dual PWM generation is a tricky business. We want the frequency of the ; waveforms to be 76.8 KHz, or the UART's interrupt rate. So the PWM duty ; cycle is some fraction of the 144 two-cycle RTCC ticks. For precision, ; each toggling of a PWM output line must occur at the beginning of an ; interrupt, and somewhere in there must be enough time to take care of the ; UART. ; ; There are three "PWM modes". Modes A and B are "staggered", and mode C ; is sequential. This is staggered: ; ____________ p = PwmPhaseLag = 9 ; A: __| |_____________________ f1 = period_A - p ; <-p-|---f1---|---f2---><----r------ f2 = period_B - period_A + p ; _________________ r = 144 - period_B + p ; B: ______| |_____________ (period_B >= period_A) ; ; Thus, the code interrupts four times during the 144-tick UART period, ; once after p ticks, once after f1 ticks, once after f2 ticks, and then ; after r ticks. The trick is that period_B and period_A are guaranteed ; not to be greater than 72 (half of 144), because if they are, we just ; invert the PWM line and use it in the inverted sense. Thus, the max ; ticks that can be used by p + f1 + f2 is 72 + 9 = 81, which means that ; r will always be at least 144 - 81 = 63 ticks. This is plenty of time ; to take care of the UART. Since period_B must be >= period_A, there are ; two staggered modes. In mode A, B = 1 and A = 2. So, this is used when ; period_1 >= period_2. Mode B is the reverse, used when ; period_2 >= period_1. ; ; The PhaseLag in staggered mode comes from the time to interrupt, ; execute the PWM toggling code, and return from the interrupt. This is ; the closest that the interrupts can be spaced. But what if we want ; a period to be < 2 x PhaseLag? Staggered doesn't work. But luckily, ; since one period is so short, this leaves enough time to do the waves ; sequentially. This is mode C: ; ______ ; 1: __| |__________________________ f1 = period_1 ; <--f1--|------f2-------><----r---- f2 = period_2 ; _______________ r = 144 - period_1 - period_2 ; 2: _________| |__________ ; ; The maximum ticks used by f1 + f2 is 2 x 9 - 1 + 72 = 89, meaning that ; r will be at least 144 - 89 = 55 ticks. Counting up the longest path ; through the interrupt routine shows that to enter, execute, and leave ; can take up to 50 ticks. So it works. But it's cutting it close. ; ; But we need more granularity than just 1/144. So there is duty cycle ; "trimming". This means that for a certain fraction of cycles, the PWM ; period is one longer than nominal. The cycles go in sets of eight. ; On one cycle, between cycle 2 and cycle 8, the period is incremented. ; It is then decremented (restored to its original value) on the eighth ; cycle. This means that we get an effective granularity, after filtering, ; of 1/(144 * 8) = 1/1152. Much better. Of course, all these different ; PWM modes and different formulas for calculating interrupt delays from ; periods means that trimming is a more little complicated than it seems. ; But it's implemented, and it works. ; ; The UART checks if there is data to be transmitted, and then receives some ; data or waits for a start bit. There is no handshaking, so any bytes ; received while the buffer is full are ignored (gracefully). InterruptRoutine ; ;; Fast-phase PWM stuff ; mov w, pwm_pointer ; get pointer to phase info mov fsr, w ; get ready to deref mov w, indf ; read the output switching mask xor rb, w ; apply the mask and flip the pins inc fsr ; increment the pointer mov w, indf ; get the period inc pwm_pointer ; update pointer for next time incsz pwm_pointer ; are we done with fast phases? retiw ; if not, let's get out of here. ; ;; Counter stuff ; inc mastercounter ; increment master counter snz ; overflow? dec eventloop_timer ; if so, decrement the countdown ; ;; ADC stuff ; mov w, #adc_Clk_mask ; toggle the ADC clock pin xor rb, w ; ADC clock speed is thus 38.4 KHz ; ;; Serial stuff ; bank BankSerial ; do our UART thang decsz serial_txdivide ; only xmit once every 8 ints jmp :sendend mov w, #SerialDivider ; reset divider mov serial_txdivide, w test serial_txlength ; is there anything in the xmit buffer? snz jmp :sendend ; if not, go receive something. snb serialTxActive ; are we already xmitting a byte? jmp :sending ; if so, continue with that :sendfirstbit _getbuf serial_txgetptr, BankTxBuf ; get a byte from the buffer mov serial_txbyte, w ; put it into the variable setb serialTxActive ; set the flag that says we're xmitting mov w, #9 ; nine more bits to go mov serial_txcount, w setb serial_Tx ; put start bit on pin jmp :sendend ; all done :sending decsz serial_txcount ; decrement bit counter jmp :sendbit ; was that the last data bit? :sendlastbit dec serial_txlength ; if so, dec buffer length _incptr serial_txgetptr ; inc buf pointer, with wrapping clrb serialTxActive ; clear the flag clrb serial_Tx ; put stop bit on pin jmp :sendend ; we outtee :sendbit rr serial_txbyte ; put next bit into carry sc ; is it 0? setb serial_Tx ; if so, raise the pin snc ; is it 1? clrb serial_Tx ; if so, lower the pin ; that's all for that :sendend :recvstart clc sb serial_Rx ; read Rx pin stc ; and copy inverse to carry bit snb serialRxActive ; are we already receiving a byte? jmp :recving ; if so, continue with that snc ; is it a start bit? jmp :recvend ; if not, nothin' else to do! mov w, #9 ; if so, let's get 9 bits. mov serial_rxcount, w setb serialRxActive ; set the flag mov w, #SerialDivider2 ; get the next bit in the middle mov serial_rxdivide, w ; of its slot jmp :recvend :recving decsz serial_rxdivide ; receive bit once every 8 ints jmp :recvend mov w, #SerialDivider ; restore divider mov serial_rxdivide, w decsz serial_rxcount ; still more bits to receive jmp :recvbit ; if so, go get a bit :recvlastbit mov w, #15 ; shouldn't put more than 15 in the buf mov w, serial_rxlength-w ; see how much is already there... snc ; are we full? jmp :recvignore ; if so, ignore this byte. mov w, serial_rxbyte ; if not, get the received byte mov stashi, w ; put it into a global _putbuf serial_rxputptr, BankRxBuf, stashi ; put it into the buffer _incptr serial_rxputptr ; inc buffer pointer, with wrapping inc serial_rxlength ; say there's one more byte in the buf setb serialRxAvail ; set the "rx data available" flag :recvignore clrb serialRxActive ; clear the active flag skip ; it's all good :recvbit rr serial_rxbyte ; rotate carry into the recv'd byte :recvend ; ;; PWM duty-cycle trimming, and end of interrupt routine ; bank BankPWM mov w, pwm_startptr ; get pointer to first phase info mov pwm_pointer, w ; and get it ready for next time mov w, pwm_trimptr ; get pointer to trim mode code mov pc, w ; and jump there ; PWM mode A is staggered, PWM1 >= PWM2 trim_modeA decsz pwm_trim1 ; dec the trim counter for PWM1 jmp :notrim1 ; is it zero? dec pwm_period3 ; if so, adjust the periods to make inc pwm_period4 ; PWM1 one longer :notrim1 decsz pwm_trim2 ; dec the trim counter for PWM2 jmp :notrim2 ; is it zero? dec pwm_period2 ; if so, adjust the periods to make inc pwm_period3 ; PWM2 one longer :notrim2 decsz pwm_trimcounter ; dec the global trim counter jmp :nocounter ; is it zero? dec pwm_period4 ; if so, we need to restore the periods inc pwm_period2 ; to what they were before mov w, #PwmMaxTrim ; as well as reset all three counters add pwm_trim1, w ; to their maximums add pwm_trim2, w add pwm_trimcounter, w :nocounter mov w, pwm_period4 ; and we're done with this interrupt! retiw ; PWM mode B is staggered, PWM2 >= PWM1 trim_modeB decsz pwm_trim1 ; dec the trim counter for PWM1 jmp :notrim1 ; is it zero? dec pwm_period2 ; if so, adjust the periods to make inc pwm_period3 ; PWM1 one longer :notrim1 decsz pwm_trim2 ; dec the trim counter for PWM2 jmp :notrim2 ; is it zero? dec pwm_period3 ; if so, adjust the periods to make inc pwm_period4 ; PWM2 one longer :notrim2 decsz pwm_trimcounter ; dec the global trim counter jmp :nocounter ; is it zero? dec pwm_period4 ; if so, we need to restore the periods inc pwm_period2 ; to what they were before mov w, #PwmMaxTrim ; as well as reset all three counters add pwm_trim1, w ; to their maximums add pwm_trim2, w add pwm_trimcounter, w :nocounter mov w, pwm_period4 ; and we're done with this interrupt! retiw ; PWM mode C is sequential, PWM1 followed by PWM2 trim_modeC decsz pwm_trim1 ; dec the trim counter for PWM1 jmp :notrim1 ; is it zero? dec pwm_period2 ; if so, adjust the periods to make inc pwm_period4 ; PWM1 one longer :notrim1 decsz pwm_trim2 ; dec the trim counter for PWM2 jmp :notrim2 ; is it zero? dec pwm_period3 ; if so, adjust the periods to make inc pwm_period4 ; PWM2 one longer :notrim2 decsz pwm_trimcounter ; dec the global trim counter jmp :nocounter ; is it zero? inc pwm_period2 ; if so, we need to restore the periods inc pwm_period3 ; to what they were before dec pwm_period4 ; in mode C, this requires a little dec pwm_period4 ; more work. mov w, #PwmMaxTrim ; now we reset all three counters add pwm_trim1, w ; to their maximums add pwm_trim2, w add pwm_trimcounter, w :nocounter mov w, pwm_period4 retiw ;;;;;;;;;;;;;; ;; ;; Reset entry ;; ResetEntry mov w, #PortA_ResetValue ; initialize port pins mov ra, w mov w, #PortB_ResetValue mov rb, w mov w, #PortAdir_ResetValue ; set up input and output pins mov !ra, w mov w, #PortBdir_ResetValue mov !rb, w clrb led_0 ; turn on red light mov w, #statusbits_resetvalue mov statusbits, w bank BankSerial ; initialize serial variables clr serial_txlength ; nothing in the xmit buffer clr serial_txgetptr clr serial_txputptr clr serial_rxlength ; nothing in the recv buffer clr serial_rxgetptr clr serial_rxputptr bank BankPWM ; initialize pwm variables mov w, #pwm_mask2 mov pwm_pointer, w ; set up a mode C pwm output mov pwm_startptr, w mov w, #trim_modeC mov pwm_trimptr, w mov w, #-PwmPhaseLag ; minimum value mov pwm_negvalue1, w mov pwm_negvalue2, w mov pwm_period1, w ; period1 must be set to PwmPhaseLag mov pwm_period2, w mov pwm_period3, w mov w, #PwmPhaseLag+PwmPhaseLag-InterruptRate mov pwm_period4, w clr pwm_mask1 ; make it impotent for now clr pwm_mask2 clr pwm_mask3 clr pwm_mask4 mov w, #PwmMaxTrim ; no trim mov pwm_trim1, w mov pwm_trim2, w mov pwm_trimcounter, w bank BankUI ; initialize UI variables mov w, #UiDefaultAutoRead mov ui_autoreadcounter, w mov ui_autoreaddelay, w mov w, #5 ; wait for a little bit page PageDelay call Delay_LongLong mov w, #%00000000 ; okay, we're ready for the interrupt mov !option, w ; prescalar 1/2, let 'er rip call Delay_FullLong ; pause a bit... bank BankGyro1 ; initialize gyro 1 clrb gyroWhichGyro mov w, #gyrostatus_resetvalue ; init the status bits mov gyro_statusbits, w mov w, #reset_1_mask ; set the resetmask variable mov gyro_resetmask, w call GyroVector_CalibrateRaw ; calibrate gyro (as an amplifier) bank BankGyro2 ; initialize gyro 2 setb gyroWhichGyro mov w, #gyrostatus_resetvalue ; init the status bits mov gyro_statusbits, w mov w, #reset_2_mask ; set the resetmask variable mov gyro_resetmask, w page PageGyro call GyroVector_CalibrateRaw ; calibrate gyro (as an amplifier) clr eventloop_timer ; init the eventloop timer setb led_0 ; turn off red light clrb led_2 ; turn on green light ;;;;;;;;;;;;;; ;; ;; Event Loop ;; EventLoop mov w, #EventLoopDelay ; go through the event loop once add eventloop_timer, w ; every EventLoopDelay x 3.3 ms. page PageGyro clrb gyroWhichGyro ; let's do gyro 1 bank BankGyro1 call GyroVector_ProcessGyro ; do everything we need to do with it setb gyroWhichGyro ; now, gyro 2 bank BankGyro2 call GyroVector_ProcessGyro ; do it too page PageAnalysis call Analysis_NewSamples ; do some data analysis, if necessary page EventLoop bank BankUI snb uiAutoReadOn ; is autoread on? decsz ui_autoreadcounter ; if so, have we waited long enough for it? jmp :noautoread page PageUI call UI_AutoRead ; if so, go do it bank BankUI mov w, ui_autoreaddelay ; and reset the counter mov ui_autoreadcounter, w :noautoread :wait page PageUI snb serialRxAvail ; receive anything on the serial port? call UI_ProcessInput ; if so, deal with the command. page EventLoop sb eventloop_timer.7 ; did the eventloop countdown roll over? jmp :wait ; if not, keep waiting jmp EventLoop ; if so, start over. ;;;;;;;;;;;;;;;;;;;;; ;; ;; ;; Pages 2 and 3 ;; ;; ;; ;;;;;;;;;;;;;;;;;;;;; org SerialPage*256 PageSerial equ $ PageUI equ $ PageDecimal equ $ ;;;;;;;;;;;;;;;;;;;;;;; ;; ;; Serial port routines ;; ; Serial_GyroSendByte (modifies w, fsr, stash2) ; sends a byte over the serial port, and restores the bank and ; page so it can be called from a Gyro routine. Serial_GyroSendByte call Serial_SendByte skip ; Serial_GyroSendCString (modifies w, fsr, stash2, stash1) ; sends a string over the serial port, and restores the bank and ; page so it can be called from a Gyro routine. Serial_GyroSendCString call Serial_SendCString _gyro_restorebank retp ; Serial_RecvByte (modifies w, fsr, stash2) ; pulls a byte out of the receive buffer, waiting for one if the buffer ; is empty. The byte is returned in w. Serial_RecvByte bank BankSerial :loop sb serialRxAvail ; is there data to be gotten? jmp :loop ; if not, wait for it. _getbuf serial_rxgetptr, BankRxBuf ; get a byte from the buffer mov stash2, w ; stash it _incptr serial_rxgetptr ; update buffer pointer clrb serialRxAvail ; update avail flag and buf length, in decsz serial_rxlength ; such a way that it is safe for an setb serialRxAvail ; interrupt to occur in the middle. mov w, stash2 ; get back byte ret ; and return with it. ; Serial_SendByte (modifies w, fsr, stash2) ; puts a byte into the transmit buffer. If the buffer is full, it waits ; until it isn't. The byte is passed in w. Serial_SendByte bank BankSerial mov stash2, w ; stash the byte to xmit :loop mov w, #15 ; shouldn't put more than 15 in the buf mov w, serial_txlength-w ; how much is already there? snc ; carry is clear if txlength < 15 jmp :loop ; otherwise, wait for buf to clear _putbuf serial_txputptr, BankTxBuf, stash2 ; put a byte in the buffer _incptr serial_txputptr ; update buffer pointer inc serial_txlength ; update buffer size ret ; that's enough. ; Serial_SendCString (modifies w, fsr, stash2, stash1) ; sends a null-terminated string (in ROM) over the serial port. Any $ff bytes ; in the string are converted to crlf's. The string must be on the memory ; page StringPage. The offset to the string from the start of the page is ; passed in w. Serial_SendCString bank BankSerial mov stash1, w ; stash our pointer :loop mov w, stash1 ; get back the pointer mov m, #StringPage ; m is high byte of address iread ; get the character from the string mov m, #$0f ; restore mode incsz wreg ; check if it's a -1 jmp :nocrlf ; if not, go send it out mov w, #13 ; if so, send a CR (ascii 13) call Serial_SendByte mov w, #11 ; and send a LF (ascii 10) :nocrlf dec wreg ; get the original character back snz ; is it zero? ret ; if so, we're done call Serial_SendByte ; if not, send out the character inc stash1 ; increment string pointer jmp :loop ; do it again ; Serial_SendDecimalNumber (modifies w, fsr, stash2, stash1) ; sends a number between -1024 and 1023 in base-10 representation, ; complete with negative sign. The lobyte of the number is passed ; in w, and the hibyte in stash1. Serial_SendDecimalNumber bank BankSerial sb stash1.2 ; is it negative? jmp :notnegative ; if not, ignore this part not wreg ; 2's complement invert the not stash1 ; two-byte number inc wreg snz inc stash1 mov serial_stash, w ; save the lobyte mov w, #'-' ; send a negative sign call Serial_SendByte mov w, serial_stash ; get back the lobyte :notnegative call :decimalconvert ; convert to BCD representation bank BankSerial mov serial_stash, w ; store the lobyte clrb serial_stash2.0 ; don't send leading zeros mov w, <>stash1 ; send hi nibble of hibyte call :digit mov w, stash1 ; send lo nibble of hibyte call :digit mov w, <>serial_stash ; send hi nibble of lobyte call :digit setb serial_stash2.0 ; always send this digit, zero or not mov w, serial_stash ; send lo nibble of lobyte call :digit ret :digit and w, #$0f ; isolate low nibble snb serial_stash2.0 ; are we skipping zeros? jmp :dodigit ; if not, go do this digit snz ; if so, check if it's zero ret ; if zero, then skip it. :dodigit setb serial_stash2.0 ; don't skip any more zeros mov stash2, w mov w, #'0' ; add ascii '0' to the nibble add w, stash2 call Serial_SendByte ; and send it out ret :decimalconvert jmp Decimal_Convert ; routine is in upper half of page ;;;;;;;;;;;;;;;;;;;;;;;;;; ;; ;; User Interface routines ;; ; UI_AutoRead (modifies w, stash2, stash1, fsr) ; should be called when the autoreadcounter rolls over. Checks which ; channel(s) are on autoread, and makes them read. UI_AutoRead clrb gyroWhichGyro ; switch to gyro 1 bank BankGyro1 snb gyrostatusAutoRead ; is autoread turned on? call UI_Read ; if so, read the channel setb gyroWhichGyro ; switch to gyro 2 bank BankGyro2 snb gyrostatusAutoRead ; is autoread turned on? call UI_Read ; if so, read the channel ret ; UI_ProcessInput (modifies w, stash2, stash1, fsr) ; receives a serial character, examines it to see if it's a valid command, ; and if so, jumps to the appropriate handler. UI_ProcessInput call Serial_RecvByte ; get the command snb wreg.7 ; high bit set? jmp UI_BadCommand ; if so, it's wrong mov stash2, w ; save our command xor w, #'?' ; version query? snz jmp UI_Version ; if so, go do that _srgtel stash2, '0' ; is it less than 1? jmp UI_BadCommand ; if so, it's wrong _srgtl stash2, '8' ; is it less than or equal to 8? jmp UI_AutoSpeed ; if so, then do autoread speed bank BankGyro1 clrb gyroWhichGyro ; if uppercase, do gyro 1 snb stash2.5 ; if lowercase, do gyro 2 bank BankGyro2 ; (the datasheet says sb will skip a setb gyroWhichGyro ; bank instruction plus the next one) mov w, stash2 and w, #$1f ; isolate lower five bits add pc, w ; and jump to the command handler :at jmp UI_BadCommand :a jmp UI_AutoReadOn :b jmp UI_BadCommand :c jmp UI_BadCommand :d jmp UI_Debug :e jmp UI_BadCommand :f jmp UI_BadCommand :g jmp UI_SwitchGyro :h jmp UI_BadCommand :i jmp UI_SwitchInt :j jmp UI_BadCommand :k jmp UI_BadCommand :l jmp UI_BadCommand :m jmp UI_BadCommand :n jmp UI_BadCommand :o jmp UI_BadCommand :p jmp UI_BadCommand :q jmp UI_AutoReadOff :r jmp UI_Read :s jmp UI_BadCommand :t jmp UI_BadCommand :u jmp UI_BadCommand :v jmp UI_Verbose :w jmp UI_BadCommand :x jmp UI_BadCommand :y jmp UI_BadCommand :z jmp UI_Zero :obr jmp UI_BadCommand :bsl jmp UI_BadCommand :cbr jmp UI_BadCommand :hat jmp UI_BadCommand :bar jmp UI_BadCommand ; UI_SendAckN ; sends an OK byte for the current channel UI_SendAckN mov w, #UiAck1 ; get ACK for gyro 1 snb gyroWhichGyro ; is it really gyro 1? mov w, #UiAck2 ; if not, get ACK for gyro 2 jmp Serial_SendByte ; send it out ; UI_SendAck ; sends an generic OK byte UI_SendAck mov w, #UiAck ; get ACK character jmp Serial_SendByte ; send it out ; UI_SendChannelHeader ; sends a newline, and then "1: " or "2: ", depending on which gyro is active ; ; UI_SendChannelHeadless ; same thing, but without the newline UI_SendChannelHeader mov w, #StringEndline ; send a newline call Serial_SendCString UI_SendChannelHeadless mov w, #'1' ; send a "1" snb gyroWhichGyro ; unless we're on gyro 2 mov w, #'2' ; in which case, send a "2" call Serial_SendByte mov w, #StringColon ; send ": " jmp Serial_SendCString ret ; UI_BadCommand ; called when the received character is not a valid command UI_BadCommand mov w, #UiNak sb uiVerbose ; verbose mode? jmp Serial_SendByte ; if not, send out the NAK char mov w, #StringBadCommand ; if so, get a string jmp Serial_SendCString ; send it out ; UI_Read ; reads from the channel UI_Read snb uiVerbose ; are we verbose? jmp :verbose ; if so, go do that mov w, #UiChannel1 ; say we're reading channel 1 snb gyroWhichGyro ; unless it's actually channel 2 mov w, #UiChannel2 ; in which case, say it's channel 2 call Serial_SendByte ; send out the header byte _gyro_restorebank ; switch back to the proper bank mov w, gyro_outputhi ; stash high byte in stash1 mov stash1, w mov w, gyro_outputlo ; send out the low byte call Serial_SendByte mov w, stash1 ; get back the high byte jmp Serial_SendByte ; send it out too, and leave. :verbose call UI_SendChannelHeadless ; send a verbose channel header _gyro_restorebank ; switch back to our bank mov w, gyro_outputhi ; put the high byte into stash1 mov stash1, w mov w, gyro_outputlo ; put the low byte into w call Serial_SendDecimalNumber ; send out a pretty number mov w, #StringComma ; get a spacer string jmp Serial_SendCString ; send it out too, and leave. ; UI_AutoReadOn ; turns on autoread for the given channel UI_AutoReadOn setb gyrostatusAutoRead ; turn on the local status bit setb uiAutoReadOn ; turn on the global status bit sb uiVerbose ; are we talking a lot? jmp UI_SendAckN ; if not, send an OK byte call UI_SendChannelHeader ; if so, send the channel number mov w, #StringAutoRead ; send "autoread" call Serial_SendCString mov w, #StringOn ; send " on" jmp Serial_SendCString ; UI_AutoReadOff ; turns off autoread for the given channel UI_AutoReadOff clrb gyrostatusAutoRead ; turn off the local status bit clrb uiAutoReadOn ; turn off the global status bit for now mov w, #$20 xor fsr, w ; switch to the other gyro bank snb gyrostatusAutoRead ; is its autoread turned on? setb uiAutoReadOn ; if so, turn the global flag back on sb uiVerbose ; are we talking a lot? jmp UI_SendAckN ; if not, send an OK byte call UI_SendChannelHeader ; if so, send the channel number mov w, #StringAutoRead ; send "autoread" call Serial_SendCString mov w, #StringOff ; send " off" jmp Serial_SendCString ; UI_AutoSpeed ; sets the autoread delay. An ascii number from '0' to '8' should be in stash2. UI_AutoSpeed bank BankUI mov w, ++stash2 ; get ascii digit plus one mov ui_stash, w ; save it for later and w, #$0f ; isolate digit part of ascii code clr ui_autoreaddelay ; get ready for exponentiation stc ; get ready to rotate in a one bit :exploop rl ui_autoreaddelay ; multiply by two decsz wreg ; decrement counter jmp :exploop ; if not done, keep multiplying mov w, ui_autoreaddelay ; copy the delay to the counter mov ui_autoreadcounter, w sb uiVerbose ; if it's not in verbose mode, jmp UI_SendAck ; send an ACK mov w, #StringCrAutoRead ; if it is, send a more informative string call Serial_SendCString mov w, #StringDelay call Serial_SendCString bank BankUI mov w, --ui_stash ; get back the ascii code for the delay number call Serial_SendByte ; send out the number mov w, #StringEndline ; send out an endline jmp Serial_SendCString ; UI_Verbose ; turns on or off verbose mode UI_Verbose clrb uiVerbose ; turn off the verbose flag sb gyroWhichGyro ; unless it was uppercase setb uiVerbose ; in which case, turn it on sb uiVerbose ; are we in verbose mode now? jmp UI_SendAck ; if not, send a global ACK mov w, #StringVerboseOn ; if so, verbosely say that verbose jmp Serial_SendCString ; mode is on ; UI_Version ; displays version information UI_Version mov w, #StringVersion ; send a version string snb uiVerbose ; unless verbose is off jmp Serial_SendCString call UI_SendACK ; in which case, send an ACK mov w, #VersionMinor ; and the minor version byte call Serial_SendByte mov w, #VersionMajor ; and the major version byte jmp Serial_SendByte ; UI_SwitchGyro ; switches the current gyro to angular velocity mode UI_SwitchGyro sb uiVerbose ; should we be quiet? jmp :noverbose call UI_SendChannelHeader ; if not, send the channel number mov w, #StringCalibratingFor call Serial_SendCString ; and say what we're doing mov w, #StringAngular call Serial_SendCString mov w, #StringModeDots call Serial_SendCString _gyro_restorebank ; go back to the proper bank :noverbose page PageGyro ; calibrate the gyro for amplifier mode call GyroVector_CalibrateRaw jmp UI_SwitchDone ; say that calibration is complete ; UI_SwitchInt ; switches the current gyro to rotation mode UI_SwitchInt sb uiVerbose ; should be be quiet? jmp :noverbose call UI_SendChannelHeader ; if not, send the channel number mov w, #StringCalibratingFor call Serial_SendCString ; and say what we're doing mov w, #StringRotation call Serial_SendCString mov w, #StringModeDots call Serial_SendCString _gyro_restorebank ; go back to the proper bank :noverbose page PageGyro ; calibrate the gyro for integrator mode call GyroVector_CalibrateIntegrator UI_SwitchDone sb uiVerbose ; if not in verbose mode jmp UI_SendAckN ; send an ACK mov w, #StringCalibration ; otherwise, say that calibration is complete jmp Serial_SendCString ; UI_Zero ; zeros the current gyro UI_Zero page PageGyro ; switch to the gyro page call GyroVector_Zero ; zero the thingee sb uiVerbose ; if not in verbose mode jmp UI_SendAckN ; send an ACK call UI_SendChannelHeader ; otherwise, send the channel number mov w, #StringZero ; and announce that zeroing has been done jmp Serial_SendCString ; UI_Debug ; turns debug mode on or off UI_Debug clrb uiDebug ; clear the debug bit sb gyroWhichGyro ; unless it was uppercase setb uiDebug ; in which case, set it sb uiVerbose ; verbose mode? jmp UI_SendAck ; if not, just send an ACK mov w, #StringDebug ; if so, say "debug mode" call Serial_SendCString mov w, #StringOff ; say " off" snb uiDebug ; unless we turned it on mov w, #StringOn ; in which case, say " on" jmp Serial_SendCString ;;;;;;;;;;;;;;;;;;;;; ;; ;; Decimal conversion ;; ; _decimal_subtract value ; part of the division routine, extracted into a macro _decimal_subtract MACRO 1 mov w, #\1 ; get 25 x 2^n mov w, decimal_numlo-w ; subtract it from the value rl decimal_subresult ; if value is bigger, rotate in a 1 snb decimal_subresult.0 ; and use the difference as the mov decimal_numlo, w ; new value ENDM ; Decimal_Convert (modifies w, stash1) ; converts a two-byte number into its base-10 representation, from ; zero to 1023. The number is passed with the hibyte in stash1 and ; the lobyte in w. The BCD representation is returned with the ; hibyte in stash1 and the lobyte in w. Returns with retp. Decimal_Convert bank BankDecimal mov decimal_numlo, w mov w, stash1 ; get the hibyte mov decimal_numhi, w ; and store it rr decimal_numhi ; divide by four rr decimal_numlo rr decimal_stash ; rotating the low bits into stash rr decimal_numhi rr decimal_numlo rr decimal_stash clr decimal_subresult ; get ready to divide by 25 _decimal_subtract 200 _decimal_subtract 100 _decimal_subtract 50 _decimal_subtract 25 mov w, #DecimalTable ; ready the lookup table add w, decimal_subresult mov m, #DecimalTable/256 iread ; look up the decimal representation mov stash1, w ; and save it as the high digits rl decimal_stash ; return the low two bits (from the rl decimal_numlo ; divide-by-4) to the remainder rl decimal_stash ; of the divide-by-25 to get a rl decimal_numlo ; divide-by-100 remainder. mov w, #DecimalTable ; ready the lookup table add w, decimal_numlo mov m, #DecimalTable/256 iread ; look up the decimal representation mov m, #$0f retp ; low digits are in w, so we're done DecimalTable dw $000,$001,$002,$003,$004,$005,$006,$007,$008,$009 dw $010,$011,$012,$013,$014,$015,$016,$017,$018,$019 dw $020,$021,$022,$023,$024,$025,$026,$027,$028,$029 dw $030,$031,$032,$033,$034,$035,$036,$037,$038,$039 dw $040,$041,$042,$043,$044,$045,$046,$047,$048,$049 dw $050,$051,$052,$053,$054,$055,$056,$057,$058,$059 dw $060,$061,$062,$063,$064,$065,$066,$067,$068,$069 dw $070,$071,$072,$073,$074,$075,$076,$077,$078,$079 dw $080,$081,$082,$083,$084,$085,$086,$087,$088,$089 dw $090,$091,$092,$093,$094,$095,$096,$097,$098,$099 ;;;;;;;;;;;;;;;;;;;;; ;; ;; ;; Pages 4 and 5 ;; ;; ;; ;;;;;;;;;;;;;;;;;;;;; org AdcPwmPage*256 PageADC equ $ PagePWM equ $ PageGyro equ $ PageDelay equ $ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;; ;; Jump vectors to gyro routines ;; GyroVector_CalibrateIntegrator jmp Gyro_CalibrateIntegrator GyroVector_CalibrateRaw jmp Gyro_CalibrateRaw GyroVector_Zero jmp Gyro_Zero GyroVector_ProcessGyro jmp Gyro_ProcessGyro ;;;;;;;;;;;;;;;;; ;; ;; Delay routines ;; ; Delay_FullLong (modifies w, stash2) ; waits for at least 11.9 ms ; ; Delay_Long (modifies w, stash2) ; waits for at least 47 us x w Delay_FullLong mov w, #0 Delay_Long mov stash2, w mov w, #0 :loop decsz wreg jmp :loop decsz stash2 jmp :loop JumpRet ret ; Delay_LongLong (modifies w, stash2, stash1) ; waits for at least 11.9 ms x w Delay_LongLong mov stash1, w :loop call Delay_FullLong decsz stash1 jmp :loop ret ; Delay_10 (modifies nothing) ; waits for 10 cycles (about 452 ns), including the call ; ; Delay_9 (modifies nothing) ; waits for 9 cycles (about 407 ns), including the call Delay_10 nop Delay_9 jmp JumpRet ;;;;;;;;;;;;;;; ;; ;; ADC routines ;; ; ADC_DoConversion (modifies w) ; performs the A/D conversion. The ADC channel to use is determined ; by gyrostatusIntegrate and gyroWhichGyro. The bank must be set to ; BankGyroN on entry. The result of the conversion is returned in w. ADC_DoConversion call ADC_ClockHighLow ; wait a little bit, to make sure call ADC_ClockHighLow ; the conversions are spaced out call ADC_ClockHighLow ; enough mov w, #PortBdir_DataOutValue mov !rb, w ; adc_Data is an output pin now setb adc_Data ; put a start bit on the data line call ADC_ClockHighLow ; wait for the clock to go low clrb adc_CS ; lower the ADC chip select call ADC_ClockHighLow ; clock in the start bit call ADC_ClockHighLow ; clock in SGL/DIF high snb gyrostatusIntegrate ; read integrator or amplifier? clrb adc_Data ; integrators = 0, amplifiers = 1 call ADC_ClockHighLow ; clock it in (ODD/SIGN bit) sb gyroWhichGyro ; which gyro to read? clrb adc_Data ; gyro1 = 0 snb gyroWhichGyro setb adc_Data ; gyro1 = 1 :waithigh sb adc_Clk ; wait for the clock to rise jmp :waithigh call Delay_10 ; wait some hold time mov w, #PortBdir_DataInValue mov !rb, w ; adc_Data is an input pin now call ADC_ClockLowHigh ; clock out the leading zero mov w, #$01 ; clear w, except for a completion flag clc ; clear carry so we rotate in a zero :loop call ADC_ClockLowHigh ; clock out a conversion bit (MSB first) snb adc_Data ; was it a 1? stc ; if so, set the carry rl wreg ; and rotate that carry into the result sc ; did the completion flag rotate out? jmp :loop ; if not, keep clockin' setb adc_CS ; if so, raise the ADC chip select ret ; and we're done here ; ADC_ClockHighLow (modifies nothing) ; waits for a falling edge of the ADC clock ADC_ClockHighLow :high sb adc_Clk ; clock high? jmp :high ; if not, wait for it :low snb adc_Clk ; clock low? jmp :low ; if not, wait for it ret ; ADC_ClockHighLow (modifies nothing) ; waits for a rising edge of the ADC clock ADC_ClockLowHigh :low snb adc_Clk ; clock low? jmp :low ; if not, wait for it :high sb adc_Clk ; clock high? jmp :high ; if not, wait for it ret ;;;;;;;;;;;;;;; ;; ;; PWM routines ;; ; PWM_NudgeUp (modifies w, stash2) ; slightly increments the PWM duty cycle, by one-eight of a "value". ; The PWM channel is determined by gyroWhichGyro. The bank on entry ; must be BankGyroN. PWM_NudgeUp mov w, gyro_statusbits ; copy the gyro statusbits into stash2 mov stash2, w mov w, #pwm_trim1 ; make fsr point to either pwm_trim1 snb gyroWhichGyro ; or pwm_trim2 mov w, #pwm_trim2 mov fsr, w _pwm_waitforsync ; wait until it's safe sb stash2.0 ; is gyrostatusPwmHigh clear? jmp :low ; if so, go do the low routine :high snb indf.3 ; is the trim eight? jmp :highsetvalue ; if so, we need to set the value inc indf ; if not, just bump it up one jmp pwmReturnToGyro ; and get out of here :highsetvalue dec fsr ; switch fsr to pwm_negvalue mov w, #InterruptRate ; get the interrupt rate add w, indf ; add the negvalue, to get the "actual value" inc wreg ; increment because we're bumping up one jmp pwmSetValueAndMaxTrim ; go set the value and max the trim :low decsz indf ; decrement trim variable jmp pwmReturnToGyro ; if not zero, we're done here dec fsr ; if zero, switch fsr to pwm_negvalue mov w, /indf ; get the negvalue, inverted inc wreg ; now we've got the actual value inc wreg ; increment because we're bumping up one jmp PWM_SetValue ; and go set that (new trim is 8, or off) ; PWM_NudgeDown (modifies w, stash2) ; slightly decrements the PWM duty cycle, by one-eight of a "value". ; The PWM channel is determined by gyroWhichGyro. The bank on entry ; must be BankGyroN. PWM_NudgeDown mov w, gyro_statusbits ; copy the gyro statusbits into stash2 mov stash2, w mov w, #pwm_trim1 ; make fsr point to either pwm_trim1 snb gyroWhichGyro ; or pwm_trim2 mov w, #pwm_trim2 mov fsr, w _pwm_waitforsync ; wait until it's safe snb stash2.0 ; is gyrostatusPwmHigh set? jmp pwmNudgeDownHigh ; if so, go do the high routine :low snb indf.3 ; is the trim eight? jmp :lowsetvalue ; if so, we need to set the value inc indf ; if not, just bump it up one jmp pwmReturnToGyro ; and get out of here :lowsetvalue dec fsr ; switch fsr to pwm_negvalue mov w, /indf ; get the negvalue inverted = value-1 pwmSetValueAndMaxTrim call PWM_SetValue ; set that to be the new value mov w, #pwm_trim1 ; now we have to set the trim to maximum snb gyroWhichGyro ; put the pointer to the appropriate trim mov w, #pwm_trim2 ; variable back into fsr mov fsr, w mov w, #1-PwmMaxTrim ; subtract 7 from the trim variable add indf, w ; (atomically, of course) jmp pwmReturnToGyro ; and we're done. pwmNudgeDownHigh decsz indf ; decrement trim variable jmp pwmReturnToGyro ; if not zero, we're done here dec fsr ; if so, switch fsr to pwm_negvalue mov w, #InterruptRate ; get the interrupt rate add w, indf ; add the negvalue, to get the "actual value" dec wreg ; decrement because we're bumping down one jmp PWM_SetValue ; set that to be the new value ; PWM_SetValue (modifies w, stash2; leaves bank as BankGyroN) ; sets one of the pwm channels to the given value. The value is bounds ; checked, and is inverted if it is greater than InterruptRate/2. ; The new value and the other channel's value are then used to ; select the new PWM mode, and set up the periods and masks appropriately. ; The trim on the channel is cleared. Which channel to use is ; determined by gyroWhichGyro, and the value is passed in w. PWM_SetValue _gyro_restorebank ; switch to the gyro bank mov stash2, w ; stash the value mov w, #-Interruptrate/2 ; compare it to InterruptRate/2 add w, stash2 mov w, #pwm_1_mask ; but before using that, put the snb gyroWhichGyro ; appropriate bitmask into w mov w, #pwm_2_mask snc ; now, if less than half, do low jmp :high ; if more than half, do high :low snb gyrostatusPwmHigh ; are we already doing a low value? xor rb, w ; if not, flip the pwm bit clrb gyrostatusPwmHigh ; and say that we're doing low now. jmp :lowhighdone :high sb gyrostatusPwmHigh ; are we already doing a high value? xor rb, w ; if not, flip the pwm bit setb gyrostatusPwmHigh ; and say that we're doing high now. not stash2 ; get the negative value, minus one mov w, #InterruptRate+1 ; add InterruptRate plus one add stash2, w ; so we subtract value from InterruptRate sc ; was it bigger than InterruptRate? clr stash2 ; if so, clip to zero :lowhighdone mov w, #PwmMinValue ; check if the result is less than the mov w, stash2-w ; minimum allowed value for PWM sc ; if it is, add the difference back in sub stash2, w ; so it clips to PwmMinValue. mov w, #pwm_negvalue1 ; get pointer to the appropriate snb gyroWhichGyro ; negvalue variable, and put it into mov w, #pwm_negvalue2 ; fsr (effectively doing a bank BankPWM mov fsr, w ; at the same time) mov w, /stash2 ; get the negative of our value inc wreg mov indf, w ; and put it into pwm_negvalue ; Now that we have the new value, we must look at the two values together to see ; what PWM mode should be used and what the periods should be. _srltel pwm_negvalue1, -PwmMinModeAB ; check if PWM1 is below the jmp :modeC ; threshold for staggered PWM. _srltel pwm_negvalue2, -PwmMinModeAB ; check the same thing for PWM2 jmp :modeC ; if either is too low, do mode C mov w, pwm_negvalue1 ; which is bigger, PWM 1 or PWM 2? _swlte pwm_negvalue2 ; mode A is PWM 1 >= PWM 2 jmp :modeB ; mode B is PWM 1 < PWM 1 :modeA _pwm_waitforsync ; wait until it's safe to mess with pwm stuff mov w, #pwm_mask1 ; set the pointer and startpointer to pwm_mask1 mov pwm_pointer, w mov pwm_startptr, w mov w, #pwm_2_mask ; set mask1 and mask3 to flip PWM2 mov pwm_mask1, w mov pwm_mask3, w mov w, #pwm_1_mask ; set mask2 and mask4 to flip PWM1 mov pwm_mask2, w mov pwm_mask4, w mov w, #PwmPhaseLag ; period2 = -(PWM2-PhaseLag) add w, pwm_negvalue2 mov pwm_period2, w mov w, pwm_negvalue1-w ; period3 = -(PWM1-PWM2+PhaseLag) mov pwm_period3, w ; = -PWM1-period2 mov w, #PwmPhaseLag-InterruptRate mov pwm_period4, w ; period4 = -(Rate-PWM1-PhaseLag) mov w, pwm_negvalue1 sub pwm_period4, w mov w, #trim_modeA ; tell it to use the mode A trim jmp :done :modeB _pwm_waitforsync ; wait until it's safe to mess with pwm stuff mov w, #pwm_mask1 ; set the pointer and startpointer to pwm_mask1 mov pwm_pointer, w mov pwm_startptr, w mov w, #pwm_1_mask ; set mask1 and mask3 to flip PWM1 mov pwm_mask1, w mov pwm_mask3, w mov w, #pwm_2_mask ; set mask2 and mask4 to flip PWM2 mov pwm_mask2, w mov pwm_mask4, w mov w, #PwmPhaseLag ; period2 = -(PWM1-PhaseLag) add w, pwm_negvalue1 mov pwm_period2, w mov w, pwm_negvalue2-w ; period3 = -(PWM2-PWM1+PhaseLag) mov pwm_period3, w ; = -PWM2-period2 mov w, #PwmPhaseLag-InterruptRate mov pwm_period4, w ; period4 = -(Rate-PWM2-PhaseLag) mov w, pwm_negvalue2 sub pwm_period4, w mov w, #trim_modeB ; tell it to use the mode B trim jmp :done :modeC _pwm_waitforsync ; wait until it's safe to mess with pwm stuff mov w, #pwm_mask2 ; set the pointer and startpointer to pwm_mask2 mov pwm_pointer, w mov pwm_startptr, w mov w, #pwm_1_mask ; set mask2 to flip PWM1 mov pwm_mask2, w mov w, pwm_negvalue1 ; period2 = -PWM1 mov pwm_period2, w mov w, #pwm_1_mask|pwm_2_mask mov pwm_mask3, w ; set mask3 to flip both PWM1 and PWM2 mov w, pwm_negvalue2 ; period3 = -PWM2 mov pwm_period3, w mov w, #pwm_2_mask ; set mask4 to flip PWM2 mov pwm_mask4, w mov w, #-InterruptRate ; period4 = -(Rate-PWM1-PWM2) mov pwm_period4, w mov w, pwm_negvalue1 add w, pwm_negvalue2 sub pwm_period4, w mov w, #trim_modeC ; tell it to use the mode C trim :done mov pwm_trimptr, w ; store the trim pointer inc fsr ; fsr points to pwm_trim1 or pwm_trim2 now mov w, pwm_trimcounter ; this should be 8... mov indf, w ; clear the trim for the PWM channel pwmReturnToGyro _gyro_restorebank ; switch bank to BankGyro1 or BankGyro2 ret ; hasta ;;;;;;;;;;;;;;;;;;;;;;;; ;; ;; calibration routines ;; ; Gyro_CalibrateIntegrator (modifies w, stash2, stash1) ; finds a reference voltage that makes the integrator output as flat as possible. ; It does this in two steps, first by using PWM_SetValue, and then with PWM_NudgeUp ; and PWM_NudgeDown. The bank on entry must be BankGyroN. Returns with retp. Gyro_CalibrateIntegrator setb gyrostatusIntegrate ; say that we are reading the integrator clr gyro_driftcounter ; intialize the drift counter ; gyroCalibrateCourse ; figures out the best PWM_SetValue value to use for the integrator's differential ; reference, one bit at a time. Bank must be BankGyroN on entry. gyroCalibrateCourse clr gyro_pwmvalue ; clear the pwm value mov w, #$80 ; start with the high bit set in the mask mov gyro_calmask, w :try mov w, gyro_calmask ; get the current bitmask or gyro_pwmvalue, w ; use it to set a bit in the value mov w, gyro_pwmvalue ; get the new pwm value call PWM_SetValue ; and go use it _gyro_reseton ; turn on the reset switch mov w, #2 ; wait for the pwm to charge up call Delay_LongLong call ADC_DoConversion ; get the output with the reset switch on mov gyro_sample, w ; and save it _gyro_resetoff ; turn off the reset switch mov w, #5 ; wait for it to start drifting call Delay_LongLong mov w, #GyroCourseTimeout ; get the timeout period mov eventloop_timer, w ; and use eventloop_timer to count it down :wait snb eventloop_timer.7 ; have we timed out with no sample change? jmp :timeout ; if so, let's just use what we have. call ADC_DoConversion ; if not, get a sample mov w, gyro_sample-w ; subtract from the sample at reset snz ; has it changed? jmp :wait ; if not, keep waiting snc ; if so, was it negative or positive? jmp :leavebitset ; if negative, we want to keep the bit set mov w, /gyro_calmask ; if positive, we want to clear the bit and gyro_pwmvalue, w ; that we just tried :leavebitset clc rr gyro_calmask ; move the active bit in the mask down one sc ; was that the last bit? jmp :try ; if not, keep at it :timeout mov w, gyro_pwmvalue ; we've got our favorite PWM value call PWM_SetValue ; so set it ; gyroCalibrateFine ; uses PWM_NudgeUp and PWM_NudgeDown to adjust the reference voltage until the ; integrator output is as flat as possible. Bank must be BankGyroN on entry. gyroCalibrateFine mov w, #GyroFineIterations ; set how many times to nudge around mov gyro_calcounter, w :cfloop _gyro_reseton ; turn on the reset switch mov w, #2 ; wait for the pwm to charge up call Delay_LongLong call ADC_DoConversion ; get the output with the reset switch on mov gyro_sample, w ; and save it _gyro_resetoff ; turn off the reset switch mov w, #5 ; wait for it to start drifting call Delay_LongLong mov w, #GyroFineTimeout ; get the timeout period mov eventloop_timer, w ; and use eventloop_timer to count it down :wait snb eventloop_timer.7 ; have we timed out with no sample change? jmp :timeout ; if so, we've waited long enough call Delay_FullLong ; wait a little bit call ADC_DoConversion ; get the output mov w, gyro_sample-w ; subtract from the reset output snz ; has it changed? jmp :wait ; if not, keep waiting sc ; if so, was it negative or positive? jmp :positive call PWM_NudgeUp ; if negative, nudge up skip :positive call PWM_NudgeDown ; if positive, nudge down decsz gyro_calcounter ; check the counter jmp :cfloop ; if we should do more, then do more :timeout jmp gyroInitAndZero ; if we're done, go set the baseline ; Gyro_CalibrateRaw (modifies w, stash2) ; finds a reference voltage that puts the amplifier output as close to midrange ; as possible. It only uses PWM_SetValue, and does not nudge. Bank must be ; BankGyroN on entry. Ends with retp. Gyro_CalibrateRaw clrb gyrostatusIntegrate ; say that we're not integrating clr gyro_pwmvalue ; clear the pwm value mov w, #$80 ; start with the high bit set in the mask mov gyro_calmask, w :try mov w, gyro_calmask ; get the current bitmask or gyro_pwmvalue, w ; use it to set a bit in the value mov w, gyro_pwmvalue ; get the new pwm value call PWM_SetValue ; and go use it mov w, #2 ; wait for the pwm to charge up call Delay_LongLong call ADC_DoConversion ; read the sensor sb wreg.7 ; is it above or below halfway? jmp :leavebitset ; if below, we want the bit to stay set mov w, /gyro_calmask ; if above, we want to clear the bit and gyro_pwmvalue, w ; that we just set :leavebitset clc rr gyro_calmask ; move the active bit in the mask down one sc ; was that the last bit? jmp :try ; if not, keep at it mov w, gyro_pwmvalue ; if so, we've got our PWM value call PWM_SetValue ; so set it gyroInitAndZero mov w, #2 ; wait a little bit for that to take effect call Delay_LongLong call ADC_DoConversion ; read the sensor mov gyro_sample, w ; say that's the current value mov gyro_oldaverage, w ; initialize the drift correction mov gyro_history1, w mov gyro_history2, w mov gyro_history3, w ; and go on to Gyro_Zero ; Gyro_Zero (modifies w) ; changes the integrator register so the current output is zero. This actually ; works in both integrator mode and raw mode. Bank must be GyroBankN on entry, ; and gyro_sample must be set properly. Ends with retp. Gyro_Zero mov w, /gyro_sample ; get the negative of the current value inc wreg mov gyro_positionlo, w ; store it in the integration register mov w, #$ff ; so "position zero" is where it is mov gyro_positionhi, w ; right now. retp ;;;;;;;;;;;;;;;;;;;; ;; ;; Input processing ;; ; Gyro_ProcessGyro (modifies w, stash2, stash1) ; does everything necessary to process a gyro. Bank must be set to BankGyroN ; on entry. Gyro_ProcessGyro call ADC_DoConversion ; get a sample mov gyro_sample, w ; store it add w, gyro_positionlo ; add the position offset mov gyro_outputlo, w ; store it as our output sample mov w, gyro_positionhi ; get the hibyte snc ; was carry set? inc wreg ; if so, inc hibyte mov gyro_outputhi, w ; and store it sb gyrostatusIntegrate ; are we looking at the integrator? ret ; if not, that's all we need to do. snb uiDebug ; are we in debug mode? jmp :nodivide ; if so, don't divide by four rr gyro_outputhi ; divide by two rr gyro_outputlo rr gyro_outputhi ; divide by two again rr gyro_outputlo mov w, gyro_outputhi ; get the high byte and w, #$3f ; clear the high two bits snb gyro_outputhi.5 ; unless the original high bit was set or w, #$c0 ; in which case, set the high two bits mov gyro_outputhi, w ; and store that :nodivide movsz w, --gyro_drifttimer ; is the counter equal to $01? dec gyro_drifttimer ; if not, decrement it ; gyroRecenterCheck (modifies w, stash2) ; checks whether the integrator output has exceeded its bounds and must be recentered. gyroRecenterCheck _srgtl gyro_sample, GyroLowThreshold ; is it lower that the low threshold? jmp :recenter ; is so, go recenter _srgtel gyro_sample, GyroHighThreshold ; is it lower that the high threshold? jmp :recenterdone ; if so, no need to recenter :recenter mov w, gyro_sample ; get the offending sample mov stash1, w ; and stash it in stash1 _gyro_reseton ; reset the integrator mov w, #5 ; wait a little bit for the cap to drain call Delay_Long call ADC_DoConversion ; sample the reset value mov gyro_sample, w ; store it as the new sample _gyro_resetoff ; turn off the reset switch mov w, gyro_sample ; get the reset sample mov w, stash1-w ; subtract from the old sample sc ; higher or lower? jmp :wrapnegative :wrappositive add gyro_positionlo, w ; add the difference to the register snc inc gyro_positionhi ; inc hibyte if necessary jmp :wrapdone :wrapnegative add gyro_positionlo, w ; add the (negative) difference sc dec gyro_positionhi ; dec hibyte if necessary :wrapdone :recenterdone ; gyroDriftCheck (modifies w, stash2, stash1) ; checks whether the output seems to be slowly drifting in one direction, and ; if so, nudges the reference voltage to try to correct for it. gyroDriftCheck mov w, ++gyro_history2 ; average the history values, tree style add gyro_history3, w ; first, get x = (c + d + 1)/2 rr gyro_history3 mov w, ++gyro_sample add w, gyro_history1 ; then, get y = (a + b + 1)/2 rr wreg add w, gyro_history3 ; now, get (x + y)/2 rr wreg mov gyro_average, w ; and that's our average! mov w, #2 add w, gyro_oldaverage ; subtract current value from stored value mov w, gyro_average-w ; w = average - oldavergage - 2 snz ; sample - oldsample = 2? jmp :plus2 ; go do something inc wreg snz ; sample - oldsample = 1? jmp :plus1 ; go do something inc wreg snz ; sample - oldsample = 0? jmp :driftdone ; no change, so don't do anything inc wreg snz ; sample - oldsample = -1? jmp :minus1 ; go do something inc wreg snz ; sample - oldsample = -2? jmp :minus2 ; go do something :clear clr gyro_driftcounter ; if none of those, clear the drift counter mov w, gyro_average ; store the current sample so we can mov gyro_oldaverage, w ; compare to it next time jmp :driftdone :minus1 test gyro_driftcounter ; are we already following a drift? sz jmp :mnotnew ; if so, keep that up clrb gyrostatusDriftUp ; if not, say we have a down drift jmp :newtimer ; and start following it :mnotnew snb gyrostatusDriftUp ; old drift in opposite direction? jmp :gotjitter ; if so, go say we got some jitter clrb gyrostatusGotJitter ; if not, clear the jitter flag jmp :driftcount ; and check the time limit :plus1 test gyro_driftcounter ; are we already following a drift? sz jmp :pnotnew ; if so, keep that up setb gyrostatusDriftUp ; if not, say we have an up drift jmp :newtimer ; and start following it :pnotnew sb gyrostatusDriftUp ; old drift in opposite direction? jmp :gotjitter ; if so, go say we got some jitter clrb gyrostatusGotJitter ; if not, clear the jitter flag jmp :driftcount ; and check the time limit :gotjitter setb gyrostatusGotJitter ; set the jitter flag jmp :driftdone ; and ignore for now :minus2 sb gyrostatusGotJitter ; is the jitter flag set? jmp :clear ; if not, start over sb gyrostatusDriftUp ; was the old drift positive? jmp :clear ; if not, start over clrb gyrostatusDriftUp ; if so, the jitter was really part clr gyro_driftcounter ; of a down drift jmp :newtimer ; go follow that up drift :plus2 sb gyrostatusGotJitter ; is the jitter flag set? jmp :clear ; if not, start over snb gyrostatusDriftUp ; was the old drift negative? jmp :clear ; if not, start over setb gyrostatusDriftUp ; if so, the jitter was really part clr gyro_driftcounter ; of an up drift :newtimer mov w, #GyroDriftTime ; reset the drift timer mov gyro_drifttimer, w clrb gyrostatusGotJitter ; clear the jitter flag inc gyro_driftcounter ; increment the drift counter mov w, gyro_average ; store the current sample so we can mov gyro_oldaverage, w ; compare to it next time jmp :driftdone ; and that's all for now :driftcount movsz w, --gyro_drifttimer ; has the DriftTime elapsed? jmp :clear ; if not, it's too fast to be a drift mov w, gyro_driftcounter ; if so, check the drift counter xor w, #3 ; are we up to the threshold yet? sz ; if so, go nudge something jmp :newtimer ; if not, reset the timer and wait sb gyrostatusDriftUp ; are we drifting up or down? jmp :nudgeup ; if drifting down, nudge up. :nudgedown mov w, #-2 ; we've been drifting up, so compensate add gyro_positionlo, w ; for it by subtracting a couple from sc ; the position register dec gyro_positionhi call PWM_NudgeDown ; nudge down the reference voltage jmp :nudgedone :nudgeup mov w, #2 ; we've been drifting down, so add gyro_positionlo, w ; compensate by adding a couple to snc ; the position register inc gyro_positionhi call PWM_NudgeUp ; nudge up the reference voltage :nudgedone sb uiDebug ; is debug mode on? jmp :clear ; if not, go clear mov w, #'n' ; if so, let the user know that we've page PageSerial ; nudged, using the gyro-friendly call Serial_GyroSendByte ; serial routine jmp :clear ; and start over with this counter stuff :driftdone mov w, gyro_history2 ; shift down the history buffer mov gyro_history3, w mov w, gyro_history1 mov gyro_history2, w mov w, gyro_sample mov gyro_history1, w ret ; end of Gyro_ProcessGyro ;;;;;;;;;;;;;;;;;;;;; ;; ;; ;; Pages 6 and 7 ;; ;; ;; ;;;;;;;;;;;;;;;;;;;;; org AnalysisPage*256 PageAnalysis equ $ ;;;;;;;;;;;;;;;; ;; ;; Data analysis ;; ; Analysis_NewSamples ; If you want to do analysis of the data, here's the place to put your ; code. This is called after every sample pair has been read and ; processed. The samples are available in gyro_outputhi and ; gyro_outputlo of BankGyro1 and BankGyro2. You have a full page ; (512 bytes) of code space available, as well as an entire RAM ; bank (BankAnalysis). You also have at least 9 ms, so that should be ; plenty of time to do your thing. Analysis_NewSamples ret ;;;;;;;;;;;;;;;; ;; ;; ;; Page 1 ;; ;; ;; ;;;;;;;;;;;;;;;; org StringPage*256 ;;;;;;;;;; ;; ;; Strings ;; StringVersion dw 255,'DAX Rotation Sensor v' dw VersionMajor+48,'.',VersionMinor+48 dw 255,'Bret Victor, 8/2000',255,255,0 StringComma dw ', ',0 StringBadCommand dw 255,'Unrecognized command!',255,0 StringDebug dw 255,'Debug mode',0 StringVerboseOn dw 255,'Verbose mode' StringOn dw ' on',255,0 StringOff dw ' off',255,0 StringCrAutoRead dw 255 StringAutoRead dw 'Auto-read',0 StringZero dw 'zeroed',255,0 StringCalibration dw 'Calibration complete.' StringEndline dw 255,0 StringDelay dw ' delay' StringColon dw ': ',0 StringCalibratingFor dw 'Calibrating for ',0 StringAngular dw 'angular velocity',0 StringRotation dw 'rotation',0 StringModeDots dw ' mode...',255,0