;************************************************************************************
; Variable speed motor controller with RC input.
; Credit for origional code to jan.huygh@skynet.be,
; But this is a complete rewrite.
; enjoy,
;    Rue Mohr. (0xDA)
;
;
; Here are the PIN connections as used     ;  H Bridge Config (L is the load)
;                                          ;
;                   ATtiny13               ;            +VBat
;                    --------              ;              |
;   1K Pullup <-  [1]PB5  VCC[8] <- +5V    ;          |-------|
;         BHI <-  [2]PB3  PB2[7] -> BLI    ;  FET AH -|       |- FET BH
;    RC Pulse ->  [3]PB4  PB1[6] -> ALI    ;          |-     -|
;         GND ->  [4]GND  PB0[5] -> AHI    ;            |-L-|
;                    --------              ;          |-     -|
;                                          ;  FET AL -|       |- FET BL
;  Clocked at 9.6 MHz                      ;          |-------|
;                                          ;              |
;                                          ;             GND
;  When FET AH is conducting together with FET BL the motor is running forward.
;  When FET AL is conducting together with FET BH the motor is running backward.
;  When only 1 FET in the bridge is on, the motor coasts.

;             AL     AH     BL     BH     ;              AL     AH     BL     BH
; Forward      L    PWM      H      L     ;  ForwardOn    L      H      H      L
;                                         ;  ForwardOff   L      L      H      L
; Backward    PWM     L      L      H     ;  BackwardOff  L      L      L      H
;                                         ;  BackwardOn   H      L      L      H 
;
;********************************************************************************

;
;==========================| Constants |============================
;

;.Include "tn13def.inc"               ; Include for atmel compilers
.device ATtiny13                      ; include for gavrasm
   
.equ    RCPulseInputPin = 4           ; RC signal 
.equ    ALIOutputPin    = 1           ; ALI (PWM)
.equ    AHIOutputpin    = 0           ; AHI (PWM)
.equ    BLIOutputpin    = 2           ; BLI 
.equ    BHIOutputpin    = 3           ; BHI

.equ INPUT              = 0
.equ OUTPUT             = 1

;
;==========================| Variables |============================
;
    
.def    A                  = R16      ; working register

.def    Direction          = R17      ; for direction flag
.equ    DirFlag             = 0       ; direction flag
.equ    OldDir              = 1       ; prev dir flag
.equ    FromStop            = 2       ; we stopped, re-init direction

.def    RCPulseCounterL    = R26      ; Low  byte of the RCPulseCounter
.def    RCPulseCounterH    = R27      ; High byte of the RCPulseCounter

;
;========================| SYSTEM VECTORS |===========================
;
.org 0x0000
    rjmp    Init   ; RESET 
    reti           ; EXT_INT0  
    reti           ; PCINT0   (pin change)
    reti           ; TIM0_OVF  
    reti           ; EE_RDY  
    reti           ; ANA_COMP  
    reti           ; TIM0_COMPA  
    reti           ; TIM0_COMPB  
    reti           ; WATCHDOG  
    reti           ; ADC  

;
;=============================| INIT |================================
;

Init:
; Disable interrupts
 cli                                   ;Clear Global Interrupt

; Set all I/O pins of port B as output except RCPulseInputPin    
 ldi   A,    ((0<<PORTB5)|(0<<PORTB4)|(0<<PORTB3)|(0<<PORTB2)|(0<<PORTB1)|(0<<PORTB0)) ; make sure nothing is set
 out   PORTB, A

 ldi   A,    0xFF-(1 << RCPulseInputPin)  ; Set all I/O pins of port B as output, except RCPulseInputPin
 out   DDRB,  A

 ; Disable Pull Up
 ldi A,      (1<<PUD)
 out MCUCR, A

 ; initialize pwm system
 rcall pwm_init

 ; clear direction flags
 clr Direction

;
;=============================| MAIN |================================
;

