cold


decimal 
variable speed 100 speed !
variable rotate 0 rotate !

variable sleep_time

decimal 7640 constant right_stop  ( pwma0 )
decimal 7680 constant left_stop   ( pwma1 )


hex
7fff pwma0 pwm-period  ( set the period for the pwm )
decimal 

: sleep ( n -- )    \ pause for the giving number of miliseconds 
  sleep_time !		
  tcfticks @        \ get the starting time )
   begin
  	tcfticks @ over -  \ check the time
	sleep_time @ > 
  until
  drop
;

: move-robot  ( n_rotate n_speed -- )     
  2dup 
  right_stop + + pwma0 pwm-out
  left_stop swap - + pwma1 pwm-out
;
  
: stop-robot ( -- )
  pwma0 off
  pwma1 off
;

machine moveforward
 on-machine moveforward
   append-state moveR
   append-state stopR

in-state moveR
  condition pa3 off? causes 
    move-robot
	20 sleep        \ debounce switch
  then-state stopR 
to-happen


in-state stopR 
  condition pa3 off? causes 
    stop-robot
	20 sleep       \ debounce switch
  then-state moveR 
to-happen




: move-head ( n -- )	\ move the head and turn off pwm 0 - 10000
  pwma2
  1000 + 
  pwm-out
  50 sleep
  off
;


isomax-start

moveR set-state

install moveforward