' ========================================================================= ' ' File...... SeaCave.bs2 ' Purpose... Prototype This: Sea Cave Control Unit ' Author.... Joe Grand ' ' {$STAMP BS2} ' {$PBASIC 2.5} ' ' ========================================================================= ' -----[ Program Description ]--------------------------------------------- ' This code is for the Sea Cave control interface board. It uses two Melexis ' MLX90333 3-D Magnetic Joystick Position Sensors to determine position ' (in 2 axes) of the left and right thumbs on the handle of the XRACER Sea ' Scooter and transmits the data in a command packet for the Seabotix ' LBV150SE ROV. The packet is sent via a UDP broadcast packet over the ' network with a Lantronix XPORT module. A Lantronix XPORT Test Board ' receives the packet and outputs the data in RS232 (9600, 8N1) to the ' actual ROV. ' ' This control mechanism allows for completely contactless control, reducing ' the chance of data to physical joysticks due to water leakage. ' ' SeaBotix-specific command/packet information removed due to non-disclosure ' agreement with SeaBotix. Contact SeaBotix (www.seabotix.com) for more ' information and details if you want to use this code to interface to your ' own SeaBotix ROV. ' ' -----[ I/O Definitions ]------------------------------------------------- XPORT_TX PIN 15 ' Serial data input to Xport FROM BS2 XPORT_RX PIN 14 ' Serial data output from XPort TO BS2 SysReady PIN 0 ' ADC0834 AdcClk PIN 1 AdcCS PIN 2 AdcDio PIN 3 AdcSars PIN 4 ' -----[ Constants ]------------------------------------------------------- #SELECT $STAMP #CASE BS2, BS2E, BS2PE T1200 CON 813 T2400 CON 396 T4800 CON 188 T9600 CON 84 T19K2 CON 32 T38K4 CON 6 #CASE BS2SX, BS2P T1200 CON 2063 T2400 CON 1021 T4800 CON 500 T9600 CON 240 T19K2 CON 110 T38K4 CON 45 #CASE BS2PX T1200 CON 3313 T2400 CON 1646 T4800 CON 813 T9600 CON 396 T19K2 CON 188 T38K4 CON 84 #ENDSELECT SevenBit CON $2000 Inverted CON $4000 Open CON $8000 XPORT_Baud CON T9600 ' raw A/D values IDLE_LOW CON 10 ' out-of-range IDLE_HIGH CON 240 DEADBAND_LOW CON 80 ' deadband around center position of magnet over sensor DEADBAND_HIGH CON 160 ' -----[ Variables ]------------------------------------------------------- idx VAR BYTE ' loop counter tmp VAR BYTE chan VAR NIB ' channel number mux VAR NIB ' ADC mux bits adc VAR BYTE(4) ' channel data packet VAR BYTE(15) ' -----[ Initialization ]-------------------------------------------------- Setup: PAUSE 1000 DEBUG CLS, "Prototype This: Sea Cave Controller", CR, CR HIGH AdcCS HIGH SysReady ' the system's ready to go! DEBUG "System ready!", CR, CR ' -----[ Program Code ]---------------------------------------------------- Main: ' read 4 channels from ADC0834 ' ch. 0 = left thumb, x ' ch. 1 = left thumb, y ' ch. 2 = right thumb, x ' ch. 3 = right thumb, y FOR chan = 0 TO 3 GOSUB Read_0834 ' DEBUG DEC3 adc(chan), " " NEXT ' DEBUG CR ' stuff packets with idle thrusters just in case packet(PORTSPEED) = THRUST_SPEED_OFF packet(STBDSPEED) = THRUST_SPEED_OFF packet(LATERALSPEED) = THRUST_SPEED_OFF packet(VERTICALSPEED) = THRUST_SPEED_OFF ' ch. 0 = left thumb, x IF (adc(0) >= IDLE_LOW AND adc(0) <= DEADBAND_LOW) THEN DEBUG "L thumb: L", CR packet(LATERALSPEED) = THRUST_SPEED_REV ELSEIF (adc(0) >= DEADBAND_HIGH AND adc(0) <= IDLE_HIGH) THEN DEBUG "L thumb: R", CR packet(LATERALSPEED) = THRUST_SPEED_FWD ELSE ' no magnet within range or it's within deadband packet(LATERALSPEED) = THRUST_SPEED_OFF ENDIF ' ch. 1 = left thumb, y IF (adc(1) >= IDLE_LOW AND adc(1) <= DEADBAND_LOW) THEN DEBUG "L thumb: D", CR packet(VERTICALSPEED) = THRUST_SPEED_REV - $08 ' increase speed slightly for descent since we're fighting the weight of the vehicle ELSEIF (adc(1) >= DEADBAND_HIGH AND adc(1) <= IDLE_HIGH) THEN DEBUG "L thumb: U", CR packet(VERTICALSPEED) = THRUST_SPEED_FWD ELSE ' no magnet within range or it's within deadband packet(VERTICALSPEED) = THRUST_SPEED_OFF ENDIF ' ch. 2 = right thumb, x IF (adc(2) >= IDLE_LOW AND adc(2) <= DEADBAND_LOW) THEN DEBUG "R thumb: L", CR packet(STBDSPEED) = THRUST_SPEED_FWD packet(PORTSPEED) = THRUST_SPEED_REV ELSEIF (adc(2) >= DEADBAND_HIGH AND adc(2) <= IDLE_HIGH) THEN DEBUG "R thumb: R", CR packet(STBDSPEED) = THRUST_SPEED_REV packet(PORTSPEED) = THRUST_SPEED_FWD ELSE ' no magnet within range or it's within deadband packet(STBDSPEED) = THRUST_SPEED_OFF packet(PORTSPEED) = THRUST_SPEED_OFF ENDIF IF (packet(PORTSPEED) = THRUST_SPEED_OFF AND packet(STBDSPEED) = THRUST_SPEED_OFF) THEN ' if the ROV isn't currently rotating left or right ' ch. 3 = right thumb, y IF (adc(3) >= IDLE_LOW AND adc(3) <= DEADBAND_LOW) THEN DEBUG "R thumb: D", CR packet(STBDSPEED) = THRUST_SPEED_REV packet(PORTSPEED) = THRUST_SPEED_REV ELSEIF (adc(3) >= DEADBAND_HIGH AND adc(3) <= IDLE_HIGH) THEN DEBUG "R thumb: U", CR packet(STBDSPEED) = THRUST_SPEED_FWD + $04 ' increase speed slightly for forward motion packet(PORTSPEED) = THRUST_SPEED_FWD + $04 ELSE ' no magnet within range or it's within deadband packet(STBDSPEED) = THRUST_SPEED_OFF packet(PORTSPEED) = THRUST_SPEED_OFF ENDIF ENDIF ' send packet to ROV SEROUT XPORT_TX, XPORT_Baud, [STR packet\7] ' check for response back from ROV ' SEROUT XPORT_TX, XPORT_Baud, ["TEST", CR, LF] PAUSE 300 GOTO Main END ' -----[ Subroutines ]----------------------------------------------------- No_ROV_Response: DEBUG CR, "No ROV Response!", CR PAUSE 1000 GOTO Setup ' Reads ADC0834 inputs as single-ended ' -- input to read is passed in 'chan' (0 - 3) Read_0834: LOW AdcCS LOOKUP chan, [%100, %110, %101, %111], mux SHIFTOUT AdcDio, AdcClk, MSBFIRST, [1\1, mux\3] ' send start, mux Wait_0834: IF AdcSars = 0 THEN Wait_0834 ' wait until a/d conversion is done SHIFTIN AdcDio, AdcClk, MSBPOST, [adc(chan)\8] ' read selected input HIGH AdcCS RETURN