; main loup:
; while the RC-Pulse is coming in, RCPulseCounter is incremented
; the pwm value is set
; the pulse counter is set to 1
; while no RC-Pulse is coming in, RCPulseCounter is incremented,
;  if RCPulseCounter == 0 then we rolled over, which means we 'timed out',
;  goes into failsafe
; the pulse counter is cleared
; loup repeated


; while (RCPulseInputPin==1) {
;   RCPulseCounterL++
; } 
; updatepwm
; RCPulseCounter = 1
; while (RCPulseInputPin == 0) {
;   wait
;   if (RCPulseCounter == 0) {
;     Failsafe
;   }
; }
; RCPulseCounter = 0


MainLoup:



; count the high time
while_1_begin:
  adiw RCPulseCounterL, 1          ; 2 cyc      RCPulseCounter++
  breq FailSafe                    ; 1 cyc     (major overflow)
  sbic    PinB, RCPulseInputPin    ; 1 cyc
    rjmp  while_1_begin            ; 2 cyc
while_1_end:   

; now that we have captured the pulse, we can doddle a little...

;------ check counter range
; measurements show:  1.5ms = about 2920 counts ( it jumps around a little )
; so .97ms = 3944
;   2.03ms = 1896
;   
;  PWM = abs((K/4)-730)
;
; both a convienient 1024 from the centre range
; these are out of the normal range a little 

 asr  RCPulseCounterH                  ; RCPulsecounter=RCPulseCounter/4
 ror  RCPulseCounterL
 asr  RCPulseCounterH                  
 ror  RCPulseCounterL

 subi RCPulseCounterL,low(620)         ; RCPulsecounter-=730
 sbci RCPulseCounterH,high(620)

; result should be 8 bit, if it isn't, were out of range

 BST Direction, DirFlag                ; make a copy of the direction flag to compare
 BLD Direction, OldDir                 ;    with later for changes



 brmi Else1                            ; If (RCPulseCounter>0) then
   SBR Direction, (1<<DirFlag)         ; set direction flag
   rjmp EndIf1
Else1:                                 ; Else	
   CBR Direction, (1<<DirFlag)         ; clear direction flag
   com  RCPulseCounterH                ;   RCPulseCounter-=RCPulseCounter
   neg  RCPulseCounterL
   sbci RCPulseCounterH, 0xFF          
EndIf1:	

                                         
 BST Direction, FromStop               ; if from stop, mismatch olddir to cause resetting of direciton  
 BRTC if_overflow
   CBR Direction, (1<<FromStop)        ; clear the fromstop flag
   CBR Direction, (1<<OldDir)          ; clear old flag
   BST Direction, DirFlag              ; if dirflag is clear, set old flag
   BRTS if_overflow
     SBR Direction, (1<<OldDir)

if_overflow:
 ; if (RCPulseCounterH != 0) OVERFLOW!!!!
  tst RCPulseCounterH
  brne FailSafe

; at this point we have an 8 bit number for our pwm register

  tst RCPulseCounterL
  brne not_zero
    rcall pwm_stop
    rjmp  Wait_low

not_zero:
  out  OCR0A, RCPulseCounterL  ; set pwm values
  out  OCR0B, RCPulseCounterL

  cpi Direction, 0x01          ; test for change in direction
  brne check2                  ; if not that change then dont update direction
    rcall pwm_forward

check2:
  cpi Direction, 0x02
  brne checkdone
    rcall pwm_reverse

checkdone:
     
;------
Wait_low:
 ldi RCPulseCounterL, low(1)       ; RCPulseCounter = 1
 ldi RCPulseCounterH, high(1)

; This code will pull a failsafe if the updates fall below about 30Hz

while_0_begin:
  adiw RCPulseCounterL, 1          ; RCPulseCounter++
  breq FailSafe                    ; major overflow
  sbis    PinB, RCPulseInputPin    ; get to the wait 1 loop about as fast as you can
    rjmp  while_0_begin            ;   this code should be just before wait 1 at the top
while_0_end:

  clr RCPulseCounterL               ; RCPulseCounter = 0
  clr RCPulseCounterH

 rjmp MainLoup


;
;--------------| stop and do sanity checks |-----------
;
FailSafe:
  rcall pwm_stop  ; stop motors
                  
  rjmp Wait_low   ; if mainloop fails, we will end up back here

;
;----------------------| PWM Init |-------------------
;
pwm_init:
  ; clear pwm levels
  clr  A
  out  OCR0A, A 
  out  OCR0B, A

  ; set up TCCR0A

  ; 0  1  WGM00: Waveform Generation Mode - Fast PWM Page 68
  ; 1  1  WGM01: Waveform Generation Mode
  ; 2  0
  ; 3  0 
  ; 4  0  COM00B: Compare Match Output A Mode - OFF
  ; 5  0  COM01B: Compare Match Output A Mode
  ; 6  0  COM00A: Compare Match Output A Mode - OFF
  ; 7  0  COM01A: Compare Match Output A Mode
  ;
  ldi  A, 0x03   
  out  TCCR0A, A

  ; set up TCCROB
  ; 0  0  CS00: Clock Select - clkI/O/256 (From prescaler) = 38Khz
  ; 1  0  CS01: Clock Select
  ; 2  1  CS02: Clock Select
  ; 3  0  WGM02: Waveform Generation Mode - Fast PWM
  ; 4  0
  ; 5  0
  ; 6  0  FOC0B: Force Output Compare B
  ; 7  0  FOC0A: Force Output Compare A
  ;
  ldi  A, 0x04     
  out  TCCR0B, A

 ret

;
;----------------------| PWM Forward |-------------------
;
; turn on BH(3) and pwm for AL(OC0B)
;
pwm_forward:

  CBI  PORTB, 2  ; set direction
  SBI  PORTB, 3

  CBI  PORTB, 0  ; make sure outputs are off!
  CBI  PORTB, 1

  ldi  A, 0x03   ; turn pwm's off
  out  TCCR0A, A
  nop  

  ; set up TCCR0A
  ; 0  1  WGM00: Waveform Generation Mode - Fast PWM Page 68
  ; 1  1  WGM01: Waveform Generation Mode
  ; 2  0
  ; 3  0 
  ; 4  0  COM00B: Compare Match Output A Mode - FAST_PWM
  ; 5  1  COM01B: Compare Match Output A Mode
  ; 6  0  COM00A: Compare Match Output A Mode - OFF
  ; 7  0  COM01A: Compare Match Output A Mode
  ;
  ldi  A, 0x23      
  out  TCCR0A, A

 ret

;
;----------------------| PWM Reverse |-------------------
;
; turn on BL(2) and pwm for AH(OC0A)
;
pwm_reverse:

  CBI  PORTB, 3  ; set direction
  SBI  PORTB, 2

  CBI  PORTB, 0  ; make sure outputs are off!
  CBI  PORTB, 1

  ldi  A, 0x03   ; turn pwm's off
  out  TCCR0A, A
  nop  

  ; set up TCCR0A
  ; 0  1  WGM00: Waveform Generation Mode - Fast PWM Page 68
  ; 1  1  WGM01: Waveform Generation Mode
  ; 2  0
  ; 3  0 
  ; 4  0  COM00B: Compare Match Output A Mode - OFF
  ; 5  0  COM01B: Compare Match Output A Mode
  ; 6  0  COM00A: Compare Match Output A Mode - FAST PWM
  ; 7  1  COM01A: Compare Match Output A Mode
  ;
  ldi  A, 0x83      
  out  TCCR0A, A

 ret

;
;----------------------| PWM Stop |-------------------
;
; turn on BL(2) and AL(1) this will put the motor into a breaking
;
pwm_stop:

  SBR Direction, (1<<FromStop)

  CBI  PORTB, 3  ; set direction
  SBI  PORTB, 2

  CBI  PORTB, 0  ; clear initial state
  CBI  PORTB, 1

  ldi  A, 0x03   ; turn pwm's off
  out  TCCR0A, A
  nop  

  CBI  PORTB, 0  ; prepare outputs
  sBI  PORTB, 1
 ret