Putting robotics at your service™

Free shipping on orders over $200

Servo Programming Help

Print view Share :
Previous topicNext topic

Page 1 of 2 [ 17 posts ]

1, 2
Rookie ( offline )
Posts: 23
Posted: 2011-03-14 22:00 
 Post subject: Servo Programming Help
Okay, so I'm new to this programming language. I think it's called MBasic or something like that. I know a decent amount of C++ and a little Java, but I really can't figure this out. I just need to know how to use servos in the code just like they're being used in this example code. The only problem is I can't figure out how to create them, like creating a servo variable and saying what port its on. I see it says "lservo var sword" and "rservo var sword" but I have pretty much no idea what that means, besides that it's creating a servo variable. I just need to know how to create them and use them in the code. I basically just need to modify the existing "drive" code to work with 9 servos instead of 2. I don't understand what's going on at the end where it says "this is where the servo motor controller is directed" either. Anyways, thanks in advance for the help, and here is the code I'm trying to modify.

Code:
'Work in progress!
LED con P0

;PS2 Controller / BotBoard I
;DAT con P4
;CMD con P5
;SEL con P6
;CLK con P7
;PS2 Controller / BotBoard II
DAT con P12
CMD con P13
SEL con P14
CLK con P15

temp1 var byte
index var byte
temp var byte(6)
cntr var byte
mode var byte

rhori var word
rvert var word
lhori var word
lvert var word
lservo var sword
rservo var sword
small var byte
large var byte

high CLK

test
low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8,$43\8,$0\8,$1\8,$0\8] ;Config Enable
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;Set Mode and Fix(Analog
high SEL

pause 100

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4D\8,$00\8,$00\8,$01\8,$ff\8,$ff\8,$ff\8,$ff\8] ;Vibration Enable
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;Enable Pressure
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;Press_trans_start
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;Config Exit
high SEL

main
' This section find the mode the PSX controller is in.

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8]
shiftin DAT,CLK,FASTLSBPOST,[mode\8]
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8]
for index = 0 to 2
shiftin DAT,CLK,FASTLSBPOST,[temp(index)\8]
next
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8,$0\8,small\8,large\8]
for index = 3 to 6
shiftin DAT,CLK,FASTLSBPOST,[temp(index)\8]
next
high SEL

' This section does the mixing for throttle steering on a diff steered vehicle.
rhori = (255 + temp(3))
rvert = (255 + temp(4))
lhori = (255 + temp(5))
lvert = (255 + temp(6))

lservo = 383 + ((lhori-383) + (lvert-383))
rservo = 383 + ((lvert-383) - (lhori-383))

if lservo < 255 then
lservo = 255
endif
if lservo > 510 then
lservo = 510
endif
if rservo < 255 then
rservo = 255
endif
if rservo > 510 then
rservo = 510
endif

lservo = 4 * (lservo - 383)
rservo = 4 * (rservo - 383)

' The following can be used as a debug to see the values of the joysticks and buttons.
' serout S_OUT,i57600,[13]
' for index = 1 to 6
' serout S_OUT,i57600,[" ",dec3 temp(index)\3]
' next

' serout S_OUT,i57600,[" ",dec3 rhori\3]
' serout S_OUT,i57600,[" ",dec3 rvert\3]

' serout S_OUT,i57600,[" LH ",dec3 lhori\3]
' serout S_OUT,i57600,[" LV ",dec3 lvert\3]
' serout S_OUT,i57600,[" LS ",sdec3 lservo\6]
' serout S_OUT,i57600,[" RS ",sdec3 rservo\6]

' The following code relates to the internal vibrator motors and is not used in this application.
small = 0 'temp(7)
large = 0 'temp(8)

' This section is where the servo style motor controller is directed.
servo 0, lservo, 1
servo 1, rservo, 1

goto main


Last edited by kurte on 2011-03-14 22:18, edited 1 time in total.
Put code in code block


User avatar
Guru ( offline )
Posts: 4877
Posted: 2011-03-14 22:21 
It would help to have some additional information.

