; ============================================================================= ; ICS-100Li Electronic Speed Controller ; ============================================================================= ; Bruce Abbott (bhabbott@paradise.net.nz) ; ; Why you might want this code:- ; ; 1. Replace blown PIC chip in your ICS/GS-100li (also works with ICS-50li). ; 2. Enhanced features (exponential throttle, arming beeps, glitch filter). ; ; NOTE: Cutoff voltage is set to 5.5V/8.3V (same as the original). For ; 6V/9V cutoff, change the 11k resistor (05C) to 10k. ; ; Summary of Changes: ; ; 2008-4-12 V1.0 first release ; 2008-11-29 V1.1 added diagnostic output (GP5 stays high if signal was lost) ; enable the next line only if CPU is NOT specified in assembly options ; PROCESSOR PIC12F675 INCLUDE radix dec ; default number format is decimal errorlevel 0,-305,-302 ; disable unimportant warnings version = 1 revision = 1 ; ========================================================================== ; Configuration is: ; Master clear pin disabled (used as input) ; Code protection OFF ; Watchdog timer ON ; Oscillator is Internal RC __config _MCLRE_OFF&_CP_OFF&_WDT_ON &_BODEN_ON&_INTRC_OSC_NOCLKOUT ; Bit definitions for I/O registers #DEFINE BattBit 0 ; pin 7 battery voltage sense #DEFINE BrakeBit 1 ; pin 6 Brake FET on when high #DEFINE RunBit 2 ; pin 5 Motor FET on when high #DEFINE ServoBit 3 ; pin 4 servo pulse input #DEFINE LedBit 4 ; pin 3 Status LED (optional) #DEFINE DiagBit 5 ; pin 2 Diagnostic LED (optional) #DEFINE TrisBits (1<1 goto $+1 ; 2 clocks NO_OP_COUNT SET NO_OP_COUNT-2 ENDW IF NO_OP_COUNT nop ; 1 clock ENDIF ENDM ; ========================================================================= ; Macro to start a PWM cycle ; DoRunBit MACRO movf PortBits,w movwf GPIO movf PwmWidth,w movwf PwmCount ENDM ; ========================================================================= ; Macro to keep up with the servo pulse duration ; The code in this Macro uses 2 clocks. ; DoServoCount MACRO btfsc GPIO, ServoBit incf ServoCount, F ENDM ; =============================== CODE ==================================== ; ; NOTE: Critical instruction timings are shown in brackets ; eg. ;(2) = 2 microseconds ; ; Coldstart: ORG 0 ; jump to high ROM area (low area needed for subroutines) goto Start ; version string ORG 8 dt "ICS100Li>>V" dt version+48 dt "." dt revision+48 dt "<<" ; ======================================================================== ; The main PWM loop ; ; Two timing loops have to be maintained simultaneously:- ; ; 1/ Increment servo pulse width counter every 7 clocks (if servo pulse is high). ; DoServoCount takes 2 clocks, leaving 5 clocks available for other processing. ; ; 2/ DoRunBit + 17 clocks + DoPwmPulse + 2 clocks etc. This creates the PWM ; cycle time of 329 clocks, corresponding to a PWM frequency of 3Khz. ; ; NOTE: DoServoCount occurs 43 times inside DoPwmPulse. ; ; ServoIsHigh: bsf Flags, PriorServoBit ;(1) servo pulse has started! DoServoCount ;(2) measure servo pulse width NO_OP 2 ;(2) incf HiCount,F ;(1) HiCount = ServoCount/47 skpnz ;(1) Limit HiCount to 255 max decf HiCount,F ;(1) MainPwmLoop: DoServoCount ;(2) measure servo pulse width NO_OP 1 ;(1) call DoPwmPulse ;(4,2,5) complete PWM cycle DoServoCount ;(2) measure servo pulse width MainPwmEntry: DoRunBit ;(4) start PWM cycle nop ;(1) DoServoCount ;(2) measure servo pulse width movwf PwmCount ;(1) btfsc GPIO, ServoBit ;(2) is servo pulse high ? goto ServoIsHigh ;(+1) btfsc Flags, PriorServoBit ;(2) Servo pulse now low, but was it high? goto EndOfPulse ;(+1) yes, then must be end of pulse clrf ServoCount ;(1) no, so reset servo pulse count DoServoCount ;(2) measure servo pulse width NO_OP 2 ;(2) goto MainPwmLoop ;(2) ; ============================================================================ ; End of Servo Pulse. We don't have to measure the servo pulse now, but we ; still have to maintain the PWM cycle timing! ; ; DoRunBit, +18 cycles, DoPwmCycle, +2 cycles, DoRunBit etc. ; ; Starting 9 clocks after the last DoRunBit ; EndOfPulse: movfw ServoCount ;(1) movwf ServoPulse ;(1) save servo pulse count movlw 3 ;(1) subwf HiCount,W ;(1) HiCount < 3 ? skpc ;(1) bsf Flags,PulseBad ;(1) servo pulse may be too short! skpc ;(1) btfsc ServoPulse,7 ;(1) servo count >= 128 ? bcf Flags,PulseBad ;(1) yes, pulse OK call DoPwmPulse ;<<< NO_OP 2 ;(2) DoRunBit ;>>> nop ;(1) movwf PwmCount ;(1) movlw 8 ;(1) subwf HiCount,W ;(1) HiCount >= 8 PWM cycles ? skpnc ;(1) bsf Flags,PulseBad ;(1) servo pulse is too long! clrf HiCount ;(1) reset HiCount movlw 128 ;(1) min servo pulse width = 0.9mS subwf ServoPulse ;(1) movlw 186 ;(1) max servo pulse width = 2.2mS subwf ServoPulse,W ;(1) skpnc ;(1) pulse width in range ? bsf Flags,PulseBad ;(1) btfsc Flags,PulseBad ;(2) ignore bad servo pulse! goto Bad_Pulse ;(+1) movfw LastPulse ;(1) calculate average of count and... addwf ServoPulse ;(1) ...last count, to reduce jitter. rrf ServoPulse ;(1) average = (count + last) / 2 call DoPwmPulse ;<<< NO_OP 2 ;(2) DoRunBit ;>>> still using old PWM movf ServoPulse, W ;(1) call ServoLookup ;(7) throttle curve lookup table movwf Command ;(1) andlw b'00111111' ;(1) isolate PWM value movwf PwmWidth ;(1) set new PWM width skpnz ;(1) bcf PortBits,RunBit ;(1) if PWM = 0 then Motor off btfsc Command,6 ;(2) Braking? goto Braking ;(+1) No_Brake: bcf PortBits,BrakeBit ;(1) Turn the brake OFF NO_OP 2 ;(2) call DoPwmPulse ;<<< still using old PWM movlw BRAKEDELAY ;(1) movwf BrakeDelayCount ;(1) set brake delay DoRunBit ;>>> skpz ;(1) bsf PortBits,RunBit ;(1) if PWM > 0 then Motor on goto Good_Pulse ;(2) ; ; Here the brake is to be turned on. Let the motor slow down first. PWM ; timing isn't critical while braking, because the motor is always off. ; Braking: bcf PortBits,RunBit ; will stop the motor movf PortBits,w movwf GPIO decfsz BrakeDelayCount ; Time to turn on the Brake? goto Good_Pulse ; No, wait for another servo pulse Brake_On: incf BrakeDelayCount ; Yes, keep brake on bsf PortBits, BrakeBit ; will turn the brake FET on movf PortBits,w movwf GPIO ; Motor FET off, Brake FET on/off Good_Pulse: movf ServoPulse,W ;(1) remember good servo pulse movwf LastPulse ;(1) tstf BadPulses ;(1) another good pulse :) skpz ;(1) decf BadPulses ;(1) Next_Pulse: NO_OP 6 ;(6) clrwdt ;(1) reset watchdog timer bcf Flags, PriorServoBit ;(1) servo pulse is finished clrf ServoCount ;(1) reset servo pulse count call DoPwmPulse ;<<< goto MainPwmEntry ;(2) DoRunBit is next ; ; Servo pulse bad, so just keep the existing PWM going. ; Don't remember the bad pulse. ; Bad_Pulse: NO_OP 2 ;(2) call DoPwmPulse ;<<< still using old PWM NO_OP 1 ;(1) bcf Flags,PulseBad ;(1) DoRunBit ;>>> still using old PWM NO_OP 2 ;(2) movwf PwmCount ;(1) incf BadPulses ;(1) another bad pulse :( movlw MAXBADPULSES ;(1) subwf BadPulses,W ;(1) too many bad pulses ? skpc ;(2) if so then shut down goto Next_Pulse ;(+1) else continue ; ; No Signal - shutdown with brake OFF and wait for arming ; No_Signal: bcf PortBits, RunBit ; will stop the Motor bcf PortBits, BrakeBit ; will turn the Brake off bcf PortBits, LedBit ; will turn status LED off bsf PortBits, DiagBit ; will turn diagnostic LED on movf PortBits,w movwf GPIO goto Rearm ; ; Low Battery - shutdown with brake ON and wait for arming. ; Low_Batt: bcf PortBits, RunBit ; will stop the Motor bcf PortBits, LedBit ; will turn status LED off movf PortBits,w movwf GPIO bsf PortBits, BrakeBit ; will turn the Brake on. movf PortBits,w movwf GPIO goto Rearm ;--------------------------------------------------------------------------- ; Play 'Beep' sound through the motor ;--------------------------------------------------------------------------- Beep: clrf PwmCount beep1: bsf GPIO,RunBit ; motor on NO_OP 13 bcf GPIO,RunBit ; motor off movlw 60 movwf PwmWidth beep2: clrwdt decfsz PwmWidth,F ; wait 0.24mS goto beep2 movlw 60 movwf PwmWidth beep3: clrwdt decfsz PwmWidth,F ; wait 0.24mS goto beep3 decfsz PwmCount,F goto beep1 ; do 256 cycles (128mS) retlw 0 ;--------------------------------------------------------------------------- ; Pause W x 1mS ;--------------------------------------------------------------------------- ; Pause128: movlw 128 ; ; Input: W = milliseconds ; Pause: movwf PwmCount pause_1 movlw 249 movwf HiCount pause_2 clrwdt ; clear the watchdog timer!!! decfsz HiCount goto pause_2 decfsz PwmCount goto pause_1 retlw 0 ; ========================================================================== ; Servo pulse to command converter. Returns with command in W. ; See speed400.inc for the table at 0x200 which is being referenced ; This routine takes 7 clocks to execute. ; ServoLookup: bsf PCLATH,1 ; = 0x02 (code page 2) movwf PCL ; goto 0x200+W ; ========================================================================== ; The main PWM pulse routine. On entry, the motor may be on or off. ; If the motor is off, it will never be turned on. However, it will be ; turned off when (if) the PWM_Count register reaches zero. ; DoPwmPulse: movf PortBits,w andlw ~(1< 9V ? goto Rearm twocells: bcf Flags,ThreeCells bsf STATUS,RP0 ; register bank 1 movlw b'10100101' ; Voltage Reference = 1.04V (6V cutoff) movwf VRCON bcf STATUS,RP0 ; register bank 0 Rearm: clrwdt btfss CMCON,COUT ; Don't leave if low voltage goto Rearm ; ; Wait for a whole servo pulse before looking for arming pulses ; RaLoop1: clrwdt btfss CMCON,COUT ; restart if battery low goto Rearm btfss GPIO, ServoBit ; wait for High goto RaLoop1 RaLoop2: btfss CMCON,COUT ; restart if battery low goto Rearm btfsc GPIO, ServoBit ; wait for Low goto RaLoop2 movlw ArmPulsesReqd movwf ArmPulseCount RaLoop3: clrf ServoCount clrwdt ; ; Measure length of the next servo pulse ; RaLoop4: btfss CMCON,COUT ; restart if low battery voltage goto Rearm btfss GPIO, ServoBit ; wait until servo pulse is high goto RaLoop4 RaLoop5: incf ServoCount ;(1) measure servo pulse width skpnz ;(2) goto Rearm ; restart if > 255 NO_OP 1 ;(1) 7uS per count btfsc GPIO,ServoBit ;(1) goto RaLoop5 ;(2) wait until pulse is low ; accept low throttle only clrwdt movlw 128 ; min count = 0.9mS subwf ServoCount,F movlw 43 ; max count = 1.2mS subwf ServoCount,W ; is this a legal arming pulse? skpnc goto RaLoop1 ; No - start over decfsz ArmPulseCount,F ; Do we have enough arm pulses? goto RaLoop3 ; Not yet. ; ; Play 'beep' sounds through the motor, indicating successful arming. ; btfss Flags,ThreeCells goto twobeeps call Beep ; 3 beeps for 3S call Pause128 twobeeps: call Beep call Pause128 ; 2 beeps for 2S call Beep ; ; wait for end of the next servo pulse. ; servo_hi: btfss GPIO,ServoBit ; wait for start of pulse goto servo_hi servo_lo: btfsc GPIO,ServoBit ; wait for end of pulse goto servo_lo Armed: clrf PwmWidth ; PWM = 0 (motor off) clrf ServoCount clrf HiCount clrf LastPulse clrf BadPulses bcf Flags,PulseBad bcf Flags,PriorServoBit bcf Flags,BattLow clrf PortBits bsf PortBits,LedBit ; will turn status LED on ; Arming complete, jump to main operating loop. The motor is off, and ; will stay off until the next servo pulse has been processed. goto MainPwmLoop ; ; This subroutine loads the OSCCAL value into W. It is normally ; pre-programmed by Microchip. ; org 0x3ff retlw 0x48 ; OSCCAL value END