#INCLUDE "P16F628A.INC"
#DEFINE PB1 PORTA,0
#DEFINE PB2 PORTA,1
#DEFINE PB3 PORTA,2
#DEFINE SW PORTA,3
ORG 0X000
GOTO MAIN
MAIN
CALL SETUP
CALL SETUP_PWM_1
GOTO LOOP
LOOP
BTFSC PB1
CALL MOTOR_STOP
BTFSC PB2
CALL MOTOR_LEFT
BTFSC PB3
CALL MOTOR_RIGHT
BTFSC SW
CALL HIGH_SPEED
BTFSC SW
CALL HALF_SPEED
GOTO LOOP
MOTOR_STOP
CLRF CCPR1L
BCF PB1
BCF PB2
RETURN
MOTOR_LEFT
BSF PB1
BCF PB2
RETURN
MOTOR_RIGHT
BCF PB1
BSF PB2
RETURN
HALF_SPEED
MOVLW D'125'
MOVWF CCPR1L
RETURN
HIGH_SPEED
MOVLW D'255'
MOVWF CCPR1L
RETURN
SETUP_PWM_1
BSF STATUS, RP0
MOVLW D'255'
MOVWF PR2
BCF TRISB, 3
BCF STATUS, RP0
MOVLW B'00000110'
MOVWF T2CON
MOVLW B'00001100'
MOVWF CCP1CON
RETURN
SETUP
CLRF PORTB
BSF STATUS, RP0
CLRF TRISB
BCF STATUS, RP0
RETURN
END