;======================================================================================= ; Speed Controller With Full Bridge Output ;======================================================================================= ; Bruce Abbott bhabbott@paradise.net.nz ; ; for Microchip PIC12F629 or PIC12F675 ; ; Features: ; ; - Bi-directional control of brushed DC motor ; - Neutral throttle detected at 1.5mS or 1.37mS ; - LVC proportional to initial battery voltage PROCESSOR PIC12F675 INCLUDE radix dec ;#DEFINE SIMULATE ; for debugging only! #DEFINE SLOW ; rate-limited throttle response #define VERSION "=BRIDGE===V0.1==" ; ===================================================================================== ; Configuration is: ; Master Clear pin is disabled (used as input) ; Code Protection is OFF ; Watchdog Timer is ON ; Oscillator is Internal RC __config _MCLRE_OFF & _CP_OFF & _WDT_ON & _INTRC_OSC_NOCLKOUT ; GPIO register bits RevLoBit EQU 0 ; GP0, pin 7. Turns on motor reverse FET BattBit EQU 1 ; GP1, pin 6. Battery status input ForLoBit EQU 2 ; GP2, pin 5. Turns on motor forward FET ServoBit EQU 3 ; GP3, pin 4. Servo pulse input RevHiBit EQU 4 ; GP3, pin 3. Turns on high-side reverse FET ForHiBit EQU 5 ; GP4, pin 2. Turns on high-side forward FET ; TRIS register value TrisBits EQU (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 PWM Cycle ; ; Takes 4 cycles to execute ; StartPWM MACRO movf MotorON,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 cycles. ; DoServoCount MACRO btfsc GPIO, ServoBit incf ServoCount, F ENDM ;================================================================================== ; RESET ;================================================================================== ; Coldstart: ORG 0 goto Start org 0x8 dt VERSION ;================================================================================= ; Throttle Curve Table ;================================================================================ ; Returns with PWM width and Reverse Flag (bit 7) in W. ; Takes 5 CPU cycles to execute (including call and retlw). ; ; There are two tables. The second table is accessed by adding 75 to W. ; ; Table1:- Has equal forward and backward range, for stick with neutral at 1.5mS ; Table2:- More forward than reverse range, for stick with neutral at 1.37mS ; Table: addwf PCL,F ; computed goto, W = 0 to 150 ; Table 1 retlw 128+43 ; 0 reverse max at 1.2mS retlw 128+42 ; 1 retlw 128+42 ; 2 retlw 128+41 ; 3 retlw 128+41 ; 4 retlw 128+40 ; 5 retlw 128+40 ; 6 retlw 128+39 ; 7 retlw 128+38 ; 8 retlw 128+37 ; 9 retlw 128+36 ;10 retlw 128+35 ;11 retlw 128+34 ;12 retlw 128+33 ;13 retlw 128+32 ;14 retlw 128+31 ;15 retlw 128+30 ;16 retlw 128+29 ;17 retlw 128+28 ;18 reverse retlw 128+26 ;19 retlw 128+24 ;20 retlw 128+22 ;21 retlw 128+20 ;22 retlw 128+18 ;23 retlw 128+16 ;24 retlw 128+14 ;25 retlw 128+12 ;26 retlw 128+10 ;27 retlw 128+8 ;28 retlw 128+6 ;29 retlw 128+4 ;30 retlw 128+2 ;31 retlw 0 ;32 } retlw 0 ;33 } retlw 0 ;34 } retlw 0 ;35 } retlw 0 ;36 } retlw 0 ;37 } STOP at 1.5mS retlw 0 ;38 } retlw 0 ;39 } retlw 0 ;40 } retlw 0 ;41 } retlw 0 ;42 } retlw 2 ;43 retlw 4 ;44 retlw 6 ;45 retlw 8 ;46 retlw 10 ;47 retlw 12 ;48 retlw 14 ;49 retlw 16 ;50 retlw 18 ;51 retlw 20 ;52 retlw 22 ;53 retlw 24 ;54 retlw 26 ;55 forward retlw 27 ;56 retlw 28 ;57 retlw 29 ;58 retlw 30 ;59 retlw 31 ;60 retlw 32 ;61 retlw 33 ;62 retlw 34 ;63 retlw 35 ;64 retlw 36 ;65 retlw 37 ;66 retlw 38 ;67 retlw 39 ;68 retlw 40 ;69 retlw 40 ;70 retlw 41 ;71 retlw 41 ;72 retlw 42 ;73 retlw 42 ;74 retlw 43 ;75 forward max at 1.8mS ; Table 2 retlw 128+30 ; 0 reverse max at 1.2mS retlw 128+29 ; 1 retlw 128+28 ; 2 retlw 128+27 ; 3 retlw 128+26 ; 4 retlw 128+25 ; 5 retlw 128+24 ; 6 retlw 128+22 ; 7 retlw 128+20 ; 8 retlw 128+18 ; 9 reverse retlw 128+16 ;10 retlw 128+14 ;11 retlw 128+12 ;12 retlw 128+8 ;13 retlw 128+6 ;14 retlw 128+4 ;15 retlw 0 ;16 } retlw 0 ;17 } retlw 0 ;18 } retlw 0 ;19 } retlw 0 ;20 } retlw 0 ;21 } STOP at 1.37mS retlw 0 ;22 } retlw 0 ;23 } retlw 0 ;24 } retlw 0 ;25 } retlw 0 ;26 } retlw 2 ;27 retlw 4 ;28 retlw 6 ;29 retlw 8 ;30 retlw 10 ;31 retlw 12 ;32 forward retlw 14 ;33 retlw 16 ;34 retlw 18 ;35 retlw 20 ;36 retlw 22 ;37 retlw 23 ;38 retlw 24 ;39 retlw 25 ;40 retlw 26 ;41 retlw 27 ;42 retlw 28 ;43 retlw 29 ;44 retlw 30 ;45 retlw 30 ;46 retlw 31 ;47 retlw 31 ;48 retlw 32 ;49 retlw 32 ;50 retlw 33 ;51 retlw 33 ;52 retlw 34 ;53 retlw 34 ;54 retlw 35 ;55 retlw 35 ;56 retlw 36 ;57 retlw 36 ;58 retlw 37 ;59 retlw 37 ;60 retlw 38 ;61 retlw 38 ;62 retlw 39 ;63 retlw 39 ;64 retlw 40 ;65 retlw 40 ;66 retlw 40 ;67 retlw 41 ;68 retlw 41 ;69 retlw 41 ;70 retlw 42 ;71 retlw 42 ;72 retlw 42 ;73 retlw 42 ;74 retlw 43 ;75 forward max at 1.8mS ;---------------------------------------------------------------------------------- ; Battery Voltage Cutoff Table ;---------------------------------------------------------------------------------- CutoffTable: addwf PCL,F ; BATTVOLTS CUTOFF BATTERY TYPE retlw 3 ; 0 4.0V retlw 3 ; 1 4.5V retlw 3 ; 2 5.0V retlw 3 ; 3 5.5V retlw 3 ; 4 6.0V 5.5V 6V SLA retlw 3 ; 5 6.5V 5.5V 6V SLA retlw 4 ; 6 7.0V 6.0V 7.2V Nicad (6 cells) retlw 4 ; 7 7.5V 6.0V 7.2V Nicad (6 cells) retlw 4 ; 8 8.0V 6.0V 7.2V Nicad (6 cells) retlw 6 ; 9 8.5V 7.0V 8.4V Nicad (7 cells) retlw 6 ; 10 9.0V 7.0V 8.4V Nicad (7 cells) retlw 6 ; 11 9.5V 7.0V 8.4V Nicad (7 cells) retlw 8 ; 12 10.0V 8.0V 9.6V Nicad (8 cells) retlw 8 ; 13 10.5V 8.0V 9.6V Nicad (8 cells) retlw 8 ; 14 11.0V 8.0V 9.6V Nicad (8 cells) retlw 14 ; 15 11.5V 11.0V 12V SLA ;================================================================================== ; Measure Battery Voltage ;================================================================================== ; BattVolts: movlw 15 movwf Volts ; start comparison at maximum Volts bv_next: bsf STATUS,RP0 movf Volts,w movwf VRCON ; set Vref divider to Volts bsf VRCON,VREN ; enable Vref bcf STATUS,RP0 NO_OP 4 ; wait 5uS btfss CMCON,COUT ; Battery voltage higher than VRef? goto bv_done ; yes, done decfsz Volts ; no, try lower Vref goto bv_next bv_done: return ;================================================================================== ; Measure servo pulse width ;================================================================================== ; Measure: movlw PRECHARGE movwf ServoCount ; start servocount at -0.9mS bcf Flags,BadPulseBit Measure1: clrwdt btfss GPIO,ServoBit ; wait for start of pulse goto Measure1 Measure2: clrwdt ;(1)> nop ;(1)> measuring first 0.9mS incf ServoCount,F ;(1)> skpnz ;(2)> done 0.9mS ? goto Measure3 ;(+1) btfsc GPIO,ServoBit ;(1)> pulse finished early ? goto Measure2 ;(2)> MeasureError: clrf ServoCount ; bad if pulse < 0.9mS goto MeasureDone Measure3: clrwdt ;(1)] nop ;(1)] measuring to end of pulse incf ServoCount,F ;(1)] skpnz ;(2)] bad if pulse > 2.9mS goto MeasureError ; btfsc GPIO,ServoBit ;(1)] done if end of pulse goto Measure3 ;(2)] MeasureDone: movf ServoCount,w return ; ================================================================================ ; PWM pulse subroutine. 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 PwmCount register reaches zero. ; ; takes 350 CPU cycles. ; ; Minimum ON time is 27uS ; 2uS inside StartPWM + 18uS + 7uS inside DoPwmPulse. ; ; Minimum OFF time is 13uS ; 7uS inside DoPwmPulse + 4uS + 2uS inside StartPWM. ; ; ; 2,2 18 7 336 cycles 7 4 2,2 ; ____________________ _______ ; _____/ \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\___________/ ; ON OFF at (PwmCount-1) * 8uS ON ; (startPWM) |<--------- DoPwmPulse -------->| (startPWM) ; ; DoPwmPulse: movlw PWMLOOPS ;(1) set PWM period counter movwf PwmPeriod ;(1) movf MotorOFF,W ;(1) get GPIO value for motor off pwmloop: decf PwmCount,F ;(1) skpnz ;(1) IF end of PWM high time movwf GPIO ;(1) THEN turn off motor IFDEF SIMULATE nop nop ELSE btfsc CMCON,COUT ;(2) goto BattLoShutdown ; quit if battery low ENDIF decfsz PwmPeriod,F ;(2) goto pwmloop ;(+1) retlw 0 ;(2) done ;------------------------------------------------------------------------------------ ; END OF SUBROUTINES ;------------------------------------------------------------------------------------ ORG 256 ; Retrieve and set the factory-programmed oscillator calibration value. Start: call 0x3ff bsf STATUS,RP0 ; Register bank 1 movwf OSCCAL ; set oscillator frequency movlw b'00000010' movwf ANSEL ; Analog I/O on GP1 bcf STATUS,RP0 ; register bank 0 movlw b'00000100' movwf CMCON ; Comparator in- to GP1, in+ to VREF ; ; Move the prescaler from tmr0 to the watchdog, without accidental resets. ; clrwdt clrf TMR0 movlw OptionBits | 7 option clrwdt movlw OptionBits option clrwdt ; Initialise GPIO register ; clrf GPIO ; all outputs will be low movlw TrisBits tris GPIO ; set I/O directions InitDelay: clrf PwmPeriod InitDelay1: clrf temp InitDelay2: clrwdt NO_OP 4 ; wait 0.5s for peripherals to stabilize decfsz temp goto InitDelay2 decfsz PwmPeriod goto InitDelay1 ; set battery cutoff voltage call BattVolts ; get battery voltage movf Volts,w call CutoffTable ; calculate cutoff voltage bsf STATUS,RP0 ; register bank 0 movwf VRCON ; set VRef voltage for cutoff bsf VRCON,VREN ; enable VRef bcf STATUS,RP0 ; register bank 0 clrf Flags ; clear memory Rearm: clrwdt IFNDEF SIMULATE btfsc CMCON,COUT ; wait for good battery voltage goto Rearm ENDIF ; ; Wait for a whole servo pulse before looking for arm pulses ; RaWaitOn: clrwdt btfss GPIO, ServoBit goto RaWaitOn RaWaitOff: btfsc GPIO, ServoBit goto RaWaitOff ; ; wait until throttle is in neutral position ; RaStartCount: movlw ArmPulsesReqd movwf ArmPulseCount ; set required number of good pulses bcf Flags,Stop1 bcf Flags,Stop2 RaNextPulse: call Measure ; measure servo pulse width skpnz ; bad pulse ? goto RaStartCount movwf PrevCount ; remember servo pulse movwf Index movlw 38 subwf Index,F skpc ; minimum = 1.2mS clrf Index movlw 75 subwf Index,W ; maximum = 1.8mS (index range: 0 to 75) skpnc subwf Index,F btfsc Flags,UseTable2 ; using throttle table 2 ? goto RaTable2 ; yes, check for table 2 STOP clrf PCLATH movf Index,w call Table xorlw 0 ; Table1 STOP ? skpnz goto RaStop1 btfsc Flags,UseTable1 ; using thottle table 1 ? goto RaStartCount ; no, stick not at neutral RaTable2: clrf PCLATH movf Index,w addlw 76 call Table xorlw 0 ; Table 2 STOP ? skpz goto RaStartCount ; no, stick not at neutral RaStop2: bsf Flags,Stop2 ; now in Table 2 STOP region btfss Flags,Stop1 ; was last pulse in Table 1 STOP region ? goto RaCount ; no, continue bcf Flags,Stop1 goto RaStartCount ; yes, restart RaStop1: bsf Flags,Stop1 ; now in table 1 STOP region btfss Flags,Stop2 ; was last pulse in Table 2 STOP region ? goto RaCount ; no, continue bcf Flags,Stop2 goto RaStartCount ; yes, restart RaCount: decfsz ArmPulseCount,F ; Do we have enough arm pulses? goto RaNextPulse ; Not yet. ; ; Set throttle table to use ; btfsc Flags,Stop1 bsf Flags,UseTable1 btfsc Flags,Stop2 bsf Flags,UseTable2 ; ; Play a 2KHz 'beep' sound for 1/8 second, to indicate successful arming. ; (uses the motor as a speaker!) ; PlayBeep: clrf PwmCount beep1: movlw ON_FORWARD movwf GPIO ; motor on movlw 5 movwf PwmPeriod beep2: clrwdt ; for 22uS decfsz PwmPeriod,F goto beep2 movlw OFF_FORWARD movwf GPIO ; motor off movlw 128 movwf PwmPeriod beep3: clrwdt decfsz PwmPeriod,F goto beep3 ; for 0.5mS decfsz PwmCount,F goto beep1 ; 256 cycles ; wait for end of next servo pulse servo_hi: btfss GPIO,ServoBit goto servo_hi servo_lo: btfsc GPIO,ServoBit goto servo_lo ; ready to go! clrf MotorON ; Motor FETs OFF clrf MotorOFF ; Direction FETs OFF clrf PwmWidth ; PWM=0 movlw PRECHARGE movwf ServoCount ; precharge ServoCount goto MainPwmEntry ; ================================================================================== ; The main PWM loop ; ; Two timing loops have to be simultaneously maintained:- ; ; 1/ Measure servo pulse width every 8uS. DoServoCount takes 2 CPU ; cycles, thus 6 CPU cycles are available between each instance. ; ; 2/ StartPWM + 18 cycles + DoPwmPulse + 4 cycles etc. This works ; out to 372 CPU cycles, creating a PWM frequency of 2.7Khz. ; ; The watchdog timer is cleared only AFTER we have received a good servo ; pulse. Loss of signal will cause the PIC to reset itself! ; ; MainPwmLoop: DoServoCount ;(2) <--- } movlw PWMLOOPS ;(1) } movwf PwmPeriod ;(1) } movf MotorOFF,W ;(1) } pwmpulseloop: decf PwmCount,F ;(1) } skpnz ;(1) } <=== Complete PWM cycle. movwf GPIO ;(1) } Also measure servo DoServoCount ;(2) <--- } pulse width. decfsz PwmPeriod,F ;(2) } goto pwmpulseloop ;(+1) } (takes 342 CPU cycles) IFDEF SIMULATE nop nop ELSE btfsc CMCON,COUT ;(2) } goto BattLoShutdown ; quit if battery low ENDIF NO_OP 2 ;(2) DoServoCount ;(2) <--- MainPwmEntry: StartPWM ;(4) ====> start 1st PWM cycle NO_OP 2 ;(2) DoServoCount ;(2) <--- btfsc GPIO, ServoBit ;(2) goto ServoIsHigh ;(+1) Is servo pulse High ? btfsc Flags, PriorServoBit ;(2) goto EndOfPulse ;(+1) end of servo pulse ? movlw PRECHARGE ;(1) movwf ServoCount ;(1) pre-charge servocount DoServoCount ;(2) <--- clrf HiCycles ;(1) NO_OP 3 ;(1) goto MainPwmLoop ;(2) ServoIsHigh: bsf Flags, PriorServoBit ;(1) servo pulse is High! NO_OP 2 ;(2) DoServoCount ;(2) <--- incf HiCycles,F ;(1) Count PWM cycles while servo is high skpnz ;(1) decf HiCycles,F ;(1) avoid HiCycles overflow NO_OP 1 ;(1) goto MainPwmLoop ;(2) ; ============================================================================ ; End of Servo Pulse. We don't have to measure the servo pulse width now, but ; we still have to maintain the PWM cycle:- ; ; StartPWM + 18 clocks + DoPwmPulse + 4 clocks + StartPWM etc. ; ; ; gets here 9 clocks after start of 1st PWM EndOfPulse: bcf Flags,BadPulseBit ;(1) assume servo pulse is OK movlw 2 ;(1) subwf HiCycles,W ;(1) short pulse ? (less than 3 PWM's) skpc ;(1) bsf Flags,BadPulseBit ;(1) movlw 7 ;(1) subwf HiCycles,W ;(1) long pulse ? (more than 6 PWM's) skpnc ;(1) bsf Flags,BadPulseBit ;(1) call DoPwmPulse ;(350) <=== Complete 1st PWM cycle movlw 163 ;(1) subwf ServoCount,W ;(1) servo pulse width in-range ? skpnc ;(1) ( 0.9mS < pulse < 2.2mS) bsf Flags,BadPulseBit ;(1) StartPWM ;(4) ====> Start 2nd PWM cycle btfsc Flags,BadPulseBit ;(2) ignore bad pulses goto BadPulse ;(+1) IFDEF SLOW ; change-rate limited throttle movf ServoCount,w ;(1) compare throttle to last position subwf PrevCount,w ;(1) skpc ;(2) going up ? goto ThrottleUp ;(+1) skpnz ;(2) going down ? goto NoChange ;(+1) ThrottleDown: addlw -2 ;(1) skpnc ;(1) addwf ServoCount ;(1) servocount = prevcount - 2 NO_OP 3 ;(3) goto Slowed ;(2) NoChange: NO_OP 5 ;(5) goto Slowed ;(2) ThrottleUp: addlw 2 ;(1) skpc ;(1) addwf ServoCount ;(1) servocount = prevcount + 2 NO_OP 6 ;(6) Slowed: movf ServoCount,w ;(1) movwf Index ;(1) ELSE ; mean-averaged throttle movf ServoCount,W ;(1) movwf Index ;(1) Index = ServoCount (0-163) movf PrevCount,W ;(1) addwf Index,F ;(1) rrf Index,F ;(1) Index = Average(ServoCount+PrevCount) NO_OP 11 ;(11) ENDIF call DoPwmPulse ;(350) <=== Complete 2nd PWM cycle NO_OP 4 ;(4) StartPWM ;(4) ====> Start 3rd PWM cycle movlw 38 ;(1) subwf Index,F ;(1) skpc ;(1) minimum = 1.2mS clrf Index ;(1) movlw 75 ;(1) subwf Index,W ;(1) maximum = 1.8mS skpnc ;(1) subwf Index,F ;(1) movlw 76 ;(1) btfsc Flags,UseTable2 ;(1) using second throttle table ? addwf Index ;(1) clrf PCLATH ;(1) required for simulation only! NO_OP 6 ;(6) call DoPwmPulse ;(350) <=== Complete 3rd PWM cycle NO_OP 4 ;(4) StartPWM ;(4) ====> Start 4th PWM cycle movf Index,W ;(1) call Table ;(5) do table lookup movwf PwmWidth ;(1) btfsc PwmWidth,7 ;(2) reverse flag set ? goto reversing ;(+1) tstf PwmWidth ;(1) if PWM Width = 0 skpnz ;(2) then stop goto stopping ;(+1) movlw ON_FORWARD ;(1) MotorON set for Forward movwf MotorON ;(1) movlw OFF_FORWARD ;(1) store new MotorOFF in temp (must movwf temp ;(1) complete this PWM with old MotorOFF) goto oldpwm ;(2) reversing: bcf PwmWidth,7 ;(1) remove reverse flag from PwmWidth movlw ON_REVERSE ;(1) MotorON set for Reverse movwf MotorON ;(1) movlw OFF_REVERSE ;(1) store new MotorOFF for Reverse movwf temp ;(1) NO_OP 1 ;(1) goto oldpwm ;(2) stopping: movlw ON_STOP ;(1) MotorON set for Stop movwf MotorON ;(1) movlw OFF_STOP ;(1) Store new MotorOFF for Stop movwf temp ;(1) NO_OP 1 ;(1) oldpwm: call DoPwmPulse ;(350) <=== Complete 4th PWM cycle movf temp,W ;(1) xorwf MotorOFF,F ;(1) are new MotorOFF bits same as old ? skpz ;(1) if not, we are changing direction movwf GPIO ;(1) so turn motor OFF now! StartPWM ;(4) ====> Start next PWM cycle movf temp,W ;(1) movwf MotorOFF ;(1) set new Motor_OFF movf ServoCount,W ;(1) movwf PrevCount ;(1) remember this servo pulse NO_OP 2 ;(2) clrwdt ;(1) Good pulse, so clear watchdog BadPulse: bcf Flags,BadPulseBit ;(1) movlw PRECHARGE ;(1) movwf ServoCount ;(1) precharge ServoCount bcf Flags,PriorServoBit ;(1) get ready for next servo pulse clrf HiCycles ;(1) '' NO_OP 6 ;(6) call DoPwmPulse ;(350) <=== Complete PWM cycle NO_OP 2 ;(2) goto MainPwmEntry ;(2) ; ; Low battery - shutdown and wait for arming. ; BattLoShutdown: movf MotorOFF,W movwf GPIO ; Stop the motor goto Rearm ; restart org 0x3ff RETLW 0x80 ; OSCCAL value programmed at factory END