(* Micro Launchpad servo testerprogram
    S?  = P1.3
    Led = P1.0
*)

hex
: FLASH     ( -- )
    8 0 do  1 21 *bix  100 ms  loop ;

\ Space for a PWM value and pause period
create SERVOS  2 cells allot

: SET-PAUSE  ( -- )
    DM 20000  servos @ -  servos cell+ ! ;

\ Set servoposition in steps from 0 to 255 pulsewidth 500 us to 2500 us
: SERVO     ( u -- )
    DM 8 * DM 500 + DM 2500 umin   dup servos !  set-pause
    s? 0= if  cr dup u.  begin s? until  50 ms  then  drop ;

\ This interrupt gives 0.5 to 2.5 millisec. pulses at 50 Hz (yy cells, xx cycles)
\ Register R11 (xx) can not be used for something else!!!!
routine SERVO-INT  ( -- a ) \ 6 - interrupt call
    day push                \ 3 - 1 Save original r8
    servos # day mov        \ 2 - 2 Load address pointer
    xx day add              \ 1 - 1 Calc. address of next period
    day ) 172 & mov         \ 5 - 2 TA0CCR0 Set next period
\ Now the piece that sets servo pulse
    #0 xx cmp               \ 1 - 1 First entry?
    =? if,                  \ 2 - 1 Yes
        10 # 021 & .b bis   \ 5 - 2 P1OUT  Set bit on (P1.4)
        #2 xx add           \ 1 - 1 Increase pointer
        rp )+ day mov       \ 3 - 1 Restore originele r8
        reti                \ 5 - 1
    then,
\ Second entry = pause period?
    10 # 021 & .b bic       \ 5 - 2 P1OUT  Reset bit on (P1.4)
    #0 xx mov
    rp )+ day mov           \ 3 - 1 Restore originele r8
    reti                    \ 5 - 1
end-code

code INTERRUPT-ON      #0 xx mov  #8 sr bis  next  end-code
code INTERRUPT-OFF     #8 sr bic  next  end-code

\ SERVO's op Px,y etc.
: SERVO-ON  ( -- )          \ Initialise servo hardware
    010 022 *bis            \ P1DIR    Bit P1.4 output
    0 160 !                 \ TA0CTL   Stop timer-A0
    DM 1000 172 !           \ TA0CCR0  First interrupt after 1 ms
    02D4  160 !             \ TA0CTL   Start timer
    0010 162 !              \ TA0CCTL0 Set compare 0 interrupt on
    7D servo                \ Default pulse lenght is 1,5 ms
    interrupt-on ;          \ Activate

: SERVO-OFF ( -- )
    0 160 !                 \ TA0CTL   Stop timer-A0
    010 162 **bic           \ TA0CCTL0 Interrupts off
    interrupt-off ;

: CENTER    ( -- )      07D servo ; \ Servo to center position

\ ADC on and sample & hold time at 8 ADC10 clocks
: SETUP-ADC ( -- )
    12 1B0 **bic                \ ADC10CTL0  Deactivate ADC & clear ENC
    10 1B0 **bis                \ ADC10CTL0  Activate ADC
    20 04A c!                   \ ADC10AE0   P1.5 is ADC in
    0010 1B0 ! ;                \ ADC10CTL0  T-sample 8 ADC10 clocks,
\   0110 1B0 ! ;                \ ADC10CTL0  T-sample 64 ADC10 clocks,
                                \            ADC on & Vref = VCC & VSS

: ADC       ( +n -- u )
    F000 and                    \ Mask ADC input channel selector
    02 1B0 **bic                \ ADC10CTL0  Clear ENC
    40 or 1B2 !                 \ ADC10CTL1  Select input, MCLK/3
    03 1B0 **bis                \ ADC10CTL0  Set ENC & ADC10SC
    begin 4 1B0 bit** until     \ ADC10CTL0  ADC10 ready?
    1B4 @ ;                     \ ADC10MEM   Read result

: POTMETER      5000 adc ;      \ Read level at P1.5

: TESTADC   ( -- )              \ print ADC value
    flash  setup-adc
    begin
        potmeter .
    key? until ;

\ A servo tester using a potmeter at P1.5
: TESTER    ( -- )
    servo-on  setup-adc  flash  cr ." Servo tester "
    begin  potmeter 2/ 2/ servo  key? until ;

servo-int  FFF2 vec!        \ Install Timer-A0 vector
' tester  to app  freeze  servo-on

\ End