What processor are you using? Is it directly handling the servos or is there a servo controller that actually handles the servos?

Kurt


Rookie ( offline )
Posts: 23
Posted: 2011-03-14 22:54 
I'm using the Atom Pro 28. The servos are plugged in directly to the bot board, there is no separate controller.. I figured it out somewhat.. However, when the robot uses more than four servos at a time, its movement stutters and isn't smooth. Could this be due to the fact that I am using one 9v battery for both VS and VL and not a 6v battery pack for VS and a 9v for VL?

Edit:

If anyone could please help, this is for the regional science fair on Wednesday :/


Rookie ( offline )
Posts: 23
Posted: 2011-03-14 23:52 
Here is the completed code. The issue still happens where the robot runs really stuttering-ly. I have no idea why and any help would be appreciated.


Code:
LED con P0

;PS2 Controller / BotBoard II
DAT con P12
CMD con P13
SEL con P14
CLK con P15

temp1 var byte
index var byte
temp var byte(6)
cntr var byte
mode var byte

rhori var word
rvert var word
lhori var word
lvert var word

flservo var sword
frservo var sword
blservo var sword
brservo var sword

lfservo var sword
lbservo var sword
rfservo var sword
rbservo var sword

ffservo var sword
bbservo var sword
ssservo var sword

small var byte
large var byte

high CLK

test
low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8,$43\8,$0\8,$1\8,$0\8] ;Config Enable
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;Set Mode and Fix(Analog
high SEL

pause 100

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4D\8,$00\8,$00\8,$01\8,$ff\8,$ff\8,$ff\8,$ff\8] ;Vibration Enable
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;Enable Pressure
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;Press_trans_start
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;Config Exit
high SEL

main
' This section find the mode the PSX controller is in.

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8]
shiftin DAT,CLK,FASTLSBPOST,[mode\8]
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8]
for index = 0 to 2
shiftin DAT,CLK,FASTLSBPOST,[temp(index)\8]
next
high SEL

low SEL
shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8,$0\8,small\8,large\8]
for index = 3 to 6
shiftin DAT,CLK,FASTLSBPOST,[temp(index)\8]
next
high SEL

' This section does the mixing for throttle steering on a diff steered vehicle.
rhori = (255 + temp(3))
rvert = (255 + temp(4))
lhori = (255 + temp(5))
lvert = (255 + temp(6))

flservo = 383 - (lvert-383)
frservo = 383 + (lvert-383)
blservo = 383 - (lvert-383)
brservo = 383 + (lvert-383)

lfservo = 383 - (lhori - 383)
lbservo = 383 + (lhori - 383)
rfservo = 383 - (lhori - 383)
rbservo = 383 + (lhori - 383)

ffservo = 383 + (rvert - 383)
bbservo = 383 - (rvert - 383)
ssservo = 383 - (rhori - 383)

if flservo < 255 then
flservo = 255
endif
if flservo > 510 then
flservo = 510
endif
if frservo < 255 then
frservo = 255
endif
if frservo > 510 then
frservo = 510
endif

if blservo < 255 then
blservo = 255
endif
if blservo > 510 then
blservo = 510
endif
if brservo < 255 then
brservo = 255
endif
if brservo > 510 then
brservo = 510
endif

if lfservo < 255 then
lfservo = 255
endif
if lfservo > 510 then
lfservo = 510
endif
if lbservo < 255 then
lbservo = 255
endif
if lbservo > 510 then
lbservo = 510
endif

if rfservo < 255 then
rfservo = 255
endif
if rfservo > 510 then
rfservo = 510
endif
if rbservo < 255 then
rbservo = 255
endif
if rbservo > 510 then
rbservo = 510
endif

if ffservo < 255 then
ffservo = 255
endif
if ffservo > 510 then
ffservo = 510
endif
if bbservo < 255 then
bbservo = 255
endif
if bbservo > 510 then
bbservo = 510
endif

if ssservo < 255 then
ssservo = 255
endif
if ssservo > 510 then
ssservo = 510
endif

flservo = 4 * (flservo - 383)
frservo = 4 * (frservo - 383)
blservo = 4 * (blservo - 383)
brservo = 4 * (brservo - 383)

lfservo = 4 * (lfservo - 383)
lbservo = 4 * (lbservo - 383)
rfservo = 4 * (rfservo - 383)
rbservo = 4 * (rbservo - 383)

ffservo = 4 * (ffservo - 383)
bbservo = 4 * (bbservo - 383)
ssservo = 4 * (ssservo - 383)

' The following can be used as a debug to see the values of the joysticks and buttons.
' serout S_OUT,i57600,[13]
' for index = 1 to 6
' serout S_OUT,i57600,[" ",dec3 temp(index)\3]
' next

' serout S_OUT,i57600,[" ",dec3 rhori\3]
' serout S_OUT,i57600,[" ",dec3 rvert\3]

' serout S_OUT,i57600,[" LH ",dec3 lhori\3]
' serout S_OUT,i57600,[" LV ",dec3 lvert\3]
' serout S_OUT,i57600,[" LS ",sdec3 lservo\6]
' serout S_OUT,i57600,[" RS ",sdec3 rservo\6]

' The following code relates to the internal vibrator motors and is not used in this application.
small = 0 'temp(7)
large = 0 'temp(8)

' This section is where the servo style motor controller is directed.
servo 0, flservo, 1
servo 1, frservo, 1
servo 2, blservo, 1
servo 3, brservo, 1
servo 4, lfservo, 1
servo 5, lbservo, 1
servo 6, rfservo, 1
servo 7, rbservo, 1
servo 8, ffservo, 1
servo 10, ssservo, 1
servo 11, bbservo, 1

goto main


Last edited by TBear on 2011-03-15 00:18, edited 2 times in total.

Rookie ( offline )
Posts: 23
Posted: 2011-03-15 00:03 
Here is a video of the stuttering issue.

http://www.youtube.com/watch?v=BYmkoC-g_9Q


Guru ( offline )
Posts: 2251
Posted: 2011-03-15 01:50 
Stuttering servos may mean you have an inadequate power supply for the servos.

_________________
Why I like my 2005 rio yellow Honda S2000 with the top down, and more!
http://youtube.com/watch?v=pWjMvrkUqX0
http://youtube.com/watch?v=qfyRA-g1nhI
http://web.comporium.net/~shb/S2000video.htm


User avatar
Guru ( offline )
Posts: 4877
Posted: 2011-03-15 10:25 
You said that there is a single 9v battery processing the 12 servos and VL??? Not sure what type of servos, but most of the hitec type servos want 6 volts so not sure what your servos will do with that...

Also you are using the servo command to control each of the servos, which will only generate 1 pulse each. With 10 servos depending on what the values that are actually outputting to the servos, your loop will take more than 20ms to complete and analog servos need to have the signal repeated every 20ms... IE if your servo average a pulse width of 2000us than 12 of those will take 24000 or 24ms and we are not talking at all about the overhead for your loop and the rest...

So if it were me, my next step would be to convert to the HSERVO system, which takes care of the timings to make sure all of the servos get their correct pulse...

Kurt


User avatar
Guru ( offline )
Posts: 9258
Posted: 2011-03-15 10:32 
Dang you beat me to it!

The problem here is you are using the atoms "servo" command. It's really never used as it works weird. In your application "mservo" may fair a little better, but there are simply too many servo to control sequentially. What you need is the background servo control method called "hservo." I'm not sure where the latest most accurate instructions are for "hservo." Anyone else know?

The problem you are having is due to the servo pulses generated by the atom chip being interrupted. Because the pulses are not constant, you get the jitter.

_________________
Jim Frye, the Robot Guy
http://www.lynxmotion.com
I've always tried to do my best...


User avatar
Guru ( offline )
Posts: 4877
Posted: 2011-03-15 10:52 
Robot Dude wrote:
Dang you beat me to it!
:P :lol:

Robot Dude wrote:
I'm not sure where the latest most accurate instructions are for "hservo." Anyone else know?

The current Syntax manual that is in the build 2.0.0.9, which you can download from: http://downloads.basicmicro.com/downloa ... -0-0-9.exe

Nathan (and others?) updated the manual, which you can also download from their website: http://downloads.basicmicro.com/downloa ... l_V2_0.pdf

The unfortunate thing about the V2.0 manual is they lost all of the bookmarks and hot links that worked in the V1.0 manual, which I mostly use. I think you can still download that from their site (But they do not provide any links to it): http://downloads.basicmicro.com/downloa ... l_V1_0.pdf

Kurt


Rookie ( offline )
Posts: 23
Posted: 2011-03-15 13:33 
It seems like the hservo stuff is mostly related to sending the servo to a specific position (i could be wrong). How would you use it to say "when the left stick is pushed forward, spin at max speed"?


Rookie ( offline )
Posts: 23
Posted: 2011-03-15 13:42 
Basically...how would you apply the same mixing I used to th hservo command?


User avatar
Guru ( offline )
Posts: 4877
Posted: 2011-03-15 14:07 
The servo command and the hservo command do similar things, the differences is that HSERVO takes care of repeating it for you. So when you go through your loop and wish for a new value, then set a new value...

The two commands have different value scaling. That is I think the servo command has the pulse widths in .5us differences where a value of 0 creates a pulse of 1500us which is standard 0. HServo has it's own scaling which turns out to be related to the clock speed of the processor. On the Bap28 it has a range of +-24000 to give a pulse width of 0 to 3000us, so it is simple math to convert between the two different scales...

What to do when you press a joystick forward to the speed of the servos is up to you. Do you make it simply linear from 0 to top speed, or like many other programs do, you add the joystick value to your current speed (hservo value) up to some maximum and only allow the changes to happen every so often as to not have the motors jump from 0 to some high speed in jerky motions. You can do this by using the speed parameters to the HSERVO command, which tells the system how much to delta per 20ms cycle of time. So if you know how much time you wish for the motors to use to ramp up to the new speed, you can calculate the appropriate delta for each motor... Then your code can use the HSERVOIDLE command to see if the motors have reached the new speed before you issue another one...

Good Luck
Kurt


Rookie ( offline )
Posts: 23
Posted: 2011-03-15 15:52 
Okay sooo... Would it work like this in order to just directly link the speed to the stick?

Code:
hservo [p0\0]

main

hservo [p0\0\lvert]

goto main



That's probably wrong, but anyways.
Sorry for my stupidity. I am the programmer on a FIRST robotics team and there are prewritten functions that interface with the hardware for you that are much simpler than this.


User avatar
Guru ( offline )
Posts: 4105
Posted: 2011-03-15 18:23 
Close, but you'd probably have to scale it. joystick may be 0-255, centered on 128, or -128 to 127. What has hservo expect? +- 12000 ?

Code:
IF ABS(Dualshock(5) - 128) > DeadZone THEN ;Left Stick L/R   
   LStickY = (Dualshock(5) - 128) ;Wheels

hservo [PanServo\((HservoPos(PanServo) + LStickY*5) max 12000) min -12000\100]


Maybe that's Kurt's demo for the RoboClaw? ;>)

Alan KM6VV

_________________
Visit:
http://groups.yahoo.com/group/SherlineCNC/
http://tech.groups.yahoo.com/group/HexapodRobotIK/


Rookie ( offline )
Posts: 23
Posted: 2011-03-15 18:38 
This is what I have and the servo just spins forward at full speed. The mixing stuff at the top is written by someone else (I'm modifying the code I posted in the topic) and I don't know really what it does or if it's necessary. Anyway, here is what I tried after reading the demo code.

Code:
 rhori = (255 + temp(3))
rvert = (255 + temp(4))
lhori = (255 + temp(5))
lvert = (255 + temp(6))

hservo[p0\((hservopos(p0) + lvert * 5) max 12000) min -12000\100]



1, 2

All times are UTC - 5 hours [ DST ]. It is currently 2014-07-29 10:45
Feedback Form
Feedback Form