Joystick servo control using BOE

Alexander14

Hello

I am using a BOE and Parallax's Gimble joystick to control (2) Hi-Tech servos off of ports 12 and 13. I have setup the circuit from your example shown in your PDF for the Gimble joystick and all is working well. Here is the example program I am using to control the (2) Hitech servos:

' {$STAMP BS2p}'
' {$PORT COM8} '


' {$PBASIC 2.5} '

LeftRight VAR Word
UpDown VAR Word

DO

HIGH 4
PAUSE 5
RCTIME 4, 1, UpDown

HIGH 11
PAUSE 5
RCTIME 11, 1, LeftRight

DEBUG HOME, "UpDown: ", DEC UpDown, CLREOL, CR,
"LeftRight: ", DEC LeftRight, CLREOL
'PAUSE 5'

PULSOUT 12 , UpDown + 1400
PULSOUT 13 , LeftRight + 1400
LOOP

_______

The problem I am having is since the UpDown and LeftRight variables are storing only incremental, single digit changes in the joystick position, I can only get the servo to move about 15 degrees from center in either direction from both vertical and horizontal movement of the joystick instead of the complete 180 degree movement capability of the servo. Here are the parameters of the readings for the variables for UpDown and LeftRight readings:

Vertical Center - 343
Vertical Full forward - 445
Vertical Full Reverse - 244

Horizontal Center - 332
Horizontal Full Left - 240
Horizontal Full Right - 430

As you can see, I added 1400 to each variable for my pulsout command to center the servos on startup and from there they will move about 15 degrees from center in either direction when executing the program. How do I get the servos to move smoothly thru the entire range of the servos capabilities?

Any and all help is appreciated!

Alex

tomcrawford

deleted

tomcrawford

You can multiply the delta from "center" prior to adding the 1400.

tomcrawford

For either axis, the input value is 340 +- 100 (more or less).
You need to map that to 750 +- 250 (assuming servos that expect 1 ms to 2 ms pulses)

So, I would calculate an offset from center (left right - 340)
Then multiply that by 2.5 (see multiply middle on page 112 of the big yellow book)
Add 750 to get the PULSOUT argument

Go through the loop every 20 msec.

Alexander14

Thank you very much for your input Tom I should also point out the following for the servo settings:

Hitec Servos

Using pulsout settings the mid, max and min position points are:

3000, 1760 and 580

I am not a programmer so examples of what you are talking about with offsets etc would be most helpful.

Thanks!

Alex

tomcrawford

I'm confused about the numbers in the previous comment (3000, 1760, 580). Are those msecs, PULSOUT arguments, or what. If PULSOUT, which BS-2 version.

Could you post a link to the data sheet for the exact servo you are using?

In the meantime, I am going to assume 2.1 msec to 0.9 sec.

Alexander14

Those are the Pulsout numbers that are used for the Hitec HS-475HB servo for its full range of motion. All Hitec servos run in this range using Pulsout. I ran the program enclosed above to find their full range numbers.

tomcrawford

I have attached a program that should help you get started. Observe that I have neither the joystick nor the servo you are using. However, the program compiles and does what I expected in engineering mode, including generating correct length pulses.

Comment out the #DEFINE Engmode at the beginning to run normally. I would run it in eng mode first to try to get a feel for what the program is doing. Note eng mode has its own entry to OneAxis that skips over the reading the joystick.

The main program passes the pin numbers and the mid-range joystick value for one axis at a time and calls OneAxis.

OneAxis reads the appropriate joystick pin, calculates the offset from the neutral position, scales to convert +- 100 to +-300, combines the result with the servo mid position value and pulses the appropriate servo pin. Note that on a green BS-2, PULSOUT values are in 2 usec units.

I used 2100 and 900 usec for the servo limits because that is what their data sheet calls for. You will probably have to adjust the PAUSE at the end of the main loop.

Hope this works for you.

Duane Degn

The code snippet above lists the processor as a BS2p. Is this correct?

If so you'll need to adjust Tom's code to generate pulses based on 0.8us per unit rather than 2us per unit of a normal BS2.

This is what I came up with:

'{$STAMP BS2p}
'{$PORT COM8} '
'{$PBASIC 2.5}

LeftRight VAR Word
UpDown VAR Word
outputX VAR Word
outputY VAR Word
MIN_PULSE_900US CON 1125
'MAX_PULSE_2100US CON 2625
'RANGE_1200US CON 1500
S_RANGE_1200US CON 150 'scaled range to prevent overflow
MIN_INPUT_X CON 240
'MAX_INPUT_X CON 430
'RANGE_X CON 190
S_RANGE_X CON 19 ' scaled range
MIN_INPUT_Y CON 244
'MAX_INPUT_Y CON 445
'RANGE_Y CON 201
S_RANGE_Y CON 20 
DO

HIGH 4
PAUSE 5
RCTIME 4, 1, UpDown

HIGH 11
PAUSE 5
RCTIME 11, 1, LeftRight

outputY = MIN_PULSE_900US + (((UpDown - MIN_INPUT_Y) * S_RANGE_1200US) / S_RANGE_Y)
outputX = MIN_PULSE_900US + (((LeftRight - MIN_INPUT_X) * S_RANGE_1200US) / S_RANGE_X)
DEBUG HOME, "UpDown: ", DEC UpDown, CLREOL, CR,
"LeftRight: ", DEC LeftRight, CLREOL, CR,
"outputY: ", DEC outputY, CLREOL, CR,
"outputX: ", DEC outputX, CLREOL
'PAUSE 5'

PULSOUT 12 , outputY
PULSOUT 13 , outputX
LOOP

I haven't tried to run nor compile the code. The main point I wanted to show was how to scale the input pulse to the output pulse. You have to be careful about overflowing your 16-bit values. This is why I scaled the input and output ranges by ten.

tomcrawford

Oops, didn't see that. If you want to press on with my code, change the following:

ServoMid CON 1875 '1875 times 0.8 = 1500 sec
Scaler CON $0780 '+- 100 scales to +- 750

If you want to go with Duane's example, I won't feel slighted . His code generally works well. :thumb:

tomcrawford

Here is a version that automatically adapts to BS2 vs BS2P. I also added limits checking to avoid invalid pulse width.

Alexander14

Tom / Duane:

Thank you both for your extensive help with this and providing code for me to try. I VERY much appreciate your time and efforts helping me I have multiple BOE boards with both BS2 and BS2P controllers. I will try both versions with the code you have both supplied and get back with you on my success. Thanks to you both for your help and time so far )

Alex

Alexander14

Tom, your code worked flawlessly "right out of the box" wow! I have smooth movement and great response thru the normal servo range If I wanted to add 1 or 2 more servos to ports 14 or 15 of the BOE, what would I do within the code? I will be adding a second joystick to ports 2 and 10 of the BOE for this.

Duane, your code worked as well but their seems to be a few issues with it. The servos do not move smoothly when moving thru their positions and a full left on the X axis and a full down of the Y axis will make the servo go in the opposite direction to the other side of its movement. This does not happen with full up and full right for some reason.

Any ideas?

Thanks to you both for your time and efforts in helping me

Alex

Duane Degn

Alexander14 wrote: »

Duane, your code worked as well but their seems to be a few issues with it. The servos do not move smoothly when moving thru their positions and a full left on the X axis and a full down of the Y axis will make the servo go in the opposite direction to the other side of its movement. This does not happen with full up and full right for some reason.

Any ideas?

My first guess is the numbers are overflowing the 16-bit words.

There could also be a problem when inputs are outside of the range you mentioned earlier. I didn't add a check on limits like Tom did.

It seems like the easiest solution is to use Tom's code.

What did the debug statements read when the servos moved the wrong direction?


What's your end goal with this project?

You mention Tom's code provides smooth movement. Is it smooth enough for your project goals?

I ask because it's possible to smooth the motion of servos using an algorithm which moves the servos with constant acceleration.

Here's a video showing an Arduino project which uses a constant acceleration algorithm to move the servos.



The equations used in the algorithm are basic constant acceleration equations taught in high school physics.

The math isn't very hard but I don't have enough experience with Basic Stamps to know if it's practical to add this sort of algorithm to your program. I personally really like the smooth motion achieved using this algorithm.

One limitation you'll likely run into if you plan to increase the number of servos, is the need to keep the program loop within the 20ms refresh period for hobby servos. You may need to drop the debug statements from every loop when you add additional servos. Of course adding a constant acceleration algorithm also makes keeping the control cycle within 20ms harder.

I'm not very experienced using capacitor decay times to measure pot values but it might be possible to reduce the 5ms charge times by changing the capacitor and resistor values.

tomcrawford

I'm glad the code did what you wanted. That's always nice.

As to adding more axis's (axi?). Duane is correct when he mentions running out of time. Lets do some arithmetic:

The overhead (setting up the subroutine call, the GOSUB, the arithmetic, and the return takes 5-6 msec on a green BS-2. Add in, say 2 msec to read the joystick and 1-2 msec for the PULSOUT and you get 8-10 msec. So that is just enough time for two axis.

You cannot improve the PULSOUT. You could maybe get the joystick read down to 1 msec by having a shorter delay and artificially shorter RC time (smaller cap). You could lessen the overhead by putting everything in-line and by basing the arithmetic on minimum RCTIME (would't have to worry about negative numbers). And, of course, using a faster BS-2. Finally, of course, you could just not worry about the 20msec and let the servos deal with it as best they can. Probably would be okay with some, no so good with others.

That said, you would have to make three additions:

Near the top, define the pins and center values for the new axis.

Ax3JS      CON   SomePinNumber
Ax3Svo   CON    SomePinNumber
AX3Mid   CON   some value around 350

Then, at begin you would add

   LOW  Ax3Svo

Finally, in the main loop, add, just prior to the pause

   JSPin = Ax3JS
   Joymid = AX3Mid
   ServoPin = Ax3Svo
   GOSUB OneAxis

The fourth axis would be similar.

tomcrawford

Here is a program that (I think) will control four channels and keep under 20 mSec. This program woke me up at 2:30 this morning

I did everything in-line (no subroutine setup, call, and return). No signed arithmetic. No limits checking.
And I charge all four RCTIME capacitors at once with no waiting. This all came out to about 15 msec on a green BS-2 so I added a PAUSE 5.

I don't have any joysticks, so I simulated the RCTIME by forcing a value and a PAUSE of 1 msec. To run it for real, uncomment the RCTIME (four places) and comment out the PAUSE 1 (four places). You may have to adjust the PAUSE 5.

The capacitors are charged with the OUTS = AllJoys and DIRS = All Outs statements. This could be a little cleaner if all the joysticks were in a common nibble.

The logic analyzer screenshot shows the scope sync (not needed for normal operation) and the four channels.

Alexander14

Wow! Thanks again for your help Tom! I am sorry to hear it woke you up at 2:30 in the morning :P Some of the best ideas either happen at night or when we wake up I will test and get back with you.

Here is a picture of the setup for a mental reference for you as well.

Best Regards,

Alex

Alexander14

Duane Degn wrote: »

Alexander14 wrote: »

Duane, your code worked as well but their seems to be a few issues with it. The servos do not move smoothly when moving thru their positions and a full left on the X axis and a full down of the Y axis will make the servo go in the opposite direction to the other side of its movement. This does not happen with full up and full right for some reason.

Any ideas?

My first guess is the numbers are overflowing the 16-bit words.

There could also be a problem when inputs are outside of the range you mentioned earlier. I didn't add a check on limits like Tom did.

It seems like the easiest solution is to use Tom's code.

What did the debug statements read when the servos moved the wrong direction?


What's your end goal with this project?

Answer: My goal is that I am building a UROV with a complete control system for the thrusters using the BOE as the main control board to control the thrusters speeds.

You mention Tom's code provides smooth movement. Is it smooth enough for your project goals?

Answer: Yes, very smooth

I ask because it's possible to smooth the motion of servos using an algorithm which moves the servos with constant acceleration.

Here's a video showing an Arduino project which uses a constant acceleration algorithm to move the servos.



The equations used in the algorithm are basic constant acceleration equations taught in high school physics.

The math isn't very hard but I don't have enough experience with Basic Stamps to know if it's practical to add this sort of algorithm to your program. I personally really like the smooth motion achieved using this algorithm.

One limitation you'll likely run into if you plan to increase the number of servos, is the need to keep the program loop within the 20ms refresh period for hobby servos. You may need to drop the debug statements from every loop when you add additional servos. Of course adding a constant acceleration algorithm also makes keeping the control cycle within 20ms harder.

I'm not very experienced using capacitor decay times to measure pot values but it might be possible to reduce the 5ms charge times by changing the capacitor and resistor values.

Alexander14

Thanks for your feedback Duane / Tom The UROV I am building will have a depth rating from 150 - 400ft and will be guided by 3 - 4 thrusters with tethered thrusters / electronics control. Once completed, It will be making its first dive into the the lake next to my house.

Best Regards,

Alex

tomcrawford

Alexander14 wrote: »

Wow! Thanks again for your help Tom! I am sorry to hear it woke you up at 2:30 in the morning :P Some of the best ideas either happen at night or when we wake up I will test and get back with you.

Here is a picture of the setup for a mental reference for you as well.

Best Regards,

Alex

That looks really cool. Here is a copy of the program with AX4JSPin defined (don't know how that got past me)

Alexander14

Thanks for the updated code Tom, I will test it out tonight.

Best Regards,

Alex

Alexander14

Tom:

I am assuming this BS2 code for the 4-axis you just released will work with the BS2P I currently have hooked up to everything?

Regards,

Alex

tomcrawford

It s'posed to. That being said:

If it messes up when the joystick goes full left (down), it is because the "JoyValue - AxnJSMin" underflows. Make AxnJSMin a little bigger smaller to prevent that.

Lemmee know.

Do you have a logic analyzer or a scope to measure what the actual loop time is?

Alexander14

Hello Tom:

I tried the program as is and just changed the directive for my BS2P and I get a lot of servo jitter. I changed the pause as directed to 1 and 10 but neither had any effect. Also, I only got 1 servo to move with the joystick on the 2 additional channels. I had the other move in one direction and then stopped responding so I will have to check the circuitry to ensure nothing came loose etc. Can you just use the original program with the addition of the other 2 servo ports since that one works flawlessly?

Best Regards,

Alex

tomcrawford

I'm a little surprised. Well, maybe have to go back to the original program.

Try the attached, if you would. You will have to change the BS-2 to BS-2p, but everything else should compile and run.

If that doesn't work, you will have to go back to the original programs with the additions I outlined in my June 19 post. That will truly violate the 20 mSec rule.

Alexander14

Thanks Tom, I tried it out and the servo jitter went away at idle (no joystick movement) but it lost resolution when moving the servos via joystick. Instead of the silky smooth movement of the limits program, it move in large"stepped" movements and does not move to its physical limits when moving the joystick. It moves about 1/3 of the motion as the original program. Thoughts?

Regards,

Alex

tomcrawford

Try increasing the gain. Each axis has a constant AXnGain CON $0300. Try increasing that to, say, $0700.

Alexander14

Hello Tom - that allows for the full range of movement but brings back the servo jitter at idle.

Regards,

Alex

tomcrawford

You *must* have all four joysticks connected and returning reasonable values.

Dang! Well, maybe my notion of charging all the caps at once doesn't work. Maybe try this:

Right above each RCTIME, insert the following: (four places)

   HIGH AxnJSPin       'where n is the axis  'charge the cap
   PAUSE 1                'wait for it

and remove the DIRS = and the OUTS = in axis 4 code.

Do you have any way to measure the loop time? Wish you were here; we could fix this easy.

Alexander14

Tom:

I was wondering that as well...no I do not have a joystick connected for the other 2 ports...ugghh...I could setup (2) 500k pots to mimic the current joystick until I get the other one, then download and try again.

Your code is probably spot on and is acting up because of the the issue above. :P Sorry Tom

Regards,

Alex

tomcrawford

Just comment out the offending RCTIMES and replace them with

PAUSE 1

to kinda keep the loop time reasonable.