'################################################### '-----------------SMARTSEA_2003.bsx----------------- 'This Code should be saved as .bsp to be used with a bs2p-40 'This code was created by Lee von Kraus, Summer 2003 'For SMARTSEA, a Semi-autonomous, Modular, 'and Affordable Research Tool for Submersed Ecosystem Analysis 'This code can be used with SmartConsole.bas running on a laptop 'connected to the Basic Stamp via a serial cable. '################################################### 'left motor should be connected to PAK chan0 'right motor should be connected to PAK chan1 'vertical motor should be connected to PAK chan2 '****************************************************************** 'Controller Declarations: PAKclk_P con 0 PAKdat_P con 1 TCMin_P con 2 'from tcm TCMout_P con 3 'to tcm PS2dat_P con 4 ' PS2cmd_P con 5 ' PS2att_P con 6 ' PS2clk_P con 7 ' BS1in_P con 8'from bs1 --see below BS1out_P con 9'to bs1 --see below Lights_P con 10'<-after pin8, I/O pin#s are diff on bs2sx than on bs2p-40 '********************************** 'values below will be conpaired to the first two binary bytes in the PS2_Vals array Square con %10000000 'in 2nd byte Cross con %01000000 '" Circle con %00100000 '" Triangle con %00010000 '" R1 con %00001000 '" L1 con %00000100 '" R2 con %00000010 '" L2 con %00000001 '" Start con %00001000 'in 1st byte Select con %00000001 'in 1st byte AllOnes con %11111111 'used to invert values with XOR '********************************** Manual con 0 Autonomous con 1 SemiAutonomous con 2 Compass_Seg con 0 Pitch_Seg con 1 Roll_Seg con 2 Temp_Seg con 3 Pressure_Seg con 4 YES con 1 NO con 0 NumSensorVals con 5 'pitch,roll,compass,temp,pressure CntrdMax con 127 '255 / 2 = 127 '********************************** '--------PAK VALUES--------- fpb var byte PAKchan var nib PAKregister var nib DURLOW con %000 DURHIGH con %001 HIGHDUR con %010 LOWDUR con %011 PSCALE con %100 PSBASE con %101 N con %110 CTRL con %111 '********************************** a var byte 'used in getLaptopCommands to b var byte 'allow multiple vars to be collected with c var byte 'only one serin command (a is used elsewhere too) x var word 'used for many uses y var word 'used for many uses Mode var nib Temp var word 'holds scratch pad ram values temporarily light var bit MissnOver var bit RJoy_midpointUD var byte RJoy_midpointLR var byte LJoy_midpointUD var byte fixedHeading var byte ERASE_ME VAR WORD '<--(used to simulate sensor readings), erase when no longer needed erase_me = 0 '----------------------------------------------------------------------------------- '-------------values below are used for scratch pad ram (SPR) management------------ '----------------------------------------------------------------------------------- 'button values from controller are also stored in scratch pad ram CntrlrScratch con 0 '<-start value within scratch pad ram Byte1_Pos con 0 Byte2_Pos con 1 RJoy_LR_Pos con 2 RJoy_UD_Pos con 3 LJoy_LR_Pos con 4 LJoy_UD_Pos con 5 '---- 'Sensor_Values are also stored in scratch pad ram SnsrScratch con 10 '<-start value within scratch pad ram Heading_Pos con 0 Roll_Pos con 1 Pitch_Pos con 2 Temp_Pos con 3 PressureLow_Pos con 4 PressureHigh_Pos con 5 '---- MSpeedsScratch con 20 LM_Speed_Pos con 0 '<-PAK channel0 RM_Speed_Pos con 1 '<-PAK channel1 FM_Speed_Pos con 2 '<-PAK channel2 BM_Speed_Pos con 3 '<-PAK channel3 for FWD, channel4 for REV PrevLM_Speed_Pos con 4 PrevRM_Speed_Pos con 5 PrevFM_Speed_Pos con 6 PrevBM_Speed_Pos con 7 '########################################################################################## '#############################----INITIALIZATION----####################################### '########################################################################################## Mode = Manual ' -------INITIALIZE THE PAK-VIII PWM CHIP---------- gosub freset ' disable PAK channels 0,1, and 2 PAKchan=1 PAKregister=6 x=1 gosub FCommand PAKchan=2 PAKregister=6 x=1 gosub FCommand '---- ' enable PAK channels 0,1, and 2 PAKchan=0 fpb=3 gosub FJam PAKchan=1 fpb=3 gosub FJam PAKchan=2 fpb=3 gosub fjam pause 3000 '-----------INITIALIZE THE JOYSTICKS-------------- GOSUB GetControllerCommands get CntrlrScratch + RJoy_UD_Pos, Temp RJoy_midpointUD = abs(temp - 255) put MSpeedsScratch + PrevLM_Speed_Pos, RJoy_midpointUD put MSpeedsScratch + PrevRM_Speed_Pos, RJoy_midpointUD get CntrlrScratch + RJoy_LR_Pos, Temp RJoy_midpointLR = abs(temp - 255) get CntrlrScratch + LJoy_UD_Pos, temp LJoy_midpointUD = abs(temp - 255) put MSpeedsScratch + PrevFM_Speed_Pos, LJoy_midpointUD ' debug "starting:",cr,"midUD:",dec RJoy_midpointUD,cr,"midLR:", dec RJoy_midpointLR,cr '########################################################################################## '##################################----MAIN----############################################ '########################################################################################## Main: ' debug cr,"****GetSensorData*****",cr GOSUB GetSensorData '<---------need tcm2-50 input ' pause 800 ' debug cr,"****GetControllerCommands******",cr GOSUB GetControllerCommands 'check<--check code, see if the clock will work ' pause 800 ' debug cr,"******UpdateMode******",cr GOSUB UpdateModes 'check ' pause 800 ' debug cr,"****UpdateLaptop******",cr GOSUB UpdateLaptop 'check '----------------------Act according to Mode-------------------- IF Mode = Autonomous then Aut IF Mode = SemiAutonomous then SemiAut IF Mode = Manual then Man SemiAut: GOSUB StayHorizontal Get (CntrlrScratch + Byte2_Pos), Temp If (Temp & R2 > 0) then Man ' debug "followHeading!!!",cr GOSUB FollowHeading GOTO AdjustSpeeds 'ROV will follow the heading it was pointing when semiaut mode was selected 'unless the R2 button is held down. In this case, normal manual control 'will be allowed until the R2 button is released. To set a new heading, just initiate semiaut mode again Man: ' debug cr,"****Execute PS2 Commands****",cr GOSUB ExecutePS2Commands 'speeds taken from PS2 controller of FollowHeading subroutine GOTO AdjustSpeeds 'adjust these speeds, then send to PAK Aut: ' debug cr,"****Get Laptop Commands****",cr GOSUB GetLaptopCommands GOSUB StayHorizontal 'speeds taken from laptop messages If MissnOver = NO then AdjustSpeeds 'adjust these speeds, then send to PAK Mode = Manual 'switch to manual mode when mission is over GOTO Main '---------------------Adjust Speeds------------------------------ AdjustSpeeds: ' debug cr,"********SpeedAdjust***********",cr for temp = 0 to 2 'temp = PAKchannel get MSpeedsScratch + Temp, x 'x = new speed get MSpeedsScratch + Temp+4, y 'y = previous speed GOSUB SafelyAdjustSpeed put MSpeedsScratch + Temp+4, x next 'safelyadjustspeed subroutine takes temp as the pwm channel, x as the speed, and y as the previous speed 'see the scratch _pos declaration to see how this incrementation works 'pause 2000 'left =chan0, right=chan1, vertical=chan2 GOTO Main '<--loop '########################################################################################## '################################--END OF MAIN LOOP--###################################### '########################################################################################## '################################## '########--Update Laptop--######### <--Done '################################## UpdateLaptop: serout 16,16624,["s"] '5 at start of each series ' pause 100 'tell laptop mode info, light info, etc too!!! For x = 0 TO (NumSensorVals) 'tell laptop warnings, if no plans received, etc! Get (SnsrScratch + x), Temp '(--SPR--) 'for x = 0 to 3 'temp = x*10+abs(y/10) serout 16,16624,["x"] 'send "X" in between numbers within each series pause 100 If Temp > 99 then dit100s If Temp >9 then dit10s If Temp <9 then dit1s dit1s: serout 16,16624,["00",dec Temp] 'if dit1s = ones digit goto SentVals dit10s: serout 16,16624,["0",dec Temp] 'if dit10s = tens digit goto SentVals dit100s: serout 16,16624,[dec Temp] 'if dit100s = hundreds digit SentVals: next '---put the autonomous flag at end of each transmission--- 'Get (SnsrScratch + AutFlag_Pos), Temp '(--SPR--) serout 16,16624,["x"] ' pause 100 IF Mode = Autonomous Then AutFlagYes serout 16,16624,["000"] 'if the laptop sees this it does not wait for the "RDY1" GOTO UpdatedLaptop AutFlagYes: serout 16, 16624,["001"] 'after the laptop sees this, it waits for the bs "RDY1" UpdatedLaptop: return 'the last two values sent before the autonomous flag are integer forms of 'the low and high byte of the Pressure word value. The laptop will take these two 'values and convert them into the single integer which the Pressure word represents 'by doing this: (highbyte * 256) + lowbyte 'because we have to send the Pressure value in two peices, the for loop goes to NumSensorVals 'instead of NumSensorVals-1 '################################## '#####--Get Laptop Commands--###### <--Done '################################## GetLaptopCommands: ' debug "--sending RDY1",cr serout 16, 16624, ["RDY1"] serin 16,16624,2000,SerialError,[wait("A"),dec x,dec y,dec temp,dec a,dec b,dec c,dec light,dec MissnOver] put MSpeedsScratch + RM_Speed_Pos, temp put MSpeedsScratch + LM_Speed_Pos, a put MSpeedsScratch + FM_Speed_Pos, b put MSpeedsScratch + BM_Speed_Pos, c '2 seconds w/out msg: go to SerialError IF x = 1 THEN MkMan 'check for manual mode IF y = 1 THEN MkSemi GOTO CheckedForPlans MkMan: ' debug "--manual mode set!",cr Mode = Manual goto CheckedForPlans MkSemi: 'debug "--semi-aut mode set!",cr SerialError: ' debug "--serial error OR semiaut mode set" Mode = Manual CheckedForPlans: return '################################## '#######--Update Modes--########### '<--Done '################################## UpdateModes: get (CntrlrScratch + Byte1_Pos), Temp If (Temp & Start = 0) then nxt1 ' debug "man mode set!!",cr mode = manual goto nxt2 'check if select is also pressed nxt1: If (Temp & Select = 0) then ModeSet ' debug "set semiaut!!, heading fixed!",cr mode = semiautonomous Get (SnsrScratch + Heading_Pos), x '(--SPR--) FixedHeading = x nxt2: If (Temp & Select = 0) OR (Temp & start = 0) THEN ModeSet ' debug "aut mode set!!",cr mode = autonomous FixedHeading = 0 'now that are in aut mode, will send the laptop the sensor values 'during laptopupdate, but with an autonomousflag=1 at the end, 'telling laptop to wait for "rdy1" then to send a mission plan. ModeSet: '==========execute non-motor related button presses============= Get (CntrlrScratch + Byte2_Pos), Temp '(--SPR--) ChkCross:if NOT (Temp & Cross) > 0 then ChkSquare 'lights on High Lights_P '-------- ChkSquare:if NOT (Temp & Square) > 0 then ModeFini 'light off LOW Lights_P '-------- ModeFini: return '################################## '###--Get Controller Commands--#### '<--Done '################################## GetControllerCommands: ' debug "****************",cr,"PS2 Data: ",cr low PS2att_P ' select the PSX controller shiftout PS2cmd_P, PS2clk_P, lsbfirst, [$01,$42] ' send "get ready" and "send data" shiftin PS2dat_P, PS2clk_P, 3, [Temp] ' read then discard this byte 'first byte returned by the ps2 controller reperesents the right button presses '2nd byte represents other button presses '3-6 byte represent joystick positions: RJoy_UD,RJoy_LR,LJoy_UD,LJoy_LR for x=0 to 5 shiftin PS2dat_P, PS2clk_P, 3, [Temp] If x > 1 then prntjoy Temp = Temp ^ %11111111 '<-XOR inverts the ps2 button press bytes ' debug "buttons:", bin Temp,cr goto storevals prntjoy: ' debug "joystick:", dec temp,cr StoreVals: put CntrlrScratch + x, Temp next high PS2att_P 'debug "*****************",cr return '#################################### '########--Get Sensor Data--######### <--do tests '#################################### GetSensorData: '===================get pressure==================== serout bs1out_P,17405,["rdy"] '<-2400 inverted to bs1 'debug "waiting",cr serin bs1in_P,17405,5,bs1error,[wait("x"),dec temp,x] put SnsrScratch+PressureLow_Pos, temp.lowbyte put SnsrScratch+PressureHigh_Pos, temp.highbyte goto getCompass bs1error: 'debug "bs1 error!!!<----" getcompass: erase_me = erase_me + 5 if erase_me > 85 then zipErase put SnsrScratch+Heading_Pos, ERASE_ME put SnsrScratch+Roll_Pos, ERASE_ME put SnsrScratch+Pitch_Pos, ERASE_ME put SnsrScratch+Temp_Pos, ERASE_ME put SnsrScratch+PressureLow_Pos, ERASE_ME put SnsrScratch+PressureHigh_Pos, 0 goto finishedsensors zipErase: erase_me = 0 '---------get compass reading--- 'put everything into the Sensor_Vals() array '"b=5",cr 'set baud rate to 9600 '"sso",cr 'serial output mode '"ec",cr,"ep",cr,"er",cr,"et",cr 'put compass, pitch, roll, temp in output word 'do a multipoint calibration (on land): "mpcal=e",cr,"go",cr,"h",cr,"mpcal=d",cr 'do an automatic calibration (in water): 'serout "s?",cr 'gets an output word from the tcm2-50 compass 'serin [(wait "C")TCM2_Vals(0), TCM2_Vals(1), TCM2_Vals(2), TCM2_Vals(3) '0=compass, 1=pitch, 2=roll, 3=temp '"go",cr 'start continuous sampling mode '"damping=e",cr 'enable digital damping '"h",cr 'halt continuous mode '--> 'get serial data from tcm2-50 regarding: 'temperature 'compass 'clinometer (both axes) 'get serial data from bs1 concerning pressure 'get additional sensor input from a blank subroutine (for future sensors) 'put into array: [compass, clinometer1, clinometer2, temp, pressure] '--> 'keep array of all past values too [..., ..., ...] finishedsensors: RETURN '######################################### '####--Execute Manual PS2 Commands--##### '<-Done '######################################### ExecutePS2Commands: ' debug "Manual Control!!",cr '================execute button presses===================== Get (CntrlrScratch + Byte2_Pos), Temp '-------- ChkR1: if NOT (Temp & R1) > 0 then ChkL1 'in-place turn right x = 255 y = 0 goto outputpwm '-------- ChkL1: if NOT (Temp & L1) > 0 then ChkL2 'in-place turn left x = 0 y = 255 goto outputpwm '-------- ChkL2: if NOT (Temp & L2) > 0 then ChkedButtons 'motors to neutral x = RJoy_MidPointUD y = RJoy_MidPointUD goto outputpwm '(sets thrust motors to neutral) '-------- ChkedButtons: '=================set horizontal motor speeds================= get CntrlrScratch + RJoy_UD_Pos, Temp 'debug "original temp:", sdec temp,cr temp = temp - 255 'debug "temp-255:", sdec temp,cr temp = abs(temp) 'debug "abs(temp):", sdec temp,cr if not ((temp < RJoy_midpointUD+7) and (temp > RJoy_midpointUD-7)) then getturn temp = RJoy_midpointUD '===============set horizontal turning======================== getturn: x = temp '<--LM_speed y = temp '<--rM_speed get CntrlrScratch + RJoy_LR_Pos, Temp temp = temp - 255 temp = abs(temp) 'debug "TURN:", dec temp,cr if not ((temp < RJoy_midpointLR+10) and (temp > RJoy_midpointLR-10)) then calcturn temp = RJoy_midpointLR calcturn: if temp = RJoy_midpointLR then calcextremesL if temp < RJoy_midpointLR and y >= RJoy_midpointUD then fwdR if temp < RJoy_midpointLR and y < RJoy_midpointUD then bckR if temp > RJoy_midpointLR and y >= RJoy_midpointUD then fwdL if temp > RJoy_midpointLR and y < RJoy_midpointUD then bckL fwdL: x = x - abs(temp - RJoy_midpointLR) '<--slow down L motor goto calcextremesL bckL: y = y - ((temp - RJoy_midpointLR)) '<--faster reverse on right motor if y = abs(y) then calcextremesL y = 0 goto calcextremesL fwdR: y = y - ((RJoy_midpointLR - temp)) '<--slow down R motor goto calcextremesL bckR: x = x - ((RJoy_midpointLR - temp)) '<--faster reverse on left motor if x = abs(x) then calcextremesL x = 0 goto calcextremesL '=============Ensure speeds are within 0-255 range============== calcextremesL: 'debug "L1:", sdec lm_speed, " R1:", sdec rm_speed,cr if x > 235 then maxL if x < 20 then minL goto calcextremesR maxL: x = 255 goto calcextremesR minL: x = 20 goto calcextremesR calcextremesR: if y > 235 then maxR if y < 20 then minR goto outputpwm maxR: y = 255 goto outputpwm minR: y = 20 goto outputpwm outputpwm: 'debug cls,"L:", sdec x, " R:", sdec y,cr,"------",cr 'pause 100 put MSpeedsScratch + LM_Speed_Pos, x put MSpeedsScratch + RM_Speed_Pos, y '===================set vertical motor speed=================== get CntrlrScratch + LJoy_UD_Pos, Temp temp = abs(temp-255) if not ((temp < LJoy_midpointUD+7) and (temp > LJoy_midpointUD-7)) then setvertical temp = LJoy_midpointUD setvertical: put MSpeedsScratch + FM_Speed_Pos, temp return '######################################### '########--Safely Adjust Speed--########## '<-Done '######################################### SafelyAdjustSpeed: If abs(x - y) > 80 then inhibition goto SpeedSet '---------------- inhibition: If y < x then InhibitIncrease If y > x then InhibitDecrease InhibitDecrease: x = y - 80 GOTO SpeedSet InhibitIncrease: x = y + 80 '---------------- SpeedSet: PAKchan=temp 'send the pwm speed to the correct channel of the pak1 PAKregister=1 gosub FCommand 'I replaced "fpx" of the pak subroutines with x to save variable space a = x PAKchan=temp PAKregister=2 x = 1700-x 'this code makes it so that the total PWM 'period' is 17ms gosub FCommand 'this is required by the Victor 883 to understand the PWM x = a ' debug "PWM Speed:", dec x,cr '<--uncomment this to see speeds!!! return '######################################### '########--PAK PWM Functions--############ '<-Done '######################################### ' Reset the Pak8 I/O FReset: LOW PAKdat_P LOW PAKclk_P HIGH PAKclk_P HIGH PAKdat_P LOW PAKclk_P return '------ FJam: fpb=PAKchan + (fpb<<3)+%01000000 goto FSendByte '------ FCommand: ' inputs PAKchan = channel, PAKregister = PAKregister fpb=(PAKregister<<3) + PAKchan gosub FSendByte fpb=x.lowbyte gosub FSendByte fpb=x.highbyte goto FSendByte '------ FReadReg: fpb=(PAKregister<<3) + PAKchan + %10000000 gosub FSendByte ' fall into FReadWord '------ FReadWord: Shiftin PAKdat_P,PAKclk_P,MSBPRE,[x.lowbyte,x.highbyte] return '------ FSendByte: Shiftout PAKdat_P,PAKclk_P,MSBFIRST,[fpb] return '######################################### '##########--Stay Horizontal--############ '######################################### StayHorizontal: '... return '######################################### '##########--Follow Heading--############# '######################################### FollowHeading: Get (SnsrScratch + Heading_Pos), Temp '(--SPR--) If (Temp > FixedHeading) then GoRight If (Temp < FixedHeading) then GoLeft ' debug "semiaut go straight!",cr Goto HeadingAdjusted GoRight: ' debug "semiaut turn right!",cr get MSpeedsScratch + PrevLM_Speed_Pos, y 'get prev L speed put MSpeedsScratch + LM_Speed_Pos, (y + (Temp - FixedHeading)) 'use it to make new L speed get MSpeedsScratch + PrevRM_Speed_Pos, y 'get prev R speed put MSpeedsScratch + RM_Speed_Pos, (y - (Temp - FixedHeading)) 'use it to make new R speed goto HeadingAdjusted GoLeft: ' debug "semiaut turn left!",cr get MSpeedsScratch + PrevLM_Speed_Pos, y 'see above put MSpeedsScratch + LM_Speed_Pos, (y - (Temp - FixedHeading)) get MSpeedsScratch + PrevRM_Speed_Pos, y put MSpeedsScratch + RM_Speed_Pos, (y + (Temp - FixedHeading)) HeadingAdjusted: return