' ========================================================================= 
' 
'   File...... seacave_etchasketch_demo.bs2 
'   Purpose... Prototype This: Sea Cave (Demonstration for Podcast)
'   Author.... J. Grand
' 
' {$STAMP BS2} 
'   {$PBASIC 2.5} 
' 
' ========================================================================= 
 

' -----[ Program Description ]--------------------------------------------- 
 
' This code is a demonstration of the Melexis MLX90333 3-D Magnetic
' Joystick Position Sensors that we are using for the Sea Cave control
' interface board. In this demo, we read the magnet position in two
' axes and use the acquired data to drive two continuous rotation servo 
' motors which control the X and Y axes of an Etch-a-Sketch. 
' 

' -----[ I/O Definitions ]------------------------------------------------- 
 
AdcClk       	PIN     	12    ' ADC clock
AdcDta         	PIN     	13    ' ADC data line
AdcCS_OUT1    	PIN     	0     ' ADC chip selects
AdcCS_OUT2		PIN		7

Servo_X		PIN		14
Servo_Y		PIN		15


' -----[ Constants ]------------------------------------------------------- 
' raw A/D values
IDLE_LOW		CON		10	' out-of-range
IDLE_HIGH      	CON 		230

DEADBAND_LOW	CON		107   ' deadband around center position of magnet over sensor
DEADBAND_HIGH	CON		147

' -----[ Variables ]------------------------------------------------------- 
 
idx          	VAR     	NIB                     ' loop counter 

' returned voltages from MLX90333
MLX_OUT1		VAR		BYTE
MLX_OUT2		VAR		BYTE
result_x		VAR		WORD
result_y		VAR		WORD

' -----[ Initialization ]-------------------------------------------------- 
 
Setup:
  PAUSE 500
  DEBUG CLS, "Prototype This: Etch-a-Sketch Controller with MLX90333!", CR, CR
 
  HIGH AdcCS_OUT1
  HIGH AdcCS_OUT2


' -----[ Program Code ]---------------------------------------------------- 
 
Main: 
  ' read both output channels of the MLX90333 with our A/Ds
  ' the Vref input of the ADC0831 is set to 2.55 vdc, giving 0.01 volts
  ' per count.
  LOW AdcCS_OUT1
  SHIFTIN AdcDta, AdcClk, MSBPOST, [MLX_OUT1\9]   ' get the voltage
  HIGH AdcCS_OUT1
  'DEBUG " OUT1: ", DEC3 MLX_OUT1

  LOW AdcCS_OUT2
  SHIFTIN AdcDta, AdcClk, MSBPOST, [MLX_OUT2\9]
  HIGH AdcCS_OUT2
  'DEBUG " OUT2: ", DEC3 MLX_OUT2

  ' set servo speed depending on the position of the magnet over the MLX90333
  ' OUT1 = left/right
  ' if no magnet is detected or if magnet is out-of-range...
  IF (MLX_OUT1 < IDLE_LOW OR MLX_OUT1 > IDLE_HIGH OR (MLX_OUT1 > DEADBAND_LOW AND MLX_OUT1 < DEADBAND_HIGH)) THEN  
    result_x = 750  ' stop moving servo
  ELSE ' there is a magnet in range, so let's scale it and set the servo speed 
    IF (MLX_OUT1 >= IDLE_LOW AND MLX_OUT1 <= DEADBAND_LOW) THEN
      'DEBUG " X: CW"
      result_x = ((MLX_OUT1 - IDLE_LOW) / 6) + 730
    ELSEIF (MLX_OUT1 >= DEADBAND_HIGH AND MLX_OUT1 <= IDLE_HIGH) THEN
      'DEBUG " X: CCW"
      result_x = ((MLX_OUT1 - DEADBAND_HIGH) / 6) + 754
    ENDIF
    'DEBUG " SERVO_X: ", DEC result_x, CR
  ENDIF

  ' OUT2 = up/down
  MLX_OUT2 = ~MLX_OUT2  ' invert MLX_OUT2 so up is down and down is up
  IF (MLX_OUT2 < IDLE_LOW OR MLX_OUT2 > IDLE_HIGH OR (MLX_OUT2 > DEADBAND_LOW AND MLX_OUT2 < DEADBAND_HIGH)) THEN  
    result_y = 750  ' stop moving servo
  ELSE ' there is a magnet in range, so let's scale it and set the servo speed 
    IF (MLX_OUT2 >= IDLE_LOW AND MLX_OUT2 <= DEADBAND_LOW) THEN
      'DEBUG " Y: CW"
      result_y = ((MLX_OUT2 - IDLE_LOW) / 6) + 730
    ELSEIF (MLX_OUT2 >= DEADBAND_HIGH AND MLX_OUT2 <= IDLE_HIGH) THEN
      'DEBUG " Y: CCW"
      result_y = ((MLX_OUT2 - DEADBAND_HIGH) / 6) + 754
    ENDIF
    'DEBUG " SERVO_Y: ", DEC result_y, CR
  ENDIF
  'DEBUG CR

  PULSOUT Servo_X, result_x
  PULSOUT Servo_Y, result_y

  GOTO Main  ' start again...
  END