ok, here's a revised version my my first program. i have the fin servo going the whole time and when i push one of the bumpers in, all the rudder servo does is twitch, and I dont hear the solenoid valve clicking open either. Again all i'm aiming for is it to swim straight, dive up and down while doing so, and if it bumps into a wall or object, it turns and swims away. any suggestions?
' {$STAMP BS1}
' {$PBASIC 1.0}
start:
PULSOUT 5,134 'rudder motor go to neutral pos
IF PIN0 = 1 THEN left
IF PIN1 = 1 THEN right
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 ' fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
IF PIN0 = 1 THEN left
IF PIN1 = 1 THEN right
'
ballast routine
HIGH 3 'opens ballast solenoid valve and starts to sink
PAUSE 15000 'allows 15 seconds to flood ballast
LOW 3 'closes ballast
PAUSE 20000 'allows fish to swim and lowest depth for 20 seconds
HIGH 2 'opens solenoid valve
PAUSE 100 'burst of 20 psi CO2 to partially blow ballast
LOW 2 'closes solenoid valve
PAUSE 20000 'swims at new depth for 20 seconds
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
IF PIN0 = 1 THEN left
IF PIN1 = 1 THEN right
GOTO start
'
turning >>fixes tail motor for about 5 seconds while the fin motor continues to oscillate
left:
PULSOUT 5, 252 'left turn
GOTO b
b:
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 'fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
IF PIN0 = 0 THEN start
IF PIN0 = 1 THEN b
GOTO b
right:
PULSOUT 5,210 'right turn
GOTO a
a:
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 'fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
Both of your servos need a pulse command every 20ms whether or not they are moving.
I asked about variables because you could have the pulse amounts for both the fin and rudder stored in variables.
The different parts of the program could change these variables as needed.
You then could use a subroutine to pulse the servos.
Near the beginning of the program you'd use:
SYMBOL rudder B1
SYMBOL fin B2
You'd want to make a subroutine to pulse the servos based on the values of "rudder" and "fin".
refresh:
PULSOUT 5, rudder
PULSOUT 6, fin
PAUSE 18 ' I don't think you need to pause 20 since the rest of your program will also take time.
' You might want to use a value less than 18 here.
RETURN
Then thoughout your code you'd call the "refresh" subroutine.
Since the servos need refreshing every 20ms, you don't want long pauses.
PAUSE 15000 'allows 15 seconds to flood ballast
The servos will likely start acting eratic with such a long pause between refreshes.
Normally in such a program, you'd have a master loop. Execution can branch from this loop but it should then return to the loop.
Since you need to have multiple things happening at once (servos refreshing, balst purging, etc.), you'll need to repeatedly switch back and forth between these tasks (again, the servos should be refreshed every 20ms).
As I said, I haven't programmed a Basic Stamp 1. Hopefully someone who has will jump in with some examples of controlling multiple servos and monitoring multiple inputs.
I decided for the turning, i can just stop motion of the fin and use the momentum of the fish with the rudder turned then have it go back to swimming straight again after it's done turning. But as far as getting the ballast to work while the servos are moving, it's over my head. here's what I have so far:
' {$STAMP BS1}
' {$PBASIC 1.0}
start:
IF PIN1 = 0 THEN a
IF PIN0 = 0 THEN b
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 ' fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
GOSUB ballast
ballast:
HIGH 3 'opens ballast solenoid valve and starts to sink
PAUSE 15000 'allows 15 seconds to flood ballast
LOW 3 'closes ballast
PAUSE 20000 'allows fish to swim and lowest depth for 20 seconds
HIGH 2 'opens solenoid valve
PAUSE 100 'burst of 20 psi CO2 to partially blow ballast
LOW 2 'closes solenoid valve
PAUSE 20000 'swims at new depth for 20 seconds
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
If I take out the ballast program, the motors work perfectly. But when i have it in there, it stops all movement of the motors and just clicks the solenoid valves. Soon as I get this bug worked out and everything running at the same time, ill be able to water proof it and throw it in the pool!
I'm going take my best guess of what you want to program to do and then I hope you'll correct me.
You want the robot to move the fin sevo back and forth from pulse value 152 to pulse value 210. Isn't 134 center?
You're presently giving the fin 0.2 seconds to move from one side to the other. Is this how fast you want it to move? Do you want the servo to jerk from one side to the other or would you prefer a smoother motion?
I think you'd prefer the fin to continue to move at a constant pace no matter what else the program is doing, but you're willing to let the fin stop if needed to perform other tasks. Is this correct?
If the button attached to pin 0 is pressed, you'd like the robot to move the rudder servo to position 210 for 2.5 seconds and then return to position 175 (I assume this is center).
If the button attached to pin 1 is pressed, you want the rudder servo moved to position 150 instead of 210.
Can both buttons be pressed by the robot's actions?
When do you want the balast sequence to start? Do you want the buttons monitored while the balast sequence is in progress (while swimming for 20 seconds in deep water and then in swallow water)?
How many times do you want the balast sequence to execute? Do you want the program to end?
I’ve bought different servos, so the center position is different. (about 175 or so)
As far as the fin motion, what I have now is just a starting place, I’ll have to see what it does to the motion of the bot in the water, but yes im fine with it just moving from one side to the other.
Ideally the fin would actuate the entire time the bot is turned on no matter what the other parts are doing, but I am willing to let it stop to rudder only. If I can continue the motion throughout the rudder action, that would be great, but either way fine.
You’re correct about the push buttons turning the rudder motor.
I have the head mounted on a springed hinge almost like a bobble head, if it hits a wall, the head turns and hits a push button on the opposite side, so it cannot push both buttons at the same time.
I would like the ballast program start once the fish is turned on and to run continuously and keep looping the entire time the robot is on. So it just keeps diving and surfacing over and over.
I was curious how one would go about driving multiple servos, using a Basic Stamp 1, with all the complications required by this project.
As I wrote the BS1 code below, I learned I much prefer to programming the Propeller over programming a BS1.
I'm hoping someone with more BS1 experience will comment on this code.
Does the code really need to be this complicated?
This is what I came up with to drive the fin and rudder servos. My intention was to smooth out the movements of both the fin and rudder.
I used two state variables; "rudderState" to keep track of which way the rudder should be moving and "finState" to determine which direction to move the fin servo.
I had the rudder hold its position once it reach either the right or the left for a predetermined about of time "RudderHoldTime". The same technique used to monitor the rudder hold time could also be used to monitor the time spent in the various ballast states.
I don't know how much of the BS1's memory this program would use and if there would be enough left over for ballast control.
I don't have the Basic Stamp editor software installed on my computer so I don't know if this program will compile.
It's a much more complicated program than I would normally feel comfortable posting to the forum without testing it first. Hopefully there aren't too many bugs.
Here's the code:
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
SYMBOL LeftRudderP = 150
SYMBOL CenterRudderP = 175
SYMBOL RightRudderP = 210
SYMBOL RudderStep = 3 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
SYMBOL RudderHoldTime = 25 ' Adjust RudderHoldTime to change how long the
' rudder servo is held at it extreme movement.
' Larger numbers will hold the servo position
' longer. Multiply RudderHoldTime by 0.02 seconds
' to calculate the aproximate hold time.
' A value of 25 will hold the position for half
' a second.
SYMBOL FinSideAP = 152
SYMBOL FinSideBP = 210
SYMBOL FinTowardAS = 0
SYMBOL FinTowardBS = 1
SYMBOL RudderCenteredS = 0
SYMBOL RudderToRightS = 1
SYMBOL RudderRightS = 2
SYMBOL RudderFromRightS = 3
SYMBOL RudderToLeftS = 4
SYMBOL RudderLeftS = 5
SYMBOL RudderFromLeftS = 6
' Add a counter for ballast timing. Also add ballast states to be checked by
' the program.
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = centerRudderP
finPosition = FinSideAP
rudderState = RudderCenteredS
finState = FinTowardBS
'-------------------- Beginning of MainLoop -------------------------
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = FinTowardBS THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft
IF rudderState = RudderLeftS THEN rudderCheckLeft
IF rudderState = RudderToLeftS THEN rudderCheckToLeft
IF rudderState = RudderFromRightS THEN rudderCheckFromRight
IF rudderState = RudderRightS THEN rudderCheckRight
IF rudderState = RudderToRightS THEN rudderCheckToRight
afterRudderCheck:
' Add checks of ballast states here.
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'-------------------- End of MainLoop -------------------------
turnLeft:
rudderState = RudderToLeftS
GOTO afterButtonCheck
turnRight:
rudderState = RudderToRightS
GOTO afterButtonCheck
finCheckToB:
finPosition = finPosition + finStep
IF finPosition >= FinSideBP THEN reverseFinToA
GOTO afterFinCheck
reverseFinToA:
finPosition = FinSideBP
finState = FinTowardAS
GOTO afterFinCheck
finCheckToA:
finPosition = finPosition - finStep
IF finPosition <= FinSideAP THEN reverseFinToB
GOTO afterFinCheck
reverseFinToB:
finPosition = FinSideAP
finState = FinTowardBS
GOTO afterFinCheck
rudderCheckFromLeft:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= CenterRudderP THEN stopRudderCenterFL
GOTO afterRudderCheck
stopRudderCenterFL:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckLeft:
rudderHoldCounter = rudderHoldCounter + 1
IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFL
GOTO afterRudderCheck
moveRudderCenterFL:
rudderState = RudderFromLeftS
GOTO afterRudderCheck
rudderCheckToLeft:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= LeftRudderP THEN stopRudderLeft
GOTO afterRudderCheck
stopRudderLeft:
rudderState = RudderLeftS
rudderPosition = LeftRudderP
rudderHoldCounter = 0
GOTO afterRudderCheck
rudderCheckFromRight:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR
GOTO afterRudderCheck
stopRudderCenterFR:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckRight:
rudderHoldCounter = rudderHoldCounter + 1
IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFR
GOTO afterRudderCheck
moveRudderCenterFR:
rudderState = RudderFromRightS
GOTO afterRudderCheck
rudderCheckToRight:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= RightRudderP THEN stopRudderRight
GOTO afterRudderCheck
stopRudderRight:
rudderState = RudderRightS
rudderPosition = RightRudderP
rudderHoldCounter = 0
GOTO afterRudderCheck
Is it really this hard to control two servos with the constraints of this project. I keep thinking there must be a better way of doing this.
If this is a valid approach, then the ballast code could be added using the same technique of setting states and counting loops to control the time spent in the various states.
Just so people know, a Propeller QuickStart board costs less than any of the Basic Stamp modules cost. IMO, the software for this project would be easier to write for the Propeller than any of the Basic Stamps. (It's no secret, I'm a big fan of the Propeller (especially for robots).)
Holy Smile man you're a genius! haha this program is gnarly. way over my head. The servos and switches all work awesomely with your program. I dont understand how i could make the ballast turn on and off with the counter though. I read your notes, and i'm gonna try to figure it out. Thank you so much!!
p.s: I was wondering if i could use an if.. then.. for if it executes the main loop x times then branches off to a diving routine (either with or without swimming) then executes the main loop x more times, then goes to a surfacing routine. then resets? would that make the program simpler? just a thought though.
Also i agree about the propeller, i should have done more research on controllers before picking the bs1. on future projects i definitely will either go with the bs2 or propeller. It took me almost the whole project time to get the parts in and now im too close to the deadline to order a new controller =/.
Wow, I'm amazed the program worked. I frequently copy one code section to use as a template for another section of code. I usually forget to change at least one of the variables from the copied section to what they should be in the new section.
My own programs rarely run right away.
Just as a used a rudder state to determine which way to move the rudder servo, you'll want to use ballast states and a ballast counter to know when to open and close the valves.
How many ballast states does the robot need?
I figure the robot will start at the surface. Maybe call it surfaceS (I added postfixes P for servo positions and S for states). A counter will be set to zero and when the counter reaches a desired amount the robot will change ballast state. This state could be "openBallastValveS". The counter would be zeroed again and the program will start counting loops to hold the valve open. Once the valve has been open long enough, the state would change again with the counter again restarted to use as a timer until the next ballast state.
So what do you want the ballast to do?
The robot swims on the surface for some time. How long?
I can see how long to hold the valves open from your code.
It looks like you want the robot to submerge all in one step and then rise in a series of small steps. Is this correct?
It looks like the robot should swim at the surface for 20s before staring to submerge again. Right?
Does the program editor let you know how much of the BS1 memory has been used? I think the final program could easily be twice as long as it is now.
My ballast consists of two 2-way solenoid valves. I’m aiming for something like this:
Solenoid 1: open as soon as the program runs. Close after 10 seconds.
Solenoid2: after solenoid 1 is closed, open for .2 seconds then close. Repeat a few times then goto solenoid 1
The amount of time it swims on the surface doesn’t matter. also the amount of time it swim submerged doesn't matter. as long as it does both at some point. Also I originally intended to sink all in one step, but if its easier to do it gradually, then I’m fine with that.
I'll try to find if it shows the memory used somewhere.
It might be easier to submerge all at once (and use less memory).
There may be parts of the program as it is now that could be cut. I wanted to show how to use a counter to measure time, but it might not be really needed with the rudder.
Gotcha. If we have to blowing the ballast in one or two bursts, to save memory, that is also acceptable. but we might get a flying fish!! Which i'm not gonna lie, would be pretty funny.
This code triggers the CO2 only once. You might want to increase the time it spends blowing the tank.
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
SYMBOL ballastState = B6
SYMBOL ballastCounter = B7
SYMBOL LeftRudderP = 150
SYMBOL CenterRudderP = 175
SYMBOL RightRudderP = 210
SYMBOL RudderStep = 3 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
SYMBOL RudderHoldTime = 25 ' Adjust RudderHoldTime to change how long the
' rudder servo is held at it extreme movement.
' Larger numbers will hold the servo position
' longer. Multiply RudderHoldTime by 0.02 seconds
' to calculate the aproximate hold time.
' A value of 25 will hold the position for half
' a second.
'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second
'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second
'SYMBOL BallastSubmergedTime = 400
' To save memory only use one time instead of the three above.
SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second
SYMBOL BallastCo2Time = 10
SYMBOL FinSideAP = 152
SYMBOL FinSideBP = 210
SYMBOL FinTowardAS = 0
SYMBOL FinTowardBS = 1
SYMBOL RudderCenteredS = 0
SYMBOL RudderToRightS = 1
SYMBOL RudderRightS = 2
SYMBOL RudderFromRightS = 3
SYMBOL RudderToLeftS = 4
SYMBOL RudderLeftS = 5
SYMBOL RudderFromLeftS = 6
SYMBOL BallastSurfaceAllClosedS = 0
SYMBOL BallastGoingDownOpenS = 1
SYMBOL BallastSubmergedAllClosedS = 2
SYMBOL BallastGoingUpCo2S = 3
' Add a counter for balast timing. Also add balast states to be checked by
' the program.
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = CenterRudderP
finPosition = FinSideBP
rudderState = RudderCenteredS
'finState = FinTowardAS ' I assume these will already be zero
'ballastState = BallastSurfaceAllClosedS
'ballastCounter = 0
'-------------------- Beginning of MainLoop -------------------------
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = FinTowardBS THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft
IF rudderState = RudderLeftS THEN rudderCheckLeft
IF rudderState = RudderToLeftS THEN rudderCheckToLeft
IF rudderState = RudderFromRightS THEN rudderCheckFromRight
IF rudderState = RudderRightS THEN rudderCheckRight
IF rudderState = RudderToRightS THEN rudderCheckToRight
afterRudderCheck:
IF ballastState = BallastGoingUpCO2S THEN ballastCheckCO2
IF ballastState = BallastSubmergedAllClosedS THEN ballastCheckSubmerged
IF ballastState = BallastGoingDownOpenS THEN ballastCheckGoingDown
GOTO ballastCheckSurface
afterBallastCheck:
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'-------------------- End of MainLoop -------------------------
turnLeft:
rudderState = RudderToLeftS
GOTO afterButtonCheck
turnRight:
rudderState = RudderToRightS
GOTO afterButtonCheck
finCheckToB:
finPosition = finPosition + finStep
IF finPosition >= FinSideBP THEN reverseFinToA
GOTO afterFinCheck
reverseFinToA:
finPosition = FinSideBP
finState = FinTowardAS
GOTO afterFinCheck
finCheckToA:
finPosition = finPosition - finStep
IF finPosition <= FinSideAP THEN reverseFinToB
GOTO afterFinCheck
reverseFinToB:
finPosition = FinSideAP
finState = FinTowardBS
GOTO afterFinCheck
rudderCheckFromLeft:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= CenterRudderP THEN stopRudderCenterFL
GOTO afterRudderCheck
stopRudderCenterFL:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckLeft:
rudderHoldCounter = rudderHoldCounter + 1
IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFL
GOTO afterRudderCheck
moveRudderCenterFL:
rudderState = RudderFromLeftS
GOTO afterRudderCheck
rudderCheckToLeft:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= LeftRudderP THEN stopRudderLeft
GOTO afterRudderCheck
stopRudderLeft:
rudderState = RudderLeftS
rudderPosition = LeftRudderP
rudderHoldCounter = 0
GOTO afterRudderCheck
rudderCheckFromRight:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR
GOTO afterRudderCheck
stopRudderCenterFR:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckRight:
rudderHoldCounter = rudderHoldCounter + 1
IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFR
GOTO afterRudderCheck
moveRudderCenterFR:
rudderState = RudderFromRightS
GOTO afterRudderCheck
rudderCheckToRight:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= RightRudderP THEN stopRudderRight
GOTO afterRudderCheck
stopRudderRight:
rudderState = RudderRightS
rudderPosition = RightRudderP
rudderHoldCounter = 0
GOTO afterRudderCheck
ballastCheckCO2:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastCo2Time THEN closeCO2
GOTO afterBallastCheck
closeCO2:
LOW 2
ballastState = BallastSurfaceAllClosedS
ballastCheckSubmerged:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN blowTank
GOTO afterBallastCheck
blowTank:
HIGH 2
ballastState = BallastGoingUpCO2S
GOTO afterBallastCheck
ballastCheckGoingDown:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN closeTank
GOTO afterBallastCheck
closeTank:
LOW 3
ballastState = BallastSubmergedAllClosedS
GOTO afterBallastCheck
ballastCheckSurface:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN floodTank
GOTO afterBallastCheck
floodTank:
HIGH 3
ballastState = BallastGoingDownOpenS
GOTO afterBallastCheck
I doubt this program will fit in the BS1.
I wonder if instead of using variables for the various states if it would save memory just to plug the number in.
So instead of:
IF finState = FinTowardBS THEN finCheckToB
You used:
IF finState = 1 THEN finCheckToB
Replace all the occuances "FinTowardBS" with "1" and delete (or comment out).
SYMBOL FinTowardBS = 1
And do the same thing for all the states. Does this save memory?
I'm going to try to change the rudder code to see if I can reduce the memory it uses.
I switched them all like you said. If it did save memory, it wasn't enough. It won't show me how much i'm over either. If i click run it says eeprom full and highlights "BallastTime"
it didn't change the amount of memory used. double check me to see if i understood what you were talking about and that i did it right.
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
'SYMBOL LeftRudderP = 150
'SYMBOL CenterRudderP = 175
'SYMBOL RightRudderP = 210
'SYMBOL RudderStep = 3 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL FinStep = 5 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL RudderHoldTime = 35 ' Adjust RudderHoldTime to change how long the
' rudder servo is held at it extreme movement.
' Larger numbers will hold the servo position
' longer. Multiply RudderHoldTime by 0.02 seconds
' to calculate the aproximate hold time.
' A value of 25 will hold the position for half
' a second.
'SYMBOL FinSideAP = 152
'SYMBOL FinSideBP = 210
'SYMBOL FinTowardAS = 0
'SYMBOL FinTowardBS = 1
'SYMBOL RudderCenteredS = 0
'RudderToRightS = 1
'SYMBOL RudderRightS = 2
'SYMBOL RudderFromRightS = 3
'SYMBOL RudderToLeftS = 4
'SYMBOL RudderLeftS = 5
'SYMBOL RudderFromLeftS = 6
' Add a counter for ballast timing. Also add ballast states to be checked by
' the program.
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = 175 'centerRudderP
finPosition = 152
rudderState = 0
finState = 1
'
Beginning of MainLoop
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = 1 THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = 6 THEN rudderCheckFromLeft
IF rudderState = 5 THEN rudderCheckLeft
IF rudderState = 4 THEN rudderCheckToLeft
IF rudderState = 3 THEN rudderCheckFromRight
IF rudderState = 2 THEN rudderCheckRight
IF rudderState = 1 THEN rudderCheckToRight
afterRudderCheck:
' Add checks of ballast states here.
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'
End of MainLoop
turnLeft:
rudderState = 4
GOTO afterButtonCheck
I was watching the Parallax Expo feed while I wrote this so I'm not sure how buggy it is.
I took out some of the rudder and ballast states to try to make it fit.
There's two versions. One with variable states and one with numbers.
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
SYMBOL ballastState = B6
SYMBOL ballastCounter = B7
SYMBOL LeftRudderP = 150
SYMBOL CenterRudderP = 175
SYMBOL RightRudderP = 210
SYMBOL RudderStep = 2 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second
'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second
'SYMBOL BallastSubmergedTime = 400
' To save memory only use one time instead of the three above.
SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second
SYMBOL BallastCo2Time = 10
SYMBOL FinSideAP = 152
SYMBOL FinSideBP = 210
SYMBOL FinTowardAS = 0
SYMBOL FinTowardBS = 1
SYMBOL RudderCenteredS = 0
SYMBOL RudderToRightS = 1
SYMBOL RudderFromRightS = 2
SYMBOL RudderToLeftS = 3
SYMBOL RudderFromLeftS = 4
SYMBOL BallastSurfaceAllClosedS = 0
SYMBOL BallastGoingDownOpenS = 1
SYMBOL BallastGoingUpCo2S = 2
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = CenterRudderP
finPosition = FinSideBP
rudderState = RudderCenteredS
'finState = FinTowardAS ' I assume these will already be zero
'ballastState = BallastSurfaceAllClosedS
'ballastCounter = 0
'-------------------- Beginning of MainLoop -------------------------
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = FinTowardBS THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft
IF rudderState = RudderToLeftS THEN rudderCheckToLeft
IF rudderState = RudderFromRightS THEN rudderCheckFromRight
IF rudderState = RudderToRightS THEN rudderCheckToRight
afterRudderCheck:
IF ballastState = BallastGoingUpCO2S THEN ballastCheckCO2
IF ballastState = BallastGoingDownOpenS THEN ballastCheckGoingDown
GOTO ballastCheckSurface
afterBallastCheck:
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'-------------------- End of MainLoop -------------------------
turnLeft:
rudderState = RudderToLeftS
GOTO afterButtonCheck
turnRight:
rudderState = RudderToRightS
GOTO afterButtonCheck
finCheckToB:
finPosition = finPosition + finStep
IF finPosition >= FinSideBP THEN reverseFinToA
GOTO afterFinCheck
reverseFinToA:
finPosition = FinSideBP
finState = FinTowardAS
GOTO afterFinCheck
finCheckToA:
finPosition = finPosition - finStep
IF finPosition <= FinSideAP THEN reverseFinToB
GOTO afterFinCheck
reverseFinToB:
finPosition = FinSideAP
finState = FinTowardBS
GOTO afterFinCheck
rudderCheckFromLeft:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= CenterRudderP THEN stopRudderFL
GOTO afterRudderCheck
stopRudderFL:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckToLeft:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= LeftRudderP THEN moveRudderCenterFL
GOTO afterRudderCheck
moveRudderCenterFL:
rudderState = RudderFromLeftS
rudderPosition = LeftRudderP
GOTO afterRudderCheck
rudderCheckFromRight:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR
GOTO afterRudderCheck
stopRudderCenterFR:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckToRight:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= RightRudderP THEN moveRudderCenterFR
GOTO afterRudderCheck
moveRudderCenterFR:
rudderState = RudderFromRightS
GOTO afterRudderCheck
ballastCheckCO2:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastCo2Time THEN closeCO2
GOTO afterBallastCheck
closeCO2:
LOW 2
ballastState = BallastSurfaceAllClosedS
blowTank:
LOW 3 ' close tank
ballastState = BallastGoingUpCO2S
HIGH 2 ' open CO2
GOTO afterBallastCheck
ballastCheckGoingDown:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN blowTank
GOTO afterBallastCheck
ballastCheckSurface:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN floodTank
GOTO afterBallastCheck
floodTank:
HIGH 3
ballastState = BallastGoingDownOpenS
GOTO afterBallastCheck
Here's the version with numbers instead of variables.
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
SYMBOL ballastState = B6
SYMBOL ballastCounter = B7
SYMBOL LeftRudderP = 150
SYMBOL CenterRudderP = 175
SYMBOL RightRudderP = 210
SYMBOL RudderStep = 2 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second
'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second
'SYMBOL BallastSubmergedTime = 400
' To save memory only use one time instead of the three above.
SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second
SYMBOL BallastCo2Time = 10
SYMBOL FinSideAP = 152
SYMBOL FinSideBP = 210
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = CenterRudderP
finPosition = FinSideBP
rudderState = 0
'finState = 0 ' I assume these will already be zero
'ballastState = 0
'ballastCounter = 0
'-------------------- Beginning of MainLoop -------------------------
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = 1 THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = 4 THEN rudderCheckFromLeft
IF rudderState = 3 THEN rudderCheckToLeft
IF rudderState = 2 THEN rudderCheckFromRight
IF rudderState = 1 THEN rudderCheckToRight
afterRudderCheck:
IF ballastState = 2 THEN ballastCheckCO2
IF ballastState = 1 THEN ballastCheckGoingDown
GOTO ballastCheckSurface
afterBallastCheck:
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'-------------------- End of MainLoop -------------------------
turnLeft:
rudderState = 3
GOTO afterButtonCheck
turnRight:
rudderState = 1
GOTO afterButtonCheck
finCheckToB:
finPosition = finPosition + finStep
IF finPosition >= FinSideBP THEN reverseFinToA
GOTO afterFinCheck
reverseFinToA:
finPosition = FinSideBP
finState = 0
GOTO afterFinCheck
finCheckToA:
finPosition = finPosition - finStep
IF finPosition <= FinSideAP THEN reverseFinToB
GOTO afterFinCheck
reverseFinToB:
finPosition = FinSideAP
finState = 1
GOTO afterFinCheck
rudderCheckFromLeft:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= CenterRudderP THEN stopRudderFL
GOTO afterRudderCheck
stopRudderFL:
rudderState = 0
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckToLeft:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= LeftRudderP THEN moveRudderCenterFL
GOTO afterRudderCheck
moveRudderCenterFL:
rudderState = 4
rudderPosition = LeftRudderP
GOTO afterRudderCheck
rudderCheckFromRight:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR
GOTO afterRudderCheck
stopRudderCenterFR:
rudderState = 0
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckToRight:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= RightRudderP THEN moveRudderCenterFR
GOTO afterRudderCheck
moveRudderCenterFR:
rudderState = 2
GOTO afterRudderCheck
ballastCheckCO2:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastCo2Time THEN closeCO2
GOTO afterBallastCheck
closeCO2:
LOW 2
ballastState = 0
blowTank:
LOW 3 ' close tank
ballastState = 2
HIGH 2 ' open CO2
GOTO afterBallastCheck
ballastCheckGoingDown:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN blowTank
GOTO afterBallastCheck
ballastCheckSurface:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN floodTank
GOTO afterBallastCheck
floodTank:
HIGH 3
ballastState = 1
GOTO afterBallastCheck
I forgot to zero the ballast counter with each state change.
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
SYMBOL ballastState = B6
SYMBOL ballastCounter = B7
SYMBOL LeftRudderP = 150
SYMBOL CenterRudderP = 175
SYMBOL RightRudderP = 210
SYMBOL RudderStep = 2 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second
'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second
'SYMBOL BallastSubmergedTime = 400
' To save memory only use one time instead of the three above.
SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second
SYMBOL BallastCo2Time = 10
SYMBOL FinSideAP = 152
SYMBOL FinSideBP = 210
SYMBOL FinTowardAS = 0
SYMBOL FinTowardBS = 1
SYMBOL RudderCenteredS = 0
SYMBOL RudderToRightS = 1
SYMBOL RudderFromRightS = 2
SYMBOL RudderToLeftS = 3
SYMBOL RudderFromLeftS = 4
SYMBOL BallastSurfaceAllClosedS = 0
SYMBOL BallastGoingDownOpenS = 1
SYMBOL BallastGoingUpCo2S = 2
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = CenterRudderP
finPosition = FinSideBP
rudderState = RudderCenteredS
'finState = FinTowardAS ' I assume these will already be zero
'ballastState = BallastSurfaceAllClosedS
'ballastCounter = 0
'-------------------- Beginning of MainLoop -------------------------
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = FinTowardBS THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft
IF rudderState = RudderToLeftS THEN rudderCheckToLeft
IF rudderState = RudderFromRightS THEN rudderCheckFromRight
IF rudderState = RudderToRightS THEN rudderCheckToRight
afterRudderCheck:
IF ballastState = BallastGoingUpCO2S THEN ballastCheckCO2
IF ballastState = BallastGoingDownOpenS THEN ballastCheckGoingDown
GOTO ballastCheckSurface
afterBallastCheck:
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'-------------------- End of MainLoop -------------------------
turnLeft:
rudderState = RudderToLeftS
GOTO afterButtonCheck
turnRight:
rudderState = RudderToRightS
GOTO afterButtonCheck
finCheckToB:
finPosition = finPosition + finStep
IF finPosition >= FinSideBP THEN reverseFinToA
GOTO afterFinCheck
reverseFinToA:
finPosition = FinSideBP
finState = FinTowardAS
GOTO afterFinCheck
finCheckToA:
finPosition = finPosition - finStep
IF finPosition <= FinSideAP THEN reverseFinToB
GOTO afterFinCheck
reverseFinToB:
finPosition = FinSideAP
finState = FinTowardBS
GOTO afterFinCheck
rudderCheckFromLeft:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= CenterRudderP THEN stopRudderFL
GOTO afterRudderCheck
stopRudderFL:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckToLeft:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= LeftRudderP THEN moveRudderCenterFL
GOTO afterRudderCheck
moveRudderCenterFL:
rudderState = RudderFromLeftS
rudderPosition = LeftRudderP
GOTO afterRudderCheck
rudderCheckFromRight:
rudderPosition = rudderPosition - RudderStep
IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR
GOTO afterRudderCheck
stopRudderCenterFR:
rudderState = RudderCenteredS
rudderPosition = CenterRudderP
GOTO afterRudderCheck
rudderCheckToRight:
rudderPosition = rudderPosition + RudderStep
IF rudderPosition >= RightRudderP THEN moveRudderCenterFR
GOTO afterRudderCheck
moveRudderCenterFR:
rudderState = RudderFromRightS
GOTO afterRudderCheck
ballastCheckCO2:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastCo2Time THEN closeCO2
GOTO afterBallastCheck
closeCO2:
LOW 2
ballastState = BallastSurfaceAllClosedS
ballastCounter = 0
GOTO afterBallastCheck
ballastCheckGoingDown:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN blowTank
GOTO afterBallastCheck
blowTank:
LOW 3 ' close tank
ballastState = BallastGoingUpCO2S
HIGH 2 ' open CO2
ballastCounter = 0
GOTO afterBallastCheck
ballastCheckSurface:
ballastCounter = ballastCounter + 1
IF ballastCounter >= BallastTime THEN floodTank
GOTO afterBallastCheck
floodTank:
HIGH 3
ballastState = BallastGoingDownOpenS
ballastCounter = 0
GOTO afterBallastCheck
I also left out one "GOTO". Hopefully this still fits.
it still fits (89%) but the solenoids still aren't clicking. I'm trying to decipher it but this program is way over my skill level. how long into the program does it start to open the ballast?
Pin 3 should go high (opening ballast valve) about 20 seconds after the start of the program. It might take a lot longer, depending on the time the main loop takes.
You could try shortening the pause at the end of the main loop. Try a pause of 10 instead of 15. If the solenoid still takes longer than 20 seconds to open try even shorter pauses.
Pin 3 should stay high for another 20 seconds, then when it goes low, the CO2 solenoid should open as the other one closes. The CO2 should be open for 0.2 seconds and the ballast solenoid should stay closed for a total of 20 seconds. Then it all repeats.
hey tried to shorten the pause to 10 at the end of the main loop and still no dice. then tried shortening it more (all the way down to 1 in increments of 1) and kept testing it and even after running for 30 seconds, it didn't click open.
Grrr.. batteries died. charging now. I'l let you know what the results are late tonight. before they died, it did click one of the solenoids pretty fast, but only one i think tho. I'll try to squeeze some LEDs leads into the heat shrink and work on the body in the mean time haha. Thanks again for all your help!
awesome man it seems to work well. If i adjust the ballast time to anything over 250, it just doesn't turn the solenoids on. How could I let it swim submerged a little longer before it blows the ballast? i tried looking at the counter and seeing if i could alter it somehow, but again your code is over my skill level.
Comments
' {$STAMP BS1}
' {$PBASIC 1.0}
start:
PULSOUT 5,134 'rudder motor go to neutral pos
IF PIN0 = 1 THEN left
IF PIN1 = 1 THEN right
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 ' fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
IF PIN0 = 1 THEN left
IF PIN1 = 1 THEN right
'
ballast routine
HIGH 3 'opens ballast solenoid valve and starts to sink
PAUSE 15000 'allows 15 seconds to flood ballast
LOW 3 'closes ballast
PAUSE 20000 'allows fish to swim and lowest depth for 20 seconds
HIGH 2 'opens solenoid valve
PAUSE 100 'burst of 20 psi CO2 to partially blow ballast
LOW 2 'closes solenoid valve
PAUSE 20000 'swims at new depth for 20 seconds
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
IF PIN0 = 1 THEN left
IF PIN1 = 1 THEN right
GOTO start
'
turning >>fixes tail motor for about 5 seconds while the fin motor continues to oscillate
left:
PULSOUT 5, 252 'left turn
GOTO b
b:
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 'fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
IF PIN0 = 0 THEN start
IF PIN0 = 1 THEN b
GOTO b
right:
PULSOUT 5,210 'right turn
GOTO a
a:
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 'fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
IF PIN1 = 0 THEN Start
IF PIN1 = 1 THEN a
GOTO a
I asked about variables because you could have the pulse amounts for both the fin and rudder stored in variables.
The different parts of the program could change these variables as needed.
You then could use a subroutine to pulse the servos.
Near the beginning of the program you'd use:
You'd want to make a subroutine to pulse the servos based on the values of "rudder" and "fin".
refresh: PULSOUT 5, rudder PULSOUT 6, fin PAUSE 18 ' I don't think you need to pause 20 since the rest of your program will also take time. ' You might want to use a value less than 18 here. RETURNThen thoughout your code you'd call the "refresh" subroutine.
Since the servos need refreshing every 20ms, you don't want long pauses.
The servos will likely start acting eratic with such a long pause between refreshes.
Normally in such a program, you'd have a master loop. Execution can branch from this loop but it should then return to the loop.
Since you need to have multiple things happening at once (servos refreshing, balst purging, etc.), you'll need to repeatedly switch back and forth between these tasks (again, the servos should be refreshed every 20ms).
As I said, I haven't programmed a Basic Stamp 1. Hopefully someone who has will jump in with some examples of controlling multiple servos and monitoring multiple inputs.
I decided for the turning, i can just stop motion of the fin and use the momentum of the fish with the rudder turned then have it go back to swimming straight again after it's done turning. But as far as getting the ballast to work while the servos are moving, it's over my head. here's what I have so far:
' {$STAMP BS1}
' {$PBASIC 1.0}
start:
IF PIN1 = 0 THEN a
IF PIN0 = 0 THEN b
FOR B0=1 TO 10 'fin moves left
PULSOUT 6,210
PAUSE 20
NEXT
FOR B0=1 TO 10 ' fin moves right
PULSOUT 6,152
PAUSE 20
NEXT
GOSUB ballast
ballast:
HIGH 3 'opens ballast solenoid valve and starts to sink
PAUSE 15000 'allows 15 seconds to flood ballast
LOW 3 'closes ballast
PAUSE 20000 'allows fish to swim and lowest depth for 20 seconds
HIGH 2 'opens solenoid valve
PAUSE 100 'burst of 20 psi CO2 to partially blow ballast
LOW 2 'closes solenoid valve
PAUSE 20000 'swims at new depth for 20 seconds
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
HIGH 2
PAUSE 100
LOW 2
PAUSE 20000
GOTO ballast
a:
PULSOUT 5,150
PAUSE 2500
PULSOUT 5, 175
PAUSE 300
IF PIN1 = 1 THEN start
GOTO a
b:
PULSOUT 5, 210
PAUSE 2500
PULSOUT 5, 175
PAUSE 300
IF PIN0 = 1 THEN start
GOTO b
If I take out the ballast program, the motors work perfectly. But when i have it in there, it stops all movement of the motors and just clicks the solenoid valves. Soon as I get this bug worked out and everything running at the same time, ill be able to water proof it and throw it in the pool!
You want the robot to move the fin sevo back and forth from pulse value 152 to pulse value 210. Isn't 134 center?
You're presently giving the fin 0.2 seconds to move from one side to the other. Is this how fast you want it to move? Do you want the servo to jerk from one side to the other or would you prefer a smoother motion?
I think you'd prefer the fin to continue to move at a constant pace no matter what else the program is doing, but you're willing to let the fin stop if needed to perform other tasks. Is this correct?
If the button attached to pin 0 is pressed, you'd like the robot to move the rudder servo to position 210 for 2.5 seconds and then return to position 175 (I assume this is center).
If the button attached to pin 1 is pressed, you want the rudder servo moved to position 150 instead of 210.
Can both buttons be pressed by the robot's actions?
When do you want the balast sequence to start? Do you want the buttons monitored while the balast sequence is in progress (while swimming for 20 seconds in deep water and then in swallow water)?
How many times do you want the balast sequence to execute? Do you want the program to end?
As far as the fin motion, what I have now is just a starting place, I’ll have to see what it does to the motion of the bot in the water, but yes im fine with it just moving from one side to the other.
Ideally the fin would actuate the entire time the bot is turned on no matter what the other parts are doing, but I am willing to let it stop to rudder only. If I can continue the motion throughout the rudder action, that would be great, but either way fine.
You’re correct about the push buttons turning the rudder motor.
I have the head mounted on a springed hinge almost like a bobble head, if it hits a wall, the head turns and hits a push button on the opposite side, so it cannot push both buttons at the same time.
I would like the ballast program start once the fish is turned on and to run continuously and keep looping the entire time the robot is on. So it just keeps diving and surfacing over and over.
I do not want the program to end.
As I wrote the BS1 code below, I learned I much prefer to programming the Propeller over programming a BS1.
I'm hoping someone with more BS1 experience will comment on this code.
Does the code really need to be this complicated?
This is what I came up with to drive the fin and rudder servos. My intention was to smooth out the movements of both the fin and rudder.
I used two state variables; "rudderState" to keep track of which way the rudder should be moving and "finState" to determine which direction to move the fin servo.
I had the rudder hold its position once it reach either the right or the left for a predetermined about of time "RudderHoldTime". The same technique used to monitor the rudder hold time could also be used to monitor the time spent in the various ballast states.
I don't know how much of the BS1's memory this program would use and if there would be enough left over for ballast control.
I don't have the Basic Stamp editor software installed on my computer so I don't know if this program will compile.
It's a much more complicated program than I would normally feel comfortable posting to the forum without testing it first. Hopefully there aren't too many bugs.
Here's the code:
' {$STAMP BS1} ' {$PBASIC 1.0} SYMBOL counter = B0 SYMBOL rudderPosition = B1 SYMBOL finPosition = B2 SYMBOL rudderState = B3 SYMBOL finState = B4 SYMBOL rudderHoldCounter = B5 SYMBOL LeftRudderP = 150 SYMBOL CenterRudderP = 175 SYMBOL RightRudderP = 210 SYMBOL RudderStep = 3 ' Adjust RudderStep to change rudder servo speed. ' Larger numbers will make the servo move faster. SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed. ' Larger numbers will make the servo move faster. SYMBOL RudderHoldTime = 25 ' Adjust RudderHoldTime to change how long the ' rudder servo is held at it extreme movement. ' Larger numbers will hold the servo position ' longer. Multiply RudderHoldTime by 0.02 seconds ' to calculate the aproximate hold time. ' A value of 25 will hold the position for half ' a second. SYMBOL FinSideAP = 152 SYMBOL FinSideBP = 210 SYMBOL FinTowardAS = 0 SYMBOL FinTowardBS = 1 SYMBOL RudderCenteredS = 0 SYMBOL RudderToRightS = 1 SYMBOL RudderRightS = 2 SYMBOL RudderFromRightS = 3 SYMBOL RudderToLeftS = 4 SYMBOL RudderLeftS = 5 SYMBOL RudderFromLeftS = 6 ' Add a counter for ballast timing. Also add ballast states to be checked by ' the program. setup: ' Set initial states and positions. ' The robot starts with the rudder centered ' an the fin in position "A". rudderPosition = centerRudderP finPosition = FinSideAP rudderState = RudderCenteredS finState = FinTowardBS '-------------------- Beginning of MainLoop ------------------------- mainLoop: IF PIN1 = 0 THEN turnLeft IF PIN0 = 0 THEN turnRight afterButtonCheck: IF finState = FinTowardBS THEN finCheckToB GOTO finCheckToA afterFinCheck: IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft IF rudderState = RudderLeftS THEN rudderCheckLeft IF rudderState = RudderToLeftS THEN rudderCheckToLeft IF rudderState = RudderFromRightS THEN rudderCheckFromRight IF rudderState = RudderRightS THEN rudderCheckRight IF rudderState = RudderToRightS THEN rudderCheckToRight afterRudderCheck: ' Add checks of ballast states here. ' Now that the servo pulse times have all been set, pulse the servos. PULSOUT 5, rudderPosition PULSOUT 6, finPosition PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms. GOTO mainLoop '-------------------- End of MainLoop ------------------------- turnLeft: rudderState = RudderToLeftS GOTO afterButtonCheck turnRight: rudderState = RudderToRightS GOTO afterButtonCheck finCheckToB: finPosition = finPosition + finStep IF finPosition >= FinSideBP THEN reverseFinToA GOTO afterFinCheck reverseFinToA: finPosition = FinSideBP finState = FinTowardAS GOTO afterFinCheck finCheckToA: finPosition = finPosition - finStep IF finPosition <= FinSideAP THEN reverseFinToB GOTO afterFinCheck reverseFinToB: finPosition = FinSideAP finState = FinTowardBS GOTO afterFinCheck rudderCheckFromLeft: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= CenterRudderP THEN stopRudderCenterFL GOTO afterRudderCheck stopRudderCenterFL: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckLeft: rudderHoldCounter = rudderHoldCounter + 1 IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFL GOTO afterRudderCheck moveRudderCenterFL: rudderState = RudderFromLeftS GOTO afterRudderCheck rudderCheckToLeft: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= LeftRudderP THEN stopRudderLeft GOTO afterRudderCheck stopRudderLeft: rudderState = RudderLeftS rudderPosition = LeftRudderP rudderHoldCounter = 0 GOTO afterRudderCheck rudderCheckFromRight: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR GOTO afterRudderCheck stopRudderCenterFR: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckRight: rudderHoldCounter = rudderHoldCounter + 1 IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFR GOTO afterRudderCheck moveRudderCenterFR: rudderState = RudderFromRightS GOTO afterRudderCheck rudderCheckToRight: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= RightRudderP THEN stopRudderRight GOTO afterRudderCheck stopRudderRight: rudderState = RudderRightS rudderPosition = RightRudderP rudderHoldCounter = 0 GOTO afterRudderCheckIs it really this hard to control two servos with the constraints of this project. I keep thinking there must be a better way of doing this.
If this is a valid approach, then the ballast code could be added using the same technique of setting states and counting loops to control the time spent in the various states.
Just so people know, a Propeller QuickStart board costs less than any of the Basic Stamp modules cost. IMO, the software for this project would be easier to write for the Propeller than any of the Basic Stamps. (It's no secret, I'm a big fan of the Propeller (especially for robots).)
p.s: I was wondering if i could use an if.. then.. for if it executes the main loop x times then branches off to a diving routine (either with or without swimming) then executes the main loop x more times, then goes to a surfacing routine. then resets? would that make the program simpler? just a thought though.
Also i agree about the propeller, i should have done more research on controllers before picking the bs1. on future projects i definitely will either go with the bs2 or propeller. It took me almost the whole project time to get the parts in and now im too close to the deadline to order a new controller =/.
Thanks again!
My own programs rarely run right away.
Just as a used a rudder state to determine which way to move the rudder servo, you'll want to use ballast states and a ballast counter to know when to open and close the valves.
How many ballast states does the robot need?
I figure the robot will start at the surface. Maybe call it surfaceS (I added postfixes P for servo positions and S for states). A counter will be set to zero and when the counter reaches a desired amount the robot will change ballast state. This state could be "openBallastValveS". The counter would be zeroed again and the program will start counting loops to hold the valve open. Once the valve has been open long enough, the state would change again with the counter again restarted to use as a timer until the next ballast state.
So what do you want the ballast to do?
The robot swims on the surface for some time. How long?
I can see how long to hold the valves open from your code.
It looks like you want the robot to submerge all in one step and then rise in a series of small steps. Is this correct?
It looks like the robot should swim at the surface for 20s before staring to submerge again. Right?
Does the program editor let you know how much of the BS1 memory has been used? I think the final program could easily be twice as long as it is now.
Solenoid 1: open as soon as the program runs. Close after 10 seconds.
Solenoid2: after solenoid 1 is closed, open for .2 seconds then close. Repeat a few times then goto solenoid 1
The amount of time it swims on the surface doesn’t matter. also the amount of time it swim submerged doesn't matter. as long as it does both at some point. Also I originally intended to sink all in one step, but if its easier to do it gradually, then I’m fine with that.
I'll try to find if it shows the memory used somewhere.
here you go found it, 82% full. yikes!
Yikes, is right.
It might be easier to submerge all at once (and use less memory).
There may be parts of the program as it is now that could be cut. I wanted to show how to use a counter to measure time, but it might not be really needed with the rudder.
' {$STAMP BS1} ' {$PBASIC 1.0} SYMBOL counter = B0 SYMBOL rudderPosition = B1 SYMBOL finPosition = B2 SYMBOL rudderState = B3 SYMBOL finState = B4 SYMBOL rudderHoldCounter = B5 SYMBOL ballastState = B6 SYMBOL ballastCounter = B7 SYMBOL LeftRudderP = 150 SYMBOL CenterRudderP = 175 SYMBOL RightRudderP = 210 SYMBOL RudderStep = 3 ' Adjust RudderStep to change rudder servo speed. ' Larger numbers will make the servo move faster. SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed. ' Larger numbers will make the servo move faster. SYMBOL RudderHoldTime = 25 ' Adjust RudderHoldTime to change how long the ' rudder servo is held at it extreme movement. ' Larger numbers will hold the servo position ' longer. Multiply RudderHoldTime by 0.02 seconds ' to calculate the aproximate hold time. ' A value of 25 will hold the position for half ' a second. 'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second 'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second 'SYMBOL BallastSubmergedTime = 400 ' To save memory only use one time instead of the three above. SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second SYMBOL BallastCo2Time = 10 SYMBOL FinSideAP = 152 SYMBOL FinSideBP = 210 SYMBOL FinTowardAS = 0 SYMBOL FinTowardBS = 1 SYMBOL RudderCenteredS = 0 SYMBOL RudderToRightS = 1 SYMBOL RudderRightS = 2 SYMBOL RudderFromRightS = 3 SYMBOL RudderToLeftS = 4 SYMBOL RudderLeftS = 5 SYMBOL RudderFromLeftS = 6 SYMBOL BallastSurfaceAllClosedS = 0 SYMBOL BallastGoingDownOpenS = 1 SYMBOL BallastSubmergedAllClosedS = 2 SYMBOL BallastGoingUpCo2S = 3 ' Add a counter for balast timing. Also add balast states to be checked by ' the program. setup: ' Set initial states and positions. ' The robot starts with the rudder centered ' an the fin in position "A". rudderPosition = CenterRudderP finPosition = FinSideBP rudderState = RudderCenteredS 'finState = FinTowardAS ' I assume these will already be zero 'ballastState = BallastSurfaceAllClosedS 'ballastCounter = 0 '-------------------- Beginning of MainLoop ------------------------- mainLoop: IF PIN1 = 0 THEN turnLeft IF PIN0 = 0 THEN turnRight afterButtonCheck: IF finState = FinTowardBS THEN finCheckToB GOTO finCheckToA afterFinCheck: IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft IF rudderState = RudderLeftS THEN rudderCheckLeft IF rudderState = RudderToLeftS THEN rudderCheckToLeft IF rudderState = RudderFromRightS THEN rudderCheckFromRight IF rudderState = RudderRightS THEN rudderCheckRight IF rudderState = RudderToRightS THEN rudderCheckToRight afterRudderCheck: IF ballastState = BallastGoingUpCO2S THEN ballastCheckCO2 IF ballastState = BallastSubmergedAllClosedS THEN ballastCheckSubmerged IF ballastState = BallastGoingDownOpenS THEN ballastCheckGoingDown GOTO ballastCheckSurface afterBallastCheck: ' Now that the servo pulse times have all been set, pulse the servos. PULSOUT 5, rudderPosition PULSOUT 6, finPosition PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms. GOTO mainLoop '-------------------- End of MainLoop ------------------------- turnLeft: rudderState = RudderToLeftS GOTO afterButtonCheck turnRight: rudderState = RudderToRightS GOTO afterButtonCheck finCheckToB: finPosition = finPosition + finStep IF finPosition >= FinSideBP THEN reverseFinToA GOTO afterFinCheck reverseFinToA: finPosition = FinSideBP finState = FinTowardAS GOTO afterFinCheck finCheckToA: finPosition = finPosition - finStep IF finPosition <= FinSideAP THEN reverseFinToB GOTO afterFinCheck reverseFinToB: finPosition = FinSideAP finState = FinTowardBS GOTO afterFinCheck rudderCheckFromLeft: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= CenterRudderP THEN stopRudderCenterFL GOTO afterRudderCheck stopRudderCenterFL: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckLeft: rudderHoldCounter = rudderHoldCounter + 1 IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFL GOTO afterRudderCheck moveRudderCenterFL: rudderState = RudderFromLeftS GOTO afterRudderCheck rudderCheckToLeft: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= LeftRudderP THEN stopRudderLeft GOTO afterRudderCheck stopRudderLeft: rudderState = RudderLeftS rudderPosition = LeftRudderP rudderHoldCounter = 0 GOTO afterRudderCheck rudderCheckFromRight: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR GOTO afterRudderCheck stopRudderCenterFR: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckRight: rudderHoldCounter = rudderHoldCounter + 1 IF rudderHoldCounter >= RudderHoldTime THEN moveRudderCenterFR GOTO afterRudderCheck moveRudderCenterFR: rudderState = RudderFromRightS GOTO afterRudderCheck rudderCheckToRight: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= RightRudderP THEN stopRudderRight GOTO afterRudderCheck stopRudderRight: rudderState = RudderRightS rudderPosition = RightRudderP rudderHoldCounter = 0 GOTO afterRudderCheck ballastCheckCO2: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastCo2Time THEN closeCO2 GOTO afterBallastCheck closeCO2: LOW 2 ballastState = BallastSurfaceAllClosedS ballastCheckSubmerged: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN blowTank GOTO afterBallastCheck blowTank: HIGH 2 ballastState = BallastGoingUpCO2S GOTO afterBallastCheck ballastCheckGoingDown: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN closeTank GOTO afterBallastCheck closeTank: LOW 3 ballastState = BallastSubmergedAllClosedS GOTO afterBallastCheck ballastCheckSurface: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN floodTank GOTO afterBallastCheck floodTank: HIGH 3 ballastState = BallastGoingDownOpenS GOTO afterBallastCheckI doubt this program will fit in the BS1.
I wonder if instead of using variables for the various states if it would save memory just to plug the number in.
So instead of:
You used:
Replace all the occuances "FinTowardBS" with "1" and delete (or comment out).
And do the same thing for all the states. Does this save memory?
I'm going to try to change the rudder code to see if I can reduce the memory it uses.
If you use numbers instead of state variables, does it leave more memory unused?
' {$STAMP BS1}
' {$PBASIC 1.0}
SYMBOL counter = B0
SYMBOL rudderPosition = B1
SYMBOL finPosition = B2
SYMBOL rudderState = B3
SYMBOL finState = B4
SYMBOL rudderHoldCounter = B5
'SYMBOL LeftRudderP = 150
'SYMBOL CenterRudderP = 175
'SYMBOL RightRudderP = 210
'SYMBOL RudderStep = 3 ' Adjust RudderStep to change rudder servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL FinStep = 5 ' Adjust FinStep to change fin servo speed.
' Larger numbers will make the servo move faster.
'SYMBOL RudderHoldTime = 35 ' Adjust RudderHoldTime to change how long the
' rudder servo is held at it extreme movement.
' Larger numbers will hold the servo position
' longer. Multiply RudderHoldTime by 0.02 seconds
' to calculate the aproximate hold time.
' A value of 25 will hold the position for half
' a second.
'SYMBOL FinSideAP = 152
'SYMBOL FinSideBP = 210
'SYMBOL FinTowardAS = 0
'SYMBOL FinTowardBS = 1
'SYMBOL RudderCenteredS = 0
'RudderToRightS = 1
'SYMBOL RudderRightS = 2
'SYMBOL RudderFromRightS = 3
'SYMBOL RudderToLeftS = 4
'SYMBOL RudderLeftS = 5
'SYMBOL RudderFromLeftS = 6
' Add a counter for ballast timing. Also add ballast states to be checked by
' the program.
setup:
' Set initial states and positions.
' The robot starts with the rudder centered
' an the fin in position "A".
rudderPosition = 175 'centerRudderP
finPosition = 152
rudderState = 0
finState = 1
'
Beginning of MainLoop
mainLoop:
IF PIN1 = 0 THEN turnLeft
IF PIN0 = 0 THEN turnRight
afterButtonCheck:
IF finState = 1 THEN finCheckToB
GOTO finCheckToA
afterFinCheck:
IF rudderState = 6 THEN rudderCheckFromLeft
IF rudderState = 5 THEN rudderCheckLeft
IF rudderState = 4 THEN rudderCheckToLeft
IF rudderState = 3 THEN rudderCheckFromRight
IF rudderState = 2 THEN rudderCheckRight
IF rudderState = 1 THEN rudderCheckToRight
afterRudderCheck:
' Add checks of ballast states here.
' Now that the servo pulse times have all been set, pulse the servos.
PULSOUT 5, rudderPosition
PULSOUT 6, finPosition
PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms.
GOTO mainLoop
'
End of MainLoop
turnLeft:
rudderState = 4
GOTO afterButtonCheck
turnRight:
rudderState = 1
GOTO afterButtonCheck
finCheckToB:
finPosition = finPosition + 5
IF finPosition >= 210 THEN reverseFinToA
GOTO afterFinCheck
reverseFinToA:
finPosition = 210
finState = 0
GOTO afterFinCheck
ect...
I took out some of the rudder and ballast states to try to make it fit.
There's two versions. One with variable states and one with numbers.
' {$STAMP BS1} ' {$PBASIC 1.0} SYMBOL counter = B0 SYMBOL rudderPosition = B1 SYMBOL finPosition = B2 SYMBOL rudderState = B3 SYMBOL finState = B4 SYMBOL rudderHoldCounter = B5 SYMBOL ballastState = B6 SYMBOL ballastCounter = B7 SYMBOL LeftRudderP = 150 SYMBOL CenterRudderP = 175 SYMBOL RightRudderP = 210 SYMBOL RudderStep = 2 ' Adjust RudderStep to change rudder servo speed. ' Larger numbers will make the servo move faster. SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed. ' Larger numbers will make the servo move faster. 'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second 'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second 'SYMBOL BallastSubmergedTime = 400 ' To save memory only use one time instead of the three above. SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second SYMBOL BallastCo2Time = 10 SYMBOL FinSideAP = 152 SYMBOL FinSideBP = 210 SYMBOL FinTowardAS = 0 SYMBOL FinTowardBS = 1 SYMBOL RudderCenteredS = 0 SYMBOL RudderToRightS = 1 SYMBOL RudderFromRightS = 2 SYMBOL RudderToLeftS = 3 SYMBOL RudderFromLeftS = 4 SYMBOL BallastSurfaceAllClosedS = 0 SYMBOL BallastGoingDownOpenS = 1 SYMBOL BallastGoingUpCo2S = 2 setup: ' Set initial states and positions. ' The robot starts with the rudder centered ' an the fin in position "A". rudderPosition = CenterRudderP finPosition = FinSideBP rudderState = RudderCenteredS 'finState = FinTowardAS ' I assume these will already be zero 'ballastState = BallastSurfaceAllClosedS 'ballastCounter = 0 '-------------------- Beginning of MainLoop ------------------------- mainLoop: IF PIN1 = 0 THEN turnLeft IF PIN0 = 0 THEN turnRight afterButtonCheck: IF finState = FinTowardBS THEN finCheckToB GOTO finCheckToA afterFinCheck: IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft IF rudderState = RudderToLeftS THEN rudderCheckToLeft IF rudderState = RudderFromRightS THEN rudderCheckFromRight IF rudderState = RudderToRightS THEN rudderCheckToRight afterRudderCheck: IF ballastState = BallastGoingUpCO2S THEN ballastCheckCO2 IF ballastState = BallastGoingDownOpenS THEN ballastCheckGoingDown GOTO ballastCheckSurface afterBallastCheck: ' Now that the servo pulse times have all been set, pulse the servos. PULSOUT 5, rudderPosition PULSOUT 6, finPosition PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms. GOTO mainLoop '-------------------- End of MainLoop ------------------------- turnLeft: rudderState = RudderToLeftS GOTO afterButtonCheck turnRight: rudderState = RudderToRightS GOTO afterButtonCheck finCheckToB: finPosition = finPosition + finStep IF finPosition >= FinSideBP THEN reverseFinToA GOTO afterFinCheck reverseFinToA: finPosition = FinSideBP finState = FinTowardAS GOTO afterFinCheck finCheckToA: finPosition = finPosition - finStep IF finPosition <= FinSideAP THEN reverseFinToB GOTO afterFinCheck reverseFinToB: finPosition = FinSideAP finState = FinTowardBS GOTO afterFinCheck rudderCheckFromLeft: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= CenterRudderP THEN stopRudderFL GOTO afterRudderCheck stopRudderFL: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckToLeft: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= LeftRudderP THEN moveRudderCenterFL GOTO afterRudderCheck moveRudderCenterFL: rudderState = RudderFromLeftS rudderPosition = LeftRudderP GOTO afterRudderCheck rudderCheckFromRight: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR GOTO afterRudderCheck stopRudderCenterFR: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckToRight: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= RightRudderP THEN moveRudderCenterFR GOTO afterRudderCheck moveRudderCenterFR: rudderState = RudderFromRightS GOTO afterRudderCheck ballastCheckCO2: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastCo2Time THEN closeCO2 GOTO afterBallastCheck closeCO2: LOW 2 ballastState = BallastSurfaceAllClosedS blowTank: LOW 3 ' close tank ballastState = BallastGoingUpCO2S HIGH 2 ' open CO2 GOTO afterBallastCheck ballastCheckGoingDown: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN blowTank GOTO afterBallastCheck ballastCheckSurface: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN floodTank GOTO afterBallastCheck floodTank: HIGH 3 ballastState = BallastGoingDownOpenS GOTO afterBallastCheckHere's the version with numbers instead of variables.
' {$STAMP BS1} ' {$PBASIC 1.0} SYMBOL counter = B0 SYMBOL rudderPosition = B1 SYMBOL finPosition = B2 SYMBOL rudderState = B3 SYMBOL finState = B4 SYMBOL rudderHoldCounter = B5 SYMBOL ballastState = B6 SYMBOL ballastCounter = B7 SYMBOL LeftRudderP = 150 SYMBOL CenterRudderP = 175 SYMBOL RightRudderP = 210 SYMBOL RudderStep = 2 ' Adjust RudderStep to change rudder servo speed. ' Larger numbers will make the servo move faster. SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed. ' Larger numbers will make the servo move faster. 'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second 'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second 'SYMBOL BallastSubmergedTime = 400 ' To save memory only use one time instead of the three above. SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second SYMBOL BallastCo2Time = 10 SYMBOL FinSideAP = 152 SYMBOL FinSideBP = 210 setup: ' Set initial states and positions. ' The robot starts with the rudder centered ' an the fin in position "A". rudderPosition = CenterRudderP finPosition = FinSideBP rudderState = 0 'finState = 0 ' I assume these will already be zero 'ballastState = 0 'ballastCounter = 0 '-------------------- Beginning of MainLoop ------------------------- mainLoop: IF PIN1 = 0 THEN turnLeft IF PIN0 = 0 THEN turnRight afterButtonCheck: IF finState = 1 THEN finCheckToB GOTO finCheckToA afterFinCheck: IF rudderState = 4 THEN rudderCheckFromLeft IF rudderState = 3 THEN rudderCheckToLeft IF rudderState = 2 THEN rudderCheckFromRight IF rudderState = 1 THEN rudderCheckToRight afterRudderCheck: IF ballastState = 2 THEN ballastCheckCO2 IF ballastState = 1 THEN ballastCheckGoingDown GOTO ballastCheckSurface afterBallastCheck: ' Now that the servo pulse times have all been set, pulse the servos. PULSOUT 5, rudderPosition PULSOUT 6, finPosition PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms. GOTO mainLoop '-------------------- End of MainLoop ------------------------- turnLeft: rudderState = 3 GOTO afterButtonCheck turnRight: rudderState = 1 GOTO afterButtonCheck finCheckToB: finPosition = finPosition + finStep IF finPosition >= FinSideBP THEN reverseFinToA GOTO afterFinCheck reverseFinToA: finPosition = FinSideBP finState = 0 GOTO afterFinCheck finCheckToA: finPosition = finPosition - finStep IF finPosition <= FinSideAP THEN reverseFinToB GOTO afterFinCheck reverseFinToB: finPosition = FinSideAP finState = 1 GOTO afterFinCheck rudderCheckFromLeft: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= CenterRudderP THEN stopRudderFL GOTO afterRudderCheck stopRudderFL: rudderState = 0 rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckToLeft: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= LeftRudderP THEN moveRudderCenterFL GOTO afterRudderCheck moveRudderCenterFL: rudderState = 4 rudderPosition = LeftRudderP GOTO afterRudderCheck rudderCheckFromRight: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR GOTO afterRudderCheck stopRudderCenterFR: rudderState = 0 rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckToRight: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= RightRudderP THEN moveRudderCenterFR GOTO afterRudderCheck moveRudderCenterFR: rudderState = 2 GOTO afterRudderCheck ballastCheckCO2: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastCo2Time THEN closeCO2 GOTO afterBallastCheck closeCO2: LOW 2 ballastState = 0 blowTank: LOW 3 ' close tank ballastState = 2 HIGH 2 ' open CO2 GOTO afterBallastCheck ballastCheckGoingDown: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN blowTank GOTO afterBallastCheck ballastCheckSurface: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN floodTank GOTO afterBallastCheck floodTank: HIGH 3 ballastState = 1 GOTO afterBallastCheckI'm not sure what to do if this doesn't fit.
How much room left?
Does one take less memory than the other?
' {$STAMP BS1} ' {$PBASIC 1.0} SYMBOL counter = B0 SYMBOL rudderPosition = B1 SYMBOL finPosition = B2 SYMBOL rudderState = B3 SYMBOL finState = B4 SYMBOL rudderHoldCounter = B5 SYMBOL ballastState = B6 SYMBOL ballastCounter = B7 SYMBOL LeftRudderP = 150 SYMBOL CenterRudderP = 175 SYMBOL RightRudderP = 210 SYMBOL RudderStep = 2 ' Adjust RudderStep to change rudder servo speed. ' Larger numbers will make the servo move faster. SYMBOL FinStep = 3 ' Adjust FinStep to change fin servo speed. ' Larger numbers will make the servo move faster. 'SYMBOL BallastSurfaceTime = 400 ' 20 seconds times 20 loops per second 'SYMBOL BallastOpenTankTime = 300 ' 15 seconds times 20 loops per second 'SYMBOL BallastSubmergedTime = 400 ' To save memory only use one time instead of the three above. SYMBOL BallastTime = 400 ' 20 seconds times 20 loops per second SYMBOL BallastCo2Time = 10 SYMBOL FinSideAP = 152 SYMBOL FinSideBP = 210 SYMBOL FinTowardAS = 0 SYMBOL FinTowardBS = 1 SYMBOL RudderCenteredS = 0 SYMBOL RudderToRightS = 1 SYMBOL RudderFromRightS = 2 SYMBOL RudderToLeftS = 3 SYMBOL RudderFromLeftS = 4 SYMBOL BallastSurfaceAllClosedS = 0 SYMBOL BallastGoingDownOpenS = 1 SYMBOL BallastGoingUpCo2S = 2 setup: ' Set initial states and positions. ' The robot starts with the rudder centered ' an the fin in position "A". rudderPosition = CenterRudderP finPosition = FinSideBP rudderState = RudderCenteredS 'finState = FinTowardAS ' I assume these will already be zero 'ballastState = BallastSurfaceAllClosedS 'ballastCounter = 0 '-------------------- Beginning of MainLoop ------------------------- mainLoop: IF PIN1 = 0 THEN turnLeft IF PIN0 = 0 THEN turnRight afterButtonCheck: IF finState = FinTowardBS THEN finCheckToB GOTO finCheckToA afterFinCheck: IF rudderState = RudderFromLeftS THEN rudderCheckFromLeft IF rudderState = RudderToLeftS THEN rudderCheckToLeft IF rudderState = RudderFromRightS THEN rudderCheckFromRight IF rudderState = RudderToRightS THEN rudderCheckToRight afterRudderCheck: IF ballastState = BallastGoingUpCO2S THEN ballastCheckCO2 IF ballastState = BallastGoingDownOpenS THEN ballastCheckGoingDown GOTO ballastCheckSurface afterBallastCheck: ' Now that the servo pulse times have all been set, pulse the servos. PULSOUT 5, rudderPosition PULSOUT 6, finPosition PAUSE 15 ' This should be adjusted so a complete loop takes about 20 ms. GOTO mainLoop '-------------------- End of MainLoop ------------------------- turnLeft: rudderState = RudderToLeftS GOTO afterButtonCheck turnRight: rudderState = RudderToRightS GOTO afterButtonCheck finCheckToB: finPosition = finPosition + finStep IF finPosition >= FinSideBP THEN reverseFinToA GOTO afterFinCheck reverseFinToA: finPosition = FinSideBP finState = FinTowardAS GOTO afterFinCheck finCheckToA: finPosition = finPosition - finStep IF finPosition <= FinSideAP THEN reverseFinToB GOTO afterFinCheck reverseFinToB: finPosition = FinSideAP finState = FinTowardBS GOTO afterFinCheck rudderCheckFromLeft: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= CenterRudderP THEN stopRudderFL GOTO afterRudderCheck stopRudderFL: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckToLeft: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= LeftRudderP THEN moveRudderCenterFL GOTO afterRudderCheck moveRudderCenterFL: rudderState = RudderFromLeftS rudderPosition = LeftRudderP GOTO afterRudderCheck rudderCheckFromRight: rudderPosition = rudderPosition - RudderStep IF rudderPosition <= CenterRudderP THEN stopRudderCenterFR GOTO afterRudderCheck stopRudderCenterFR: rudderState = RudderCenteredS rudderPosition = CenterRudderP GOTO afterRudderCheck rudderCheckToRight: rudderPosition = rudderPosition + RudderStep IF rudderPosition >= RightRudderP THEN moveRudderCenterFR GOTO afterRudderCheck moveRudderCenterFR: rudderState = RudderFromRightS GOTO afterRudderCheck ballastCheckCO2: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastCo2Time THEN closeCO2 GOTO afterBallastCheck closeCO2: LOW 2 ballastState = BallastSurfaceAllClosedS ballastCounter = 0 GOTO afterBallastCheck ballastCheckGoingDown: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN blowTank GOTO afterBallastCheck blowTank: LOW 3 ' close tank ballastState = BallastGoingUpCO2S HIGH 2 ' open CO2 ballastCounter = 0 GOTO afterBallastCheck ballastCheckSurface: ballastCounter = ballastCounter + 1 IF ballastCounter >= BallastTime THEN floodTank GOTO afterBallastCheck floodTank: HIGH 3 ballastState = BallastGoingDownOpenS ballastCounter = 0 GOTO afterBallastCheckI also left out one "GOTO". Hopefully this still fits.
You could try shortening the pause at the end of the main loop. Try a pause of 10 instead of 15. If the solenoid still takes longer than 20 seconds to open try even shorter pauses.
Pin 3 should stay high for another 20 seconds, then when it goes low, the CO2 solenoid should open as the other one closes. The CO2 should be open for 0.2 seconds and the ballast solenoid should stay closed for a total of 20 seconds. Then it all repeats.
To:
For debugging purposes.
Hopefully it wont take so long to see a change.
Would it be hard to switch the solenoids for LEDs until it's working properly?
We could make it a word and it should work fine with larger values.
Would you do another screen grab of the memory use chart? It might help.