; ; ; Code for the 12F675 PIC to manage an ESC for brushed motors. ; SCCS: @(#) esc.asm 1.12@(#) processor 12F675 radix dec errorlevel -302 ; ; This module is configurable for a number of different combinations of ; facilities. Please note that the circuit used for each of the configurations ; is different. So pay attention to the diagram below. ; ; It is *your* responsibility to ensure that the configuration parameters set ; in the code below match your circuit. ; ; The options are: ; ESC without brake (use 'BRAKE equ 0") ; ESC with brake (use 'BRAKE equ 1,2") ; ; Low Voltage Cutoff (set 'LOWVOLTS' in mV) ; *NOTE* a LOWVOLTS of 0 will disable the Low Voltage Cutoff ; ; Alternate Low Voltage Cutoff (set 'LOWVOLTSALT' in mV) ; The alternate LVC is used when GP3 is connected to 0 volts ; (ie. the jumper is installed) ; *NOTE* a LOWVOLTSALT of 0 will disable the alternate LVC ; in which case LOWVOLTS will apply in all situations. ; ; Auto throttle adjustment, or fixed throttle stops ; (Set PULSE_LO_HI to 0 for auto adjust, or width in 0.001msec) ; ; Conversion to use for throttle -> PWM drive. ; Set THROTTLEMAP to 1 for 50% throttle = 50% PWM rate ; Set THROTTLEMAP to 2 for 50% throttle = 50% motor input power ; Set THROTTLEMAP to 3 for car use with 50% -> 50% motor power ; ; Lost Model Alert option. ; Set LMAFREQUENCY to 0 to disable LMA option ; Set LMAFREQUENCY to 1 to drive siren with inbuilt driver ; Set LMAFREQUENCY to ???? to drive siren with given frequency, ; eg. 4500 means 4.5kHz. ; ; Reverse mode ; Set REVERSE to 0 for no reverse processing. ; Set REVERSE to 1 for reverse output toggled by brake. ; SLOWSTART equ 1 ; 0 for fast start, 1 for slow BRAKE equ 2 ; 0 for no brake, 1 for a hard, 2 for soft BRAKEENB equ 0 ; 0 for brake always on, 1 for GP3 control REVERSE equ 1 ; 0 for no reverse, 1 for output toggle on brake LOWVOLTS equ 6000 ; 6 Volt cutoff LOWVOLTSALT equ 0 ; 9 Volt cutoff when GP3 configured PULSE_LO_HI equ 0 ; 0 for auto throttle calibrate THROTTLEMAP equ 3 ; 0/1 is linear, 2 is power, 3 is car LMAFREQUENCY equ 0 ; 0, 1 or frequency in Hz GLITCHENB equ 0 ; 0 for no glitch counter, 1 for GP3 control ; ; The reason the circuit is depends on the various options is that there ; are not quite enough pins on the 12F675 to do everything if you want a brake ; (or other output pin such as reverse or the LMA). ; The main problem arises because to do accurate PWM in software while reading ; an accurate value for the throttle pulse width requires the externally gated ; TIMER1 to be used, and the gate is active low - whereas the r/c control ; signal is active high. While it is possible to use the PICs comparitor to do ; the inversion this requires 2 pins. When you have a brake or other output ; there are not enough pins available. ; ; Assumes: ; Bandgap as calibrated at factory ; CPD disabled ; CP disabled ; BODEN enabled ; MCLR disabled ; PWRTE enabled ; WDT (Watchdog) enabled ; OSC internal RC (no clockout) ; ; The basic circuit for the ESC is as follows: ; ; ; power ----------------------- ; | | ; ----------------- ; | + (1) (8) - | ; | PIC12F675 | ; | | ; rx | (3) GP4 |-<- ; >--XXXX-------| GP1 (6) | | (active low rx pulse) ; 4k7 | (5) GP2 |->- ; | | ; sense >----| GP? (?) | ; | (2) GP5 | ----------> Motor On (active high) ;Brake off >----| GP3 (4) | ; | (7) GP0 | ----------> Brake On (active high) ; ----------------- ; ; ; See the full circuit diagram for further details. ; ; Features of this software are as follows: ; ; Watchdog timer ensure the software is running, this is used to ensure ; that the software always gets back to the main sensing code. If ; it doesn't the chip is reset. ; ; Timer 0 is used to measure the time for the PWM drive to the motor ; and brake. ; ; This timer runs at 1Mhz with a 1:2 prescaler. These interrupt, ; together with the programable interrupt routine permits PWM ; frequencies from 8kHz at minimum throttle down to 2kHz at full ; throttle. ; ; Timer 1 is used to measure the input pulse width. Pulse width should be ; 1.5msec nominal at central position +/- 0.5msec depending on the ; control direction. ; ; At 4Mhz clock timer 1 will run at 0.001msec count rate. This gives ; count values for 1.5msec that are well within 16-bit resolution. ; ; ; The throttle can either be auto calibrating, or operate over a fixed range. ; When the throttle operates over a fixed range it is assumed to be centred ; on 1.500 msec, however in the auto-calibrate mode the throttle will cover ; the range of values seen during programming. ; ; The auto throttle calibration assumes that the first receiver input it sees ; is throttle off. The ESC then tracks the highest, stable, pulse seen and uses ; this as the maximum throttle. When the throttle again returns to idle the ; ESC enters normal operation. A stable pulse is a sequence of 'autopulse' ; pulses that are within +/- autofuzz of the first pulse. ; ; The LOS system is based on a software timer in the code that waits for ; valid throttle pulses. This timer is part of the various loops and so ; the exact LOS time varies depending on what arrives on the input line. ; ; The power up arming delay is 'armpulse' counts from powerup, and ; 'rearmpulse' after a LOS or power-fail detection. ; ; The low supply voltage is checked every time we receive a servo pulse. ; In theory we don't want to discharge cells below 0.9V/cell, also ; we want to ensure there is sufficient battery voltage to power the LDO ; regulator for a reasonable time. The ESC uses the A/D converter in the ; PIC to measure the battery supply voltage and averages this over 4 (2^lowvavgln2) ; samples (to stop a single 'spike' in current from turning the ESC off). ; When low battery voltage is detected this is treated as identical to ; LOS, this causes the motor to stop and wait for rearming. ; ; Possible issues: ; ; -> The brake is off when there is no valid receiver input. ; A: This is probably desireable, if the receiver input is intermittent then ; applying the brake is probably a bad idea since when the receiver input ; starts working the power is probably going to be applied. ; ; -> The controller switches the brake on immediately that the controller starts up ; and detects a valid receiver sequence. ; A: I don't think that this is a problem. ; ; Possible enhancements: ; ; i) Perhaps a slower soft brake may be required, check this out during ; testing. No: the current braking rate is very good with the GWS ; EPS400 gearbox. ; ii) It may be possible to use the EEPROM to configure brake and other ; parameters. ; iii) It may be possible to use the EEPROM to record minimum voltages etc. ; However, writing the EEPROM requires interrupts are off, this may affect ; PWM generation. ; iv) Check the audio level via the motor at low RPM, David reports that ; there is significant audio artifacts at low RPM - the high frequency ; PWM is supposed to stop this... This was significantly improved ; by the change to the PWM engine that replaced the lowest two throttle ; possitions with idle. The problem was that these PWM rates were ; insufficient to spin the motor and just caused noise. ; #include __CONFIG(_BODEN_ON & _MCLRE_OFF & _PWRTE_ON & _WDT_ON & _INTRC_OSC_NOCLKOUT) ; Configuration parameters ; Main parameters autopulse equ 5 ; Required number 'same value' to program autofuzz equ 3 ; +/- autofuzz is the same value armpulse equ 20 ; Required throttle off to arm at start rearmpulse equ 4 ; Rearm count after LOS/low volts downpulse equ 4 ; Throttle decrease requires this many pulses uppulse equ 4 ; Throttle increase requires this many pulses revpulse equ (5*50) ; Reverse relay requires this many pulses losms equ 100 ; LOS if no signal in 100ms lowvavgln2 equ 2 ; ln2(number of low volts avg) lmanegsteps equ 4 ; -ve throttle steps for LMA turnon lmaLOSms equ 400 ; LMA triggers LOS after 400ms no signal lmareqpulse equ 10 ; Required to turn LMA on lmaalarmcnt equ (6*4) ; 6 seconds of alarm lmasilentcnt equ (24*4) ; 24 seconds of silence ; GPIO register bits and other constants inrxbit equ 4 ; Active low rx input always on GP4 motoron equ 5 ; Motor is always controlled by GP5 ; If both a brake and low voltage cutout are required then ; the internal comparitor cannot be used as an inverter and a separate ; transistor received inverter is required. The options at this stage ; involving moving the pins around... if LOWVOLTS > 0 voltsense equ 0 ; Main battery sense voltage GP0 if LOWVOLTSALT > 0 altlowvolts equ 3 ; GP3 low for alternate LVC point endif endif if BRAKE > 0 && BRAKEENB > 0 brakeenb equ 3 ; GP3 high to enable brake endif if GLITCHENB glitchrpt equ 3 ; GP3 low to triger glitch readout endif ; How many output signals do we need? outputcnt = 0 if BRAKE > 0 outputcnt += 1 endif if LMAFREQUENCY > 0 outputcnt += 1 endif if REVERSE > 0 outputcnt += 1 endif if (outputcnt == 0) || ((LOWVOLTS == 0) && (outputcnt <= 1)) ; In this case we can use the internal comparitor as an inverter ; because there are enough pins available comparitorinuse equ 1 ; Comparitor is in use inrxraw equ 1 ; Active high receiver input outrxinv equ 2 ; Active low receiver output (--> inrxbit) if BRAKE > 0 brakeon equ 0 ; brake FET control on GP0 endif if REVERSE > 0 reverseon equ 0 ; Reverse relay on GP0 endif if LMAFREQUENCY > 0 piezodrive equ 0 ; LMA output signal on GP0 endif else comparitorinuse equ 0 ; Comparitor is not in use if BRAKE > 0 brakeon equ 2 ; brake FET control on GP2 if both in use endif if REVERSE > 0 reverseon equ 1 ; reverse out on GP1 endif if LMAFREQUENCY > 0 piezodrive equ 1 ; LMA output signal on GP1 endif endif ; RAM Definitions cblock 20h w_save ; Int save: !! Both banks used !! sts_save ; escstatus ; Various status bits rearmcnt ; Count for next arm operation armcnt ; ESC arm counter downcnt ; Throttle decrease filter counter upcnt ; Throttle increase filter counter (!SLOWSTART) throttle ; Current throttle pwmmode ; PWM Interupt offset pwmONio ; GPIO value during phase A (on) pwmOFFio ; GPIO value during phase B (off) pwmONt0 ; TMR0 value during phase A (on) pwmOFFt0 ; TMR0 value during phase B (off) newpwmON ; Next PWM 'ON' time newpwmOFF ; Next PWM 'OFF' time cntperth ; # of 0.001 counts per throttle step cntthbl ; Throttle base in counts (LSB) cntthbh ; Throttle base in counts (MSB) lowvoltavg:1< 0 dt "B" if BRAKEENB dt "e" endif endif if LOWVOLTS > 0 dt "V", LOWVOLTS/100 endif if PULSE_LO_HI != 0 dt "F" endif if LMAFREQUENCY > 0 dt "L" endif if GLITCHENB dt "G" endif if REVERSE > 0 dt "R", 30h+REVERSE endif while ($ & 7) != 0 dt " " endw ; ----------------- Maths library -------------------------------------- ; Maths Library Data cblock ; --- Input Parameters --- math_al ; LSB of A register math_ah ; MSB of A register math_b ; The B register math_rl ; LSB of result math_rh ; MSB of result ; --- Internal Temporary variables math_bex ; Top 8 bits of B math_ml ; Mask of where we are upto math_mh endc ; Compute A:16 / B:8 and put the result:16 into R, the ; remained is left in A ; This is unsigned division, anyone dividing by zero ; will put this into an infinite loop and the watchdog ; will operate... math_a_div_b ; Initialise clrf math_bex ; Top 8 bits of B clrf math_ml ; Mask of where we are upto clrf math_mh incf math_ml,f ; Bottom bit clrf math_rl ; Result clrf math_rh ; Shift up and extend B to start division div_s1 bcf STATUS,C ; setup for rotate rlf math_b,f rlf math_bex,f btfsc STATUS,C ; All done? goto div_s2 ; yes rlf math_ml,f rlf math_mh,f goto div_s1 div_s2 rrf math_bex,f rrf math_b,f ; Do the division, see if the subtraction is possible div_l1 movfw math_b subwf math_al,f movfw math_bex btfss STATUS,C addlw 1 subwf math_ah,f btfss STATUS,C ; compute A-B goto div_l2 ; Too big, can't count this movfw math_ml iorwf math_rl,f movfw math_mh iorwf math_rh,f ; Or on the mask div_l3 bcf STATUS,C ; shift down the mask then B rrf math_mh,f rrf math_ml,f btfsc STATUS,C return ; All done, result is set rrf math_bex,f rrf math_b,f goto div_l1 div_l2 movfw math_b ; Add back what we subtracted addwf math_al,f btfsc STATUS,C incf math_ah,f movfw math_bex addwf math_ah,f goto div_l3 ; Compute A:16 * B:8 and put the result:16 into R, the ; This is unsigned multiplication math_a_mul_b clrf math_rl clrf math_rh movfw math_b movwf math_bex mul_1 btfss math_bex,0 ; Something to add? goto mul_2 movfw math_al addwf math_rl,f movfw math_ah btfsc STATUS,C addlw 1 addwf math_rh,f mul_2 bcf STATUS,C rrf math_bex,f movfw math_bex btfsc STATUS,Z return ; run out of things to multiply bcf STATUS,C rlf math_al,f rlf math_ah,f goto mul_1 ; ----------------- LMA system -------------------------- if LMAFREQUENCY > 0 ; ; For piezo elements that do not have a built in siren we need ; to be able to generate frequencies between 800Hz and 5000Hz. ; At 800Hz we need one output change every 625us, at 5000Hz ; we need an output change every 100us. ; ; For normal on/off control of a piezo element with built in siren ; there is no timing control. ; ; It is desirable that the LMA alarm stop immediately that the LMA ; condition no longer exists. For example, if the LMA is turned off ; while the 'SOS' is sounding it should stop immediately. ; ; To do all this timing we modify the configuration of timer 1 (the PWM) ; timer to obtain the apprpriate interrupt rate. The special timer1 code ; also counts down another counter for measuring elapsed time. ; For LMAFREQUENCY == 1 we interrupt every 499us and do not touch the GPIO. ; ; ; LMA specific register data cblock lmatL ; 16-bit timer containing time delay lmatH lmatival ; Interval counter lmatmpcnt ; Counter for the 'beep' loop endc ; LMA constants, this depends on the type of piezo ; siren we have if LMAFREQUENCY == 1 lmaTrte equ 2000 ; 2000 interrupts per second lmaTcnt equ 256 - ((1000000+lmaTrte/2) / lmaTrte - 21 + 1)/2 else if LMAFREQUENCY < 1000 ; Determine parameters for < 1kHz using 8us per timer clock lmaTrte equ LMAFREQUENCY*2 lmaTcnt equ 256 - ((1000000+lmaTrte/2) / lmaTrte - 25 + 4)/8 else ; Determine parameters for >= 2kHz using 2us per timer clock lmaTrte equ LMAFREQUENCY*2 lmaTcnt equ 256 - ((1000000+lmaTrte/2) / lmaTrte - 25 + 1)/2 endif endif ; Now calculate the 'unit' time constant in terms of the interrupt rate lmaIcnt equ (lmaTrte/4)+1 ; Four time units per second ; Switch the interrupt routines etc into appropriate mode to ; drive the piezo sounder turnpiezoon bsf escstatus, esc_LMApiezoon ; Flag piezo active ; Stop the ESC driving the motor & brake FETs alloff ; Then configure timer0 as required for LMA mode, only required for low ; frequency piezo units if LMAFREQUENCY > 1 && LMAFREQUENCY < 1000 bsf STATUS,RP0 movlw 00000010b ; Enable pullups, timer0 clk/4 with 1:8 prescale movwf OPTION_REG bcf STATUS,RP0 endif ; Commence LMA interrupts for timing and piezo tone generation movlw int_lma - int_refpoint movwf pwmmode return ; Switch the piezo sounder off turnpiezooff bcf escstatus, esc_LMApiezoon ; Restore the timer0 configuration if we altered it, only applies to ; low frequency piezo units if LMAFREQUENCY > 1 && LMAFREQUENCY < 1000 bsf STATUS,RP0 movlw 00000000b ; Enable pullups, timer0 clk/4 with 1:2 prescale movwf OPTION_REG bcf STATUS,RP0 endif return ; Exit LMA mode a reenter ESC mode exitLMA call turnpiezooff goto offreset turnLMAon ; Flag we are in LMA mode bsf escstatus, esc_LMAon call turnpiezoon ; Use the arm counter as the number of valid 0 throttles we need to see ; to exit LMA mode movlw armpulse movwf armcnt ; ; Now we are in LMA mode, in LMA mode we want to produce the 'SOS' pattern ; on the piezo once every 30 seconds until we see a number of valid ; throttle off pulses. ; ; The 'SOS' pattern can either be produced as a oscillation on the Piezo drive ; signal at the LMAFREQUENCY (to drive a piezo element directly) or an on/off ; pattern for use with sirens with built in oscillators. ; ; To get accurate LMA timing we use the PWM engine interrupts to perform timing. ; We install a special PWM interrupt routine that will be called while the LMA ; is running. ; ; Go through the process of sounding the alarm ; then pausing for 30 sec, then sound the alarm etc. ; Although this loop appears to only check once every 30 seconds it ; actually exits immediately the LMA event is over. The reason ; being that the subroutines return immediately once the LMA event is over ; and hence the SOS stuff completes immediately and the check for end of LMA ; performs the exit operation. ; This saves a lot of repetative test for exit code. lmaloop btfss escstatus,esc_LMAon goto exitLMA ; LMA event is over movlw lmaalarmcnt/2 movwf lmatmpcnt lmalp movlw 1 call lma_sound movlw 1 call lma_silent decfsz lmatmpcnt,F goto lmalp movlw lmasilentcnt ; And silence call lma_silent goto lmaloop ; Sound the Morse signals, and pauses, while monitoring for the end of LMA lma_silent movwf lmatival pwmon 0 ; disable piezo drive goto lma_ilp ; assumes piezo is already off lma_sound movwf lmatival ; remember how many timer loops we are doing pwmon 1< 1 btfsc escstatus,esc_pwmOFF goto lma_pwmOFF ; We are currently in phaseA (on), so produce phaseB (off) bsf escstatus,esc_pwmOFF movfw pwmOFFio movwf GPIO goto lmasrvtmr lma_pwmOFF ; We are currently in phaseB bcf escstatus,esc_pwmOFF endif ; drive the output in the 'ON' phase movfw pwmONio movwf GPIO lmasrvtmr ; Decrement the timer decfsz lmatL,F goto lmasrvdone decf lmatH,F ; All done, come back in a known time... lmasrvdone movlw lmaTcnt goto rearm endif ; ------------------ Glitch reporting function --------------------- ; ; Beeps out on either the piezo, or motor, the glitch counter if GLITCHENB ; Define some macros that manage the production of the glitch report codes if LMAFREQUENCY > 0 ; Report using the LMA piezo beep_s macro movlw 2 call lma_sound endm beep_l macro movlw 6 call lma_sound endm delay_s macro movlw 2 call lma_silent endm delay_l macro movlw 6 call lma_silent endm else ; Report using the motor as a buzzer beep_s macro call sound_250 endm beep_l macro call sound_250 call sound_250 call sound_250 endm delay_s macro call delay_250 endm delay_l macro call delay_250 call delay_250 call delay_250 endm endif cblock beep10s ; The number of 10s to beep out beep1s ; The number of 1s to beep out beeptmp ; countdown as things are beeped out endc glitchreport if LMAFREQUENCY > 0 call turnpiezoon else ; Stop the interrupts alloff bcf INTCON,GIE endif ; The user has requested that we 'beep' out the glitch counter movfw glitchcnt sublw 99 ; > 99 is too big... btfss STATUS,C goto beeptoobig ; How many 10's clrf beep10s movfw glitchcnt count10s addlw -10 btfss STATUS,C goto got10s incf beep10s,F goto count10s got10s ; The rest are ones addlw 10 movwf beep1s movfw beep10s btfsc STATUS,Z goto no10s call beepdigit delay_l no10s movfw beep1s call beepdigit delay_s goto endreport beepdigit movwf beeptmp iorlw 0 ; check for zero btfsc STATUS,Z goto beepzero beepdlp beep_s decfsz beeptmp,F goto beepdlp1 return beepdlp1 delay_s goto beepdlp beepzero beep_l return beeptoobig beep_l delay_s beep_l delay_s goto endreport endreport if LMAFREQUENCY > 0 call turnpiezooff else bsf INTCON,GIE endif goto offreset endif ; ----------------- PWM Table Lookup ----------------------------------- g_pwmmode addlw pwmtype_tbl - int_refpoint movwf PCL ; Off to the table lookup g_pwmONt0 addlw pwmONt0_tbl - int_refpoint movwf PCL ; Off to the table lookup g_pwmOFFt0 addlw pwmOFFt0_tbl - int_refpoint movwf PCL ; Off to the table lookup ; ----------------- PWM Computed Goto Code Page ------------------------ ; All computed entry points MUST reside in this 256 byte page ; org ($+0FFh) & ~0FFh pwm_page int_refpoint ; ; Define the LMA system interrupts if we need them if LMAFREQUENCY > 0 int_lma goto lmaservice endif ; ; Include the appropriate interrupt routines and data lookups for the ; variable rate PWM. You can include one of the available PWM engines. ; Set THROTTLEMAP to the appropriate value. if THROTTLEMAP <= 1 #include endif if THROTTLEMAP == 2 #include endif if THROTTLEMAP == 3 #include endif ; check that this entry point is 'within' the page if (pwm_page >> 8) != ($ >> 8) fatal error PWM code too large endif ; ; Be careful with this code, the instruction cound has been ; carefully balanced to provide symetrical paths with GPIO ; updated at the same point in each flow. int_std btfsc escstatus,esc_pwmOFF goto in_pwmOFF ; We are currently in phaseA (on), so produce phaseB (off) bsf escstatus,esc_pwmOFF movfw pwmOFFio movwf GPIO movfw pwmOFFt0 goto rearm in_pwmOFF ; We are currently in phaseB movfw pwmONio movwf GPIO movfw pwmONt0 bcf escstatus,esc_pwmOFF nop rearm ; Clear the timer interrupt, and reload counter movwf TMR0 bcf INTCON, T0IF ; All done, restore status swapf sts_save,w movwf STATUS swapf w_save,f swapf w_save,w retfie rearm_ONt0 movfw pwmONt0 goto rearm rearm_OFFt0 movfw pwmOFFt0 goto rearm ; Provide a return instruction for software delay timing emptyreturn return ; -------------------------- Main ESC code ---------------------- ; ; Chip reset, or similar, operation. start ; Calibrate the internal oscilator bsf STATUS,RP0 call 3FFh movwf OSCCAL bcf STATUS,RP0 ; Initialise PCLATH. WARNING: don't touch this register.... movlw int_refpoint >> 8 movwf PCLATH ; Default to output driven low when enabled clrf GPIO ; Configure the weak pullups, and analog input bits, and the comparator that ; is used as an inverter, so that the control signal can control TMR1 if comparitorinuse bsf STATUS,RP0 movlw 10101000b movwf VRCON ; Select a 1/3 supply rail reference point bcf STATUS,RP0 movlw 00000011b ; Comparitor with internal reference movwf CMCON endif bsf STATUS,RP0 movlw 01010000b ; 16Tosc for A/D conversion if comparitorinuse iorlw 1< 0 iorlw 1< 0 andlw ~(1< 0 andlw ~(1< 0 andlw ~(1< 0 movlw 00000001b | (voltsense << 2) movwf ADCON0 ; Enable the A/D converter endif ; Initialise RAM, start with all zeros in bank 0 movlw 0x20 movwf FSR clrlp clrf INDF incf FSR,F btfss FSR,7 goto clrlp ; Then the special values movlw armpulse movwf rearmcnt movlw lmareqpulse movwf lmareqcnt ; Initialise PWM engine alloff ; Initialise the low voltage averaging variable i i = 0 movlw 0FFh while i < (1< 0 ; This is the fixed endpoint version, the arithmetic in this case ; is done by the assembler constant counts = PULSE_LO_HI / (throttlesteps-throttlecalidle) constant calpoint = 1500 - (counts * (throttlesteps-throttlecalidle))/2 constant zerobase = calpoint - counts * throttlecalidle constant lmaonpoint = (calpoint-(1500 - PULSE_LO_HI/2)) + lmanegsteps*counts movlw counts movwf cntperth ; The width of each step movlw low zerobase movwf cntthbl movlw high zerobase movwf cntthbh if LMAFREQUENCY != 0 movlw low lmaonpoint movwf lmanegl movlw high lmaonpoint movwf lmanegh endif else ; The variable end point is more complex, we assume that what we are ; seeing originally is the 'min' setting and then we look for the largest ; pulse width and that is the 'max' setting. Then we calculate the ; constants we need. ; ; The algorithm is: ; Wait for ? consecutive pulses within ?us of each other, the smallest ; of these is the minimum position. ; ; Keep reading pulses and wait for ? pulses that are within ?us of each ; other. If they are > the minimum and > current max then remember this as ; the maximum. If the repetitive value is the zero throttle position the ; calculation is complete. ; cblock autominl ; minimum pulse seen autominh automaxl ; maximum pulse seen automaxh endc waitforset0 call getstablepulse btfsc escstatus, esc_LOS goto waitforset0 ; Signal has gone away again... ; This is the zero point.... movfw stableh movwf autominh movwf automaxh movfw stablel movwf autominl ; Remmember the minimum ; Calculate the assumed maximum point... addlw throttlesteps-throttlecalidle movwf automaxl btfsc STATUS,C incf automaxh,f ; Wait for the throttle to move to at least automaxh... waitforset1 call getstablepulse btfsc escstatus, esc_LOS goto waitforset1 ; no signal, still hold low point ; See if we have reached this limit movfw stablel subwf automaxl,w movfw stableh btfss STATUS,C addlw 1 subwf automaxh,w btfsc STATUS,C goto waitforset1 ; not enough throttle yet... ; Recalculate the pulse -> throttle map using the supplied information waitforset2 movfw stablel movwf automaxl movfw stableh movwf automaxh movfw autominl subwf automaxl,w movwf math_al movwf cntthbl movfw autominh btfss STATUS,C addlw 1 subwf automaxh,w movwf math_ah ; math_a:16 is the range movwf cntthbh ; So is cntthb:16 movlw throttlesteps-throttlecalidle movwf math_b ; Divide into steps call math_a_div_b movfw math_rl movwf cntperth ; That is all divided up... if throttlecalidle == 0 ; If the idle calibration point is zero throttle add the unused ; throttle range equally to the top and bottom of the range movwf math_al clrf math_ah ; Multiply back up... call math_a_mul_b movfw math_rl ; subtract from initial range subwf cntthbl,f movfw math_rh btfss STATUS,C addlw 1 subwf cntthbh,f ; cntthb is now the 'spare' range bcf STATUS,C rrf cntthbh,f rrf cntthbl,f ; Halve the space (some top, some bottom) else ; If the idle calibration point is not zero throttle then ; leave the unused throttle range at the top of the space clrf cntthbl clrf cntthbh endif if LMAFREQUENCY != 0 ; Determine the amount of -ve shift, this is the offset to the minimum we saw ; plus the appropriate number of -ve throttle steps movfw cntperth movwf math_al clrf math_ah movlw lmanegsteps movwf math_b call math_a_mul_b movfw cntthbl movwf lmanegl movfw cntthbh movwf lmanegh movfw math_rl addwf lmanegl,F movfw math_rh btfsc STATUS,C addlw 1 addwf lmanegh,F endif if throttlecalidle > 0 ; Determine the size of the 'brake' zone movfw cntperth movwf math_al clrf math_ah movlw throttlecalidle movwf math_b call math_a_mul_b ; This is how much we need on the bottom ; Subtract it from the bottom movfw math_rl subwf cntthbl,f movfw math_rh btfss STATUS,C addlw 1 subwf cntthbh,f endif ; Add back in the minimum value movfw autominl addwf cntthbl,f movfw autominh btfsc STATUS,C addlw 1 addwf cntthbh,f ; Assign the bottom cutoff ; Wait now and track the largest throttle we have seen, recompute the ; throttle mapping until we see zero throttle again..... waitforset3 call getstablepulse btfsc escstatus, esc_LOS goto waitforset3 ; no signal, keep looking ; Is this bigger? movfw automaxl subwf stablel,w movfw automaxh btfss STATUS,C addlw 1 subwf stableh,w btfsc STATUS,C goto waitforset2 ; new width ; no it is <=, so can we convert to a zero throttle? movfw stablel movwf math_al movfw stableh movwf math_ah call math_a_to_throttle ; check to see if this is <= throttlecalidle point addlw -(throttlecalidle+1) btfsc STATUS,C goto waitforset3 ; Conversion parameters for pulse -> throttle have now been set. ; Make a sound with the motor to indicate all OK and ready, the ; motor is the only transducer we have... ; Make a two 0.25sec 800hz 'blips' 0.25 sec apart to signal all OK and armed. ; These blips are not enough to cause the motor to turn, but enough to ; make an audible sound. ; Stop the interrupts bcf INTCON,GIE call sound_250 ; Sound for 250ms call delay_250 call sound_250 ; Restart the interrupts bsf INTCON,GIE endif ; Turn off all the drivers and wait for arming offreset pwmon 0 ; All FETs off clrf throttle ; Throttle is zero call updatepwm ; Update the PWM engine ; Initialise for arming state resettoarm movfw rearmcnt movwf armcnt ; Wait for valid arming state waitforarm call getthrottle if LMAFREQUENCY > 0 btfsc escstatus, esc_LMAlos goto turnLMAon btfsc escstatus, esc_LMAreq goto turnLMAon ; LMA mode requested endif btfsc escstatus, esc_LOS goto resettoarm ; Signal has gone away again... call g_pwmmode ; Convert to a PWM mode addlw -(int_0on-int_refpoint) ; check for idle position btfsc STATUS,Z goto waitiszero ; Throttle is idle addlw -(int_0brake - int_0on) ; check for brake position btfss STATUS,Z goto resettoarm ; Throttle not idle/brake waitiszero decfsz armcnt,F goto waitforarm ; Arming period has expired, adjust the rearm timer for next time movlw rearmpulse movwf rearmcnt ; The brake is now off, time to throttle up usemotor pwmon 0 ; Turn off all FETs in interrupt code movwf GPIO ; Force FETs off anyway (brake -> motor switch) movlw throttlenonidle-1 movwf throttle ; Set throttle to lowest 'off' setting call updatepwm ; And set the PWM system pwmon 1< 0 btfsc escstatus, esc_LMAlos goto turnLMAon btfsc escstatus, esc_LMAreq goto turnLMAon ; LMA mode requested endif btfsc escstatus, esc_LOS goto offreset ; Update PWM for throttle, use slow accelerate and immediate reduction, ; although reduction is conditioned by a test to remove single pulse 'blips' ; of reduced throttle subwf throttle,W btfsc STATUS,Z goto samethrottle ; There is no change in throttle btfsc STATUS,C goto slowdown ; 1.2sec to get max indicates an increase of 1... if SLOWSTART incf throttle,F ; perform slow start else btfss upcnt,7 ; if upcnt >= 0 we are OK to increase goto speedup incfsz upcnt,F goto running speedup subwf throttle,F ; perform immediate throttle change endif call updatepwm movlw -downpulse movwf downcnt goto running slowdown btfss downcnt,7 ; if downcnt >= 0 we are OK to decrease goto doslow incfsz downcnt,F goto running doslow subwf throttle,F ; immediate drop in power level call updatepwm if !SLOWSTART movlw -uppulse ; configure for filtering upward change movwf upcnt endif goto running ; The throttle is constant (ie. this pulse is same as last pulse) ; If the throttle is at brake then we should switch the brake on, ; in order to cope with multiple throttle positions that are '0' ; we use the pwmmode and check for the 'int_0brake' case. samethrottle if BRAKE == 0 goto running ; No brake installed, just keep going else movfw pwmmode addlw -(int_0brake-int_refpoint) ; check for brake btfss STATUS,Z goto running ; Not at brake throttle, just keep going ; ; Check that brakeing is enabled if BRAKEENB > 0 btfss GPIO,brakeenb goto running ; Ignore braking endif ; ; The throttle is now at 'brake', and has been for the last 20ms or so ; start the brake... pwmon 1< 0 btfsc escstatus, esc_LMAlos goto turnLMAon btfsc escstatus, esc_LMAreq goto turnLMAon ; LMA mode requested endif btfsc escstatus, esc_LOS goto offreset ; lost signal, drop brake and reset if LMAFREQUENCY > 0 btfsc escstatus, esc_LMAreq goto turnLMAon ; LMA mode requested endif ; Keep the brake on while the throttle is at brake call g_pwmmode addlw -(int_0brake-int_refpoint) btfss STATUS,Z goto usemotor ; Braking is over if BRAKE > 1 incf throttle,w sublw throttlesteps-1 btfss STATUS,C goto braking ; Throttle is already >= throttlesteps-1 incf throttle,f call updatepwm ; Apply more braking throttle endif goto braking endif ; ; If we support reverse operation then this code manages the detection ; and the toggle of the reversing relay if REVERSE cblock revstatus ; Various reverser status conditions revcnt ; Count down to reverse endc ; Reverser status rev_ok equ 0 ; Reversing is permitted now reverser_run movfw pwmmode ; What is the current PWM operation - brake? addlw -(int_0brake-int_refpoint) btfsc STATUS,Z goto reverser_brk ; Brake is currently on ; Brake is off, so arm the reverser bsf revstatus, rev_ok movlw revpulse movwf revcnt ; rearm for the reversing count return reverser_brk ; Brake is on, see if we can reverse.... btfss revstatus, rev_ok return ; reverser is not armed... decfsz revcnt,F return ; not enough brake activity to reverse ; Looks good, time to reverse bcf revstatus, rev_ok bcf INTCON, T0IE ; Interrupts off for 4us - but only braking movlw 1< 0 bcf escstatus,esc_LMAreq movlw lmareqpulse movwf lmareqcnt endif ; Now we need to divide by the count per throttle position movfw cntperth movwf math_b call math_a_div_b ; compute A /= B ; Limit to the maximum throttle value movf math_rh,F btfss STATUS,Z retlw throttlesteps-1 ; throttle > 255 so must be at max movlw throttlesteps subwf math_rl,W btfsc STATUS,C retlw throttlesteps-1 ; throttle >= throttle steps so must be max ; Between 0 and throttlesteps-1 so just return it movfw math_rl return ; Check to see if the throttle is below the LMA point, the LMA turn on point isthlma if LMAFREQUENCY > 0 movfw lmanegl addwf math_al,W movfw lmanegh btfsc STATUS,C addlw 1 addwf math_ah,W btfsc STATUS,C goto notlma decfsz lmareqcnt,F retlw 0 ; Still waiting for enough requests bsf escstatus,esc_LMAreq retlw 0 ; LMA has been requested ; Clear LMA request notlma bcf escstatus,esc_LMAreq movlw lmareqpulse movwf lmareqcnt endif retlw 0 ; Definitely zero throttle, LMA not requested ; ; This subrouting returns when a valid RX pulse is seen. ; The return value is in the TMR1 registers.... ; Note caller must check the sts_validrx prior to proceeding, valid signal ; may have stopped, or low voltage has occured - low voltage is treated ; as loss of signal for the purposes of this code. ; Return value is between 0300h (0.768msec) and 09ffh (2.559msec) and ; represents a 'valid' sort of pulse. ; ; Local data cblock loscntL ; LOS counter low loscntH ; LOS counter high endc getrxpulse ; Arm the LOS information and glitch flag bcf escstatus, esc_rxglitch bcf escstatus, esc_LOS movlw (losms * (1000/12+1)) & 0FFh movwf loscntL movlw ((losms * (1000/12+1)) >> 8) + 1 movwf loscntH pulselp ; Rearm the R/C pulse counter clrf TMR1L ; start at 0 clrf TMR1H ; Enable Timer 1 for pulse width counting movlw 01000001b ; Enable movwf T1CON ; While there is nothing to do just stay here and check for LOS .... whileoff call checklos btfsc escstatus,esc_LOS retlw 0 movfw TMR1L iorwf TMR1H,W ; Anything on the counter? btfsc STATUS,Z goto whileoff ; Trigger a A/D voltage check on the lowvolts signal when we see the start ; of a receiver pulse if LOWVOLTS > 0 movlw 00000011b | (voltsense << 2) movwf ADCON0 ; Commence operation of the A/D endif ; Now wait for the pulse to end whileon call checklos btfsc escstatus,esc_LOS retlw 0 movlw 9 ; If counter >= 2.303 msec signal pulse is faulty subwf TMR1H,W btfsc STATUS,C goto toolong btfss GPIO,inrxbit goto whileon ; Stop timer so we can convert things clrf T1CON ; Check the A/D for low voltage... if LOWVOLTS > 0 btfsc ADCON0,1 goto itislos ; No A/D completion, stop motor... ; Perform averaging if required, else just get the AD value if lowvavgln2 > 0 call checklowv else movfw ADRESH endif ; Now consider the voltage... if LOWVOLTSALT > 0 btfsc GPIO,altlowvolts goto lowvnormal sublw (LOWVOLTSALT * 10 / (10+27)) / (1000/(255/5)) goto lowvtest lowvnormal endif sublw (LOWVOLTS * 10 / (10+27)) / (1000/(255/5)) lowvtest btfsc STATUS,C goto itislos ; Low voltage... endif ; Now the pulse has ended and we are ready to consider the outcome of this... ; First drop pulses that are too small to be valid, these must be an error... movlw 3 subwf TMR1H,W btfss STATUS,C goto hadglitch ; < 0.768msec if LMAFREQUENCY != 0 bcf escstatus, esc_LMAlos movlw lmaLOSms / losms movwf lmaloscnt endif return ; A pulse > 2.560msec is present, wait for it to end and ignore it toolong call checklos btfsc escstatus,esc_LOS retlw 0 btfss GPIO,inrxbit goto toolong hadglitch bsf escstatus, esc_rxglitch goto pulselp ; Watch for LOS, this routine is called about once every 12 cycles, ; or every 0.012msec and if 80msec expires then LOS is detected ; this requires a count of ~8000/12... ; ; Similarly the watchdog is cleared so that if the PIC doesn't get back to ; processing the next receiver pulse in 18msec the chip resets... ; checklos clrwdt ; track software is working... decfsz loscntL,F return decfsz loscntH,F return bsf escstatus, esc_rxglitch if LMAFREQUENCY != 0 decfsz lmaloscnt,F goto itislos bsf escstatus, esc_LMAlos endif itislos bsf escstatus,esc_LOS ; Flag LOS return ; ; If low voltage averaging is active then compute and return the average if lowvavgln2 > 0 cblock lvt_l ; Low Voltage Total LSB lvt_h ; Low Voltage Total MSB endc checklowv ; Shift down the averaging registers i = (1<= 1 movfw lowvoltavg+i-1 movwf lowvoltavg+i i -= 1 endw ; Grab the current A/D value and put it the the most recent slot movfw ADRESH movwf lowvoltavg ; Add up all the values to form a total movwf lvt_l clrf lvt_h i = 1 while i < (1<