Servo Control using PS2 style joysticks

Jkelsey2

Member
Greetings from California!
I understand that this topic has been discussed heavily but I'm still having some trouble with joystick control. My project is a 5 axis robot arm with each axis being controlled by a servo motor (5 axis = 5 servos, 1 servo per axis, you get it). What I want is to use 2 PS2 style joysticks to control the 4 main axis's of the arm (base, shoulder, elbow, and wrist. The 5th servo will open or close the gripper which I may use the button feature for). I've been looking into code to do this and have found some code examples that get me close to achieving my goal; however, the problem is that because the joysticks are spring returned the servos will snap back to the center position. Therefore, I have to hold the joystick to keep the servo in the desired position. Here is the example code that shows what I'm talking about above:

servo C.4, 150

do
readadc C.1, b0
b1=150*b0/255+75
servopos C.4, b1
loop

Again with this code, a servo will move in the direction indicated from the joystick but once the joystick is returned to the center the servo returns to its center position which is not desirable for my application.

Can anyone help me write a code where the servo will move in the direction that the joystick is moved but the servo will not snap back to center when the joysick is back at its center? Is this even possible to do? Thank you all in advance!
 

lbenson

Senior Member
How is the code supposed to know the difference between when the joystick "snaps" back to center and when the user moves it to the center?
 

Jkelsey2

Member
I honestly have no idea. That seems to be the impossible part. But what im thinking is when a joystick is moved in one direction the servo motor moves in that direction but if the joystick is moved back to its center (or snaps back to the center) the servo will stop moving and hold its current position until the joystick is moved again. Maybe the code can use the analog signal from the joystick being in the center as a sort of buffer? Like if b"whatever" = this amount then hold?
 

Buzby

Senior Member
Imagine you were controlling one axis with two buttons, FWD and REV.

You would have code like,

If FWD pressed then INC SERVOPOS
If REV pressed then DEC SERVOPOS

As you press the FWD button you would increase the SERVOPOS value.
When the axis got where you want, you would release the button, and the SERVOPOS value stays at the last value.

To move the axis back you would press the REV button, which decreases the SERVOPOS value.
Again, when the button not pressed the value stays as set.

So with an analogue input, you would need something like,

If ADC > centre_value then INC SERVOPOS
If ADC < centre_value then DEC SERVOPOS

Now the ADC value, higher or lower then the centre_val, acts as FWD / REV buttons.

Once you get this working we can move on to using the ADC value to adjust the speed of the FWD/REV actions.

( If you want a different system, where the axis follows the actual position of the joystick, then you need a joystick that doesn't snap back. )

Cheers,

Buzby
 

Jkelsey2

Member
Update...
So I got a working code... sort of. This is what I've come up with so far:

Init:
Servo C.2, 150

Main:
readadc C.4, b0
servopos C.2, b1
if b0 < 128 then decX
If b0 > 133 then incX
goto main

decx:
dec b1
goto main

incx:
Inc b1
goto main

This works great! However the new issue is the output shaft of the servo will rotate to the opposite end when either limit is exceeded (i hope this makes sense). So my question is how can I program limits so that when the output shaft of the servo reaches one of its limits it will back off, not rotate to the opposite end?
 

Gramps

Senior Member
( If you want a different system, where the axis follows the actual position of the joystick, then you need a joystick that doesn't snap back. )
The cheap (5 for a buck) joysticks we ordered snap-back in one direction and stay where you want them in the other.
I dissected them and took the springs out.:rolleyes:
Gramps
 

lbenson

Senior Member
Once you determine what the limits are (try putting in pauses and SERTXD(#b1," ") before your SERVOPOS), define them with the symbols maxX and minX and then replace INC and DEC with

if b1<maxX then: inc b1: endif

if b1>minX then: dec b1: endif
 

Jkelsey2

Member
Once you determine what the limits are (try putting in pauses and SERTXD(#b1," ") before your SERVOPOS), define them with the symbols maxX and minX and then replace INC and DEC with

if b1<maxX then: inc b1: endif

if b1>minX then: dec b1: endif
Thank you for this. I apologize I'm fairly new to microcontroller programming and am unfamiliar with this command. Can you give me an example of what the syntax would look like exactly? Would I first need to debug the limits i want for the servo and then set those in this command? I looked in the manual and it just confused me lol.
 

Jkelsey2

Member
Thank you for this. I apologize I'm fairly new to microcontroller programming and am unfamiliar with the sertxd command. Can you give me an example of what the syntax would look like exactly? Would I first need to debug the limits i want for the servo and then set those in this command? I looked in the manual and it just confused me lol.
 

Buzby

Senior Member
A few helpful suggestions.

Use symbols. Although you can keep track of each pin and each variable in your current code, once you get beyond a few axises you will definately get them mixed up. Use symbols that identify the function of the variable, and which axis it belongs to.

Use 'gosub' instead of 'goto'. Your code will be a real struggle to follow when it get to more than one axis. e.g. If you always 'goto main' after each inc or dec, then you will never execute any code beyond the first 'goto main'. Also, debugging is much easier, as you can easily 'comment out' calls to routines you are not testing.

Try this code.

Rich (BB code):
#terminal 9600 ' Start terminal app

' Symbols
symbol adcX    = b0 ' Value from joystick X
symbol reqposX = b1 ' Required value of SERVOPOS

symbol serpinX = c.2 ' Servo pin X
symbol adcpinX = c.4 ' ADC pin X

symbol cvhiX  = 133 ' Centre value hi X
symbol cvloX  = 128 ' Centre value lo X

symbol minposX = 75   ' Lowest required value of SERVOPOS ( Adjust to set minimum position of axis )
symbol maxposX = 225  ' Highest required value of SERVOPOS ( Adjust to set maximum position of axis )

' Code starts here
init:
Servo serpinX, 150

do ' Main loop
      readadc adcpinX, adcX
      if adcX < cvloX then 
            gosub decX
      endif
      If adcX > cvhiX then 
            gosub incX
      endif
      servopos serpinX, reqposX
      
      sertxd(#adcX,9,9,#reqposX,cr,lf) ' Send X values to terminal
loop


' Subroutines

decX:
if reqposX > minposX then
      dec reqposX
endif
return

incX:
if reqposX < maxposX then 
      inc reqposX
endif
return
Cheers,

Buzby
 
Last edited:

Jkelsey2

Member
A few helpful suggestions.

Use symbols. Although you can keep track of each pin and each variable in your current code, once you get beyond a few axises you will definately get them mixed up. Use symbols that identify the function of the variable, and which axis it belongs to.

Use 'gosub' instead of 'goto'. Your code will be a real struggle to follow when it get to more than one axis. e.g. If you always 'goto main' after each inc or dec, then you will never execute any code beyond the first 'goto main'. Also, debugging is much easier, as you can easily 'comment out' calls to routines you are not testing.

Try this code.

Rich (BB code):
#terminal 9600 ' Start terminal app

' Symbols
symbol adcX    = b0 ' Value from joystick X
symbol reqposX = b1 ' Required value of SERVOPOS

symbol serpinX = c.2 ' Servo pin X
symbol adcpinX = c.4 ' ADC pin X

symbol cvhiX  = 133 ' Centre value hi X
symbol cvloX  = 128 ' Centre value lo X

symbol minposX = 75   ' Lowest required value of SERVOPOS ( Adjust to set minimum position of axis )
symbol maxposX = 225  ' Highest required value of SERVOPOS ( Adjust to set maximum position of axis )

' Code starts here
init:
Servo serpinX, 150

do ' Main loop
      readadc adcpinX, adcX
      if adcX < cvloX then 
            gosub decX
      endif
      If adcX > cvhiX then 
            gosub incX
      endif
      servopos serpinX, reqposX
      
      sertxd(#adcX,9,9,#reqposX,cr,lf) ' Send X values to terminal
loop


' Subroutines

decX:
if reqposX > minposX then
      dec reqposX
endif
return

incX:
if reqposX < maxposX then 
      inc reqposX
endif
return
Cheers,

Buzby
Thank you for this! This was very helpful 🙂. I will try this tomorrow and will post an update.
 

Jkelsey2

Member
Buzby,
The code you provided works perfectly! Now I just need to develop code for the other servos. Thanks again for your help.
 

Jkelsey2

Member
Okay...
I've made a code that will control each servo; however, I am also getting a lot of weird behavior from each servo. I believe I am having lots of electrical noise interfering with the system and I wouldn't be surprised if it were also related to my code ( i will include the code in a bit). I've tried to eliminate as much noise as i can. I'm using a 12v wall wort for the power supply, and off that I have regulated 5v with a 78L05 voltage regulator to power the picaxe and both of the joysticks. For the servos I am using 2 "UBEC" 6v regulators (1 UBEC for 2 servos). Also each servo has its own 100uF capacitor in parallel with the +5v and 0v lines. I'm not sure what else I can do to eliminate any noise. The weird behavior I'm getting is: upon POR, the servos will move sequentially to their respective centers which is what I programmed them to do, but then what happens is when I move any of the joysticks nothing will happen and then the servos start moving to random positions and chattering. This will happen for about 20 seconds and then I am able to use the joysticks to move the servos (with some chattering still). Here is my code so far:

Rich (BB code):
symbol servoB = B.0 'Base servo
symbol servoS = B.1 'Shoulder servo
symbol servoE = B.2 'Elbow servo
symbol servoW = B.3 'Wrist servo

symbol adcB = b0 'Base servo position
symbol reqposB = b1 'Required base position
symbol adcS = b2 'Shoulder servo postion
symbol reqposS = b3 'Required shoulder position
symbol adcE = b4 'Elbow servo Position
symbol reqposE = b5 'Required elbow position
symbol adcW = b6 'Wrist servo position
symbol reqposW = b7 'Required wrist position

symbol adcpinB = B.4 'Joystick 1 X axis
symbol adcpinS = B.5 'Joystick 1 Y axis
symbol adcpinE = B.7 'Joystick 2 Y axis
symbol adcpinW = B.6 'Joystick 2 X axis

symbol cvhiB = 133 '128 to 133 = Joystick center value
symbol cvloB = 128
symbol cvhiS = 133
symbol cvloS = 128
symbol cvhiE = 133
symbol cvloE = 128
symbol cvhiW = 133
symbol cvloW = 128

symbol minposB = 75 '75 = servo minimum limit
symbol maxposB = 255 '255 = servo maximum limit
symbol minposS = 75
symbol maxposS = 255
symbol minposE = 75
symbol maxposE = 255
symbol minposW = 75
symbol maxposW = 255
init:                ' Servos move to respective center value sequentially
servo servoB, 150
wait 1
servo servoS, 150
wait 1
servo servoE, 150
wait 1
servo servoW, 150
wait 1
goto main

main:                'main program starts here

do
    readadc adcpinB, adcB 'Joystick 1 X axis signal
    readadc adcpinS, adcS 'Joystick 1 Y axis signal
    readadc adcpinE, adcE 'Joystick 2 Y axis signal
    readadc adcpinW, adcW 'Joystick 2 X axis signal
    if adcB < cvloB then
        gosub DecB
    endif                    'compares signal to decide where servo needs
    if adcB > cvhiB then        'to move to. program enters subroutine for
        gosub IncB            'associated signal
    endif
    servopos servoB, reqposB
   
    if adcS < cvloS then
        gosub DecS
    endif
    if adcS > cvhiS then
        gosub IncS
    endif
    servopos servoS, reqposS
    if adcE < cvloE then
        gosub DecE
    endif
    if adcE > cvhiE then
        gosub IncE
    endif
    servopos servoE, reqposE
    if adcW < cvloW then
        gosub DecW
    endif
    if adcW > cvhiW then
        gosub IncW
    endif
    servopos servoW, reqposW
loop
' Subroutines
' Each subroutine is used to keep each servo from moving outside of the minimum
' and maximum positions
DecB:    'Base Servo
if reqposB > minposB then
    dec reqposB
endif
return

IncB:
if reqposB < maxposB then
    inc reqposB
endif
return

DecS: 'Shoulder Servo
if reqposS > minposS then
    dec reqposS
endif
return

IncS:
if reqposS < maxposS then
    inc reqposS
endif
return

DecE: 'Elbow Servo
if reqposE > minposE then
    dec reqposE
endif
return

IncE:
if reqposE < maxposE then
    inc reqposE
endif
return

DecW: 'Wrist Servo
if reqposW > minposW then
    dec reqposW
endif
return

IncW:
if reqposW < maxposW then
    inc reqposW
endif
return
I assume my issues are stemming from both code and circuit. Any help with this will be greatly appreciated!!!! My head is swimming at this point.
 
Last edited:

erco

Senior Member
Servos will always twitch when reading pots with ADC and using SERVO/SERVOPOS.

You need to use PULSOUTs instead of SERVO/SERVOPOS, requiring your code to be in a short, tight loop to keep sending pulses to the servos something close to 50 hz.
 

Jkelsey2

Member
Servos will always twitch when reading pots with ADC and using SERVO/SERVOPOS.

You need to use PULSOUTs instead of SERVO/SERVOPOS, requiring your code to be in a short, tight loop to keep sending pulses to the servos something close to 50 hz.
I apologize...im confused. How will the PULSOUTs adjust with the signal from the joystick? When I look at the command in the manual, it doesn't appear to give the option to use byte variables. Can you give an example of what it would look like?
 

erco

Senior Member
Servo C.4, 150 initiates a steady stream of 1.5 ms output pulses to drive a servo.

Which is equivalent to this short program loop:

do
pulsout c.4,150
pause 20
loop


You need to write your program to loop in a short fashion to read the adc and drive the pot. Every command slows the loop down, so you must adjust the pause accordingly. Something like:

do
readadc b.0, b5 ' read pot on pin b.0, save value in variable b5
pause 8 ' sometimes a brief pause after an ADC helps reduce jitter
pulsout c.4, b0 ' send a single servo pulse out of pin c.4, value stored from reading ADC
pause 8 ' add another pause after pulsout, total pause now 16 ms , less than 20 because readadc and extra commands slow down the loop
loop
 

Jkelsey2

Member
Servo C.4, 150 initiates a steady stream of 1.5 ms output pulses to drive a servo.

Which is equivalent to this short program loop:

do
pulsout c.4,150
pause 20
loop


You need to write your program to loop in a short fashion to read the adc and drive the pot. Every command slows the loop down, so you must adjust the pause accordingly. Something like:

do
readadc b.0, b5 ' read pot on pin b.0, save value in variable b5
pause 8 ' sometimes a brief pause after an ADC helps reduce jitter
pulsout c.4, b0 ' send a single servo pulse out of pin c.4, value stored from reading ADC
pause 8 ' add another pause after pulsout, total pause now 16 ms , less than 20 because readadc and extra commands slow down the loop
loop
Thank you! This was helpful. I will give this a try.
 

erco

Senior Member
Here's a snippet from my 08M2 program that reads 2 pots and drives 2 servos.

do
readadc 2,b0
b0=b0/2+100 min 130 max 200 ' math converts ADC reading on pot (pin 2) to the right servo pulse width
readadc 4,b1
b1=b1/2+100 min 125 max 200 ' math converts ADC reading on pot (pin 4) to the right servo pulse width
pause 8
pulsout 0,b0 ' drive servo on pin 0 with pulse from pot (pin 2)
pulsout 1,b1 ' drive servo on pin 1 with pulse from pot (pin 4)
pause 8
loop
 
Top