Basic Stamp & servo controller with more than 1 servo- Please Help!

Ryan1234 · 2011-02-08 07:03

I have written a program to control a servo with the basic stamp 2 and parallax servo controller, but I want to be able to control more than 1, preferably simultaneously and moving differently. What is the best way to do this? Here is the program, please help, I am very frustrated.

' {$STAMP BS2}
' {$PBASIC 2.5}
CH VAR Byte
PW VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
SDAT CON 15
BAUD CON 396
COL VAR Nib
ROW VAR Nib
INIT:
RA =30: CH = 15
COL = 0
DEBUG "

SPEED SET ---"
DO WHILE (COL < 10)
FOR ROW = 0 TO 15
IF (ROW > 9) THEN EXIT
DEBUG CRSRXY, (COL * 8), ROW,
DEC COL, "/", DEC ROW, CR
NEXT
COL = COL + 1
IF (COL = 20) THEN GOSUB ENGSTOP '2 CYCLES + 1

PW = 750: GOSUB WRSERVO1 'EE IDLE - 23 SEC
PW = 670: GOSUB WRSERVO2 '9E IDLE - 25 SEC
PW = 749: GOSUB WRSERVO1 'ED IDLE - 23 SEC
PW = 661: GOSUB WRSERVO2 '95 IDLE - 25 SEC
PW = 751: GOSUB WRSERVO1 'EF IDLE - 23 SEC
PW = 670: GOSUB WRSERVO2 '9E IDLE - 25 SEC
PW = 751: GOSUB WRSERVO1 'EF IDLE - 23 SEC
PW = 660: GOSUB WRSERVO2 '94 IDLE - 25 SEC
PW = 750: GOSUB WRSERVO1 'EE IDLE - 23 SEC
PW = 670: GOSUB WRSERVO2 '9E IDLE - 25 SEC
PW = 750: GOSUB WRSERVO1 'EE IDLE - 23 SEC
PW = 660: GOSUB WRSERVO2 '94 IDLE - 25 SEC
PW = 690: GOSUB WRSERVO1 'B2 IDLE - 25 SEC

DEBUG CLS
DEBUG "

SPEED SET---"
LOOP
ENGSTOP: 'ENGINE STOP
PW = 760: GOSUB WRSERVO1
PW = 780: GOSUB WRSERVO1
SEROUT SDAT, BAUD+$8000,["!SC", CH, RA, PW.LOWBYTE, PW.HIGHBYTE, CR]
DEBUG CLS, "

SPEED SET

"
DEBUG "-- CYCLE COMPLETE -- ", DEC COL (0), "-- ENGINE STOP"
END
WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT SDAT, BAUD+$8000,["!SC", CH, RA, PW.LOWBYTE, PW.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 1600
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN
WRSERVO2: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT SDAT, BAUD+$8000,["!SC", CH, RA, PW.LOWBYTE, PW.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 2000
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN

Mike Green · 2011-02-08 08:41

First of all, what do you want to do? You say you want to control two servos simultaneously, possibly moving differently. What does that mean?

The servo controller can move servos independently from a current position to a desired position with a specified rate of change of position, but each position change has to be commanded separately, so two of them would start at slightly different times (ten milliseconds or so).

Ryan1234 · 2011-02-08 09:14

Thank you for your reply,

I would like to that 2 servos going simultaneously (or as close as possible) and moving differently (moving independent of each other) one could go forward and one could to backward for example.

I would like them to move something like this. (not actual code, example)
servo 1 (position x) servos starting position
servo 2 (position y)
pause
servo 1 (position x1) servos changing position
servo 2 (position y1)
pause
servo 1 (position x2) servos ending position
servo 2 (position y2)
end

I hope this helps, i am not sure if i am explainging very well.

Thank You, Ryan

Mike Green · 2011-02-08 12:21

I don't understand what's different from what the Servo Controller normally does. When you send a command to the Servo Controller, it starts moving the servo. The Controller can immediately get another command for a different servo and the 1st servo will continue to move while the 2nd servo starts moving. The Servo Controller can handle up to 16 servos simultaneously, each moving differently.

Ryan1234 · 2011-02-08 12:51

Yes, that is true, but i am having trouble getting the second servo to work properly, It seems the first servo will runn trough the comands and the second will just run constantly in one direction, or nothing will happen. I am trying to get them to go together (or at lease seem to go together)

' {$STAMP BS2}
' {$PBASIC 2.5}
ch0 VAR Byte
ch1 VAR Byte
pw0 VAR Word
pw1 VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
SDAT CON 15
BAUD CON 396
COL VAR Nib
ROW VAR Nib
INIT:
RA =7
ch0 = 14
ch1 = 15
COL = 0
DEBUG "

950cc, SPEED SET ---"
DO WHILE (COL < 10)
FOR ROW = 0 TO 15
IF (ROW > 9) THEN EXIT
DEBUG CRSRXY, (COL * 8), ROW,
DEC COL, "/", DEC ROW, CR
NEXT
COL = COL + 1
IF (COL = 20) THEN GOSUB ENGSTOP '2 CYCLES + 1
PW0 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW0 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
DEBUG CLS
DEBUG "

950cc, SPEED SET---"
LOOP
ENGSTOP: 'ENGINE STOP
PW0 = 760: GOSUB WRSERVO1
PW1 = 760: GOSUB WRSERVO1
PW0 = 780: GOSUB WRSERVO1
PW1 = 780: GOSUB WRSERVO1
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
DEBUG CLS, "

950cc, SPEED SET

"
DEBUG "-- CYCLE COMPLETE -- ", DEC COL (0), "-- ENGINE STOP"
END
WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 2000
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH0, CR]
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH1, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN

Looking to see if the is somthing wrong with my program or if there is a better way to do it, The servo controller instructions dose not give a good way for controling multiple servos.

Thank You again for you input.

Ryan

Spiral_72 · 2011-02-08 13:04

Ryan1234 wrote: »

Yes, that is true, but i am having trouble getting the second servo to work properly, It seems the first servo will runn trough the comands and the second will just run constantly in one direction, or nothing will happen. I am trying to get them to go together (or at lease seem to go together)

Ryan

Are these continuous rotation servos? Or positioning servos?

If continuous there is a pot you need to adjust first, otherwise it'll never work right

Ryan1234 · 2011-02-08 13:14

I am using Firgelli linear actuator 6V.
http://www.robotshop.ca/firgelli-technologies-L12-50-210-06-I-2.html

Mike Green · 2011-02-08 15:56

Make sure you're using RC servo mode. That's the only mode that will work with the Servo Controller.

Ryan1234 · 2011-02-09 06:38

Yes, It is the RC servo mode. Can you help me with what the best way to adapt this program for 2 servos?

' {$STAMP BS2}
' {$PBASIC 2.5}
'

[ Constants ]

SDAT CON 15
BAUD CON 396
'

[ Variables ]

CH VAR Byte
PW VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
COL VAR Nib
ROW VAR Nib
'

[ Initialization ]

RA =30: CH = 15
COL = 0
'

[ Program Code ]

DEBUG "

SPEED SET ---"
DO WHILE (COL < 10)
FOR ROW = 0 TO 15
IF (ROW > 9) THEN EXIT
DEBUG CRSRXY, (COL * 8), ROW, DEC COL, "/", DEC ROW, CR
NEXT
COL = COL + 1
IF (COL = 10) THEN GOSUB ENGSTOP '2 CYCLES + 1
PW = 750: GOSUB WRSERVO1 'EE IDLE - 23 SEC
PW = 670: GOSUB WRSERVO2 '9E IDLE - 25 SEC
PW = 749: GOSUB WRSERVO1 'ED IDLE - 23 SEC
PW = 661: GOSUB WRSERVO2 '95 IDLE - 25 SEC
PW = 751: GOSUB WRSERVO1 'EF IDLE - 23 SEC
PW = 670: GOSUB WRSERVO2 '9E IDLE - 25 SEC

DEBUG CLS
DEBUG "

SPEED SET---"
LOOP
'

[ Subroutines ]

ENGSTOP: 'ENGINE STOP
PW = 760: GOSUB WRSERVO1
PW = 780: GOSUB WRSERVO1
SEROUT SDAT, BAUD+$8000,["!SC", CH, RA, PW.LOWBYTE, PW.HIGHBYTE, CR]
DEBUG CLS, "

SPEED SET

"
DEBUG "-- CYCLE COMPLETE -- ", DEC COL (0), "-- ENGINE STOP"
END
WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT SDAT, BAUD+$8000,["!SC", CH, RA, PW.LOWBYTE, PW.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 1600
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN
WRSERVO2: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT SDAT, BAUD+$8000,["!SC", CH, RA, PW.LOWBYTE, PW.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 2000
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN

Mike Green · 2011-02-09 08:28

No. Your program is complicated, there are no comments or explanation about what you're trying to do, and I'd have to figure it out first before I could give you any specific advice.

Let's start with the basics ... Do you understand that the "CH" byte in the "!SC" command selects a particular servo? In your program, you set CH to 15 to select servo #15. If you want to control a second servo, you'd have to send a different servo number in a "!SC" command to start that servo moving, something like:

ramp15 = 30 : ramp14 = 50 ' You can use different ramping values for each servo
position15 = 799 : position14 = 700 ' Just for an example, move servo on #15 to 799 and servo on #14 to 700
SEROUT SDAT, BAUD+$8000, ["!SC", 15, ramp15, position15&$FF, position15>>8, cr] ' send new setting for servo on #15
SEROUT SDAT, BAUD+$8000, ["!SC", 14, ramp14, position14&$FF, position14>>8, cr] ' send new setting for servo on #14

Both servos would start to move within a few milliseconds of each other. Note that the ramping values and the position values can be variables, constants, or expressions

Ryan1234 · 2011-02-09 09:33

Something like this? basic version...

' {$STAMP BS2}
' {$PBASIC 2.5}
ch0 VAR Byte
ch1 VAR Byte
pw0 VAR Word
pw1 VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
SDAT CON 15
BAUD CON 396

INIT:
RA =7
ch0 = 14
ch1 = 15
PW0 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW0 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC

WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 2000
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH0, CR]
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH1, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
NEXT
RETURN

I am not sure on what you did with "position15&$FF" and "position14&$FF" ,"position15>>8" and "position14>>8", i am using LOWBYTE AND HIGHBYTE...?

What is the "&$FF"? my programing is very basic i think...

What i am specifically trying to do is, I have my current set up to run one throttle control for a small engine(and works great), and now I want to adapt my program to be able to do two different throttle controls at the same time, so two engines can be controlled simultaneously by one controller.

Thank you for you continued input.

Mike Green · 2011-02-09 10:37

Based on what you've just posted, I think you're trying to do something too complex for your current level of understanding. I think you have to step back and invest some time and energy in learning how to program the Stamps. The "What's a Microcontroller?" tutorial and the "Basic Stamp Syntax and Reference Manual" are included with the Stamp Editor's help files and can be downloaded from Parallax's Download webpage. Work through the examples and exercises.

The Servo Controller performs commands as they're sent to it. It doesn't make sense to have SEROUT "!SCRSP", then SEROUT "!SCRSP", then SERIN. The Controller is still sending the response to the first "!SCRSP" when you send the second "!SCRSP". Think about what you're doing in the program. Sometimes it helps to simulate your program on paper with a column for each variable, writing down the values at each step.

"&$FF" is described in the Stamp Manual. "&" is a bit-wise AND. "$FF" is a byte consisting of 8 one bits (written in hexadecimal). Look up ">>" in the Stamp Manual.

Ryan1234 · 2011-02-09 12:57

I am using the SERIN for position feed back, to confirm location.

So maybe i cannot use the position feed back the way i want...

Mike Green · 2011-02-09 13:06

You have not described how you want to use the position feedback, so I can't advise you on whether it's possible or not. Do re-read the documentation on the !SCRSP command.

Ryan1234 · 2011-02-09 14:09

Mike Green wrote: »

You have not described how you want to use the position feedback, so I can't advise you on whether it's possible or not. Do re-read the documentation on the !SCRSP command.

Ryan1234 · 2011-02-09 14:12

I searched through the The "What's a Microcontroller?" tutorial and the "Basic Stamp Syntax and Reference Manual" for the !SCRSP command. I did not find anything on it, i did search for the SERIN and SEROUT and found info on them.
Is !SCRSP command maybe called somthing else in the manuals?

Mike Green · 2011-02-09 15:01

The !SC commands are described in the documentation on the Propeller Servo Controller. They're not part of PBasic. They're sent as text/bytes from your PBasic program to the Servo Controller. All of Parallax's documentation can be found via links on the product page for the specific product in the webstore (here). This includes links to sample programs, schematics, etc.

Note that the Propeller Servo Controller is a very powerful microcontroller in its own right. It comes preprogrammed as described in the documentation for use with a PC or another microcontroller, but it can be used by itself for very complex robotics as well as other tasks.

bsnut · 2011-02-10 00:04

Ryan1234,
You need to use the demo program that comes with the Serial Servo Controllers and then work your way up to another small program.

What are trying to do is not hard to do. Remember, keep it simple. And, what Mike is talking about reading "What is a Microcontroller" book is to help you learn the code and shows you how code something like servos. That's why we point you to this book.

Ryan1234 · 2011-02-18 14:02

Using the Basic stamp and servo controller, what is the best way to make 2 servos move at the same time (relativly) and move different directions?

Mike Green · 2011-02-18 14:37

You've already demonstrated how:

SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]

The first SEROUT sends a command to the servo controller that tells it to start moving the servo in channel 0 to position or speed/direction pw0 with a ramp speed of ra. The second SEROUT sends a command to start moving the servo in channel 1 to pw1 with the same ramp value. The second servo will start moving while the first servo is still starting to move. If you're using continuous motion servos, the pw# value specifies the speed and direction of the movement. For standard servos, the pw# value specifies the desired end position of the servo. The ramp value indicates how quickly the servo should change its motion or position. Most servos are designed to respond to a control pulse repeated about 50 times a second and ranging from about 1ms to 2ms in width. This pulse is what the servo controller produces and what the pw# value represents. For continuous motion servos, 1ms is maximum speed in one direction while 2ms is maximum speed in the other direction. Usually 1.5ms is the "stop" value between the two extremes. For standard servos, 1ms and 2ms are the ends of the range of motion of the servo. Some servos allow a wider range of pulse widths.

erco · 2011-02-18 19:09

Ryan1234: If you're only dealing with 2 or 3 servos in the 3-4 scenarios you outlined in your first reply to Mike, you can do it with just a Stamp; no servo controller is necessary.

Have you "mastered" driving a single servo at a given speed from position 1 to position 2 and back? You can use DO loops, FOR-NEXT loops, manual counters, there are several ways to skin this cat. Once you can do that, you can weave two servo routines together to move them in sync, in reverse, or any way you like.

Mark Kibler · 2011-02-19 06:25

erco;978359]Ryan1234: If you're only dealing with 2 or 3 servos in the 3-4 scenarios you outlined in your first reply to Mike, you can do it with just a Stamp; no servo controller is necessary.

I also thought that using a simple program might work too. We've never used a servoconroller since there was no apparent need to. This is also how our mentors on the forum taught us how to do it. We're only using 2 or 3 servos at one time.

Would this work?

' {$STAMP BS2}
' {$PBASIC 2.5}

counter VAR Word

DEBUG "ARLISS Team New Hampshire", CR, CR
DEBUG "Continuous servomotor rotation program", CR, CR
DEBUG "Program will start in 3 seconds" CR, CR

PAUSE 3000 'This is the 3 second pause command

FOR counter = 1 TO 2000 '<---- This counts or determines the number of "loops" sent to the servomotors
' One loop takes 1.3 ms + 1.7 ms + 50 ms + 1.6 ms
'
' To calculate how long (in sec) the motors will run at FULL speed (650, 850) use:
' 2000 * 1000 ms/sec / (1.3 ms + 1.7 ms + 50 ms + 1.6 ms)
' 1.6 ms is the 'program code overhead' value, how long it takes the BOE to execute ONE loop

PULSOUT 12, 650 'Clockwise, full speed - The pulse length is 1.3 ms
PULSOUT 13, 850 'Counter clockwise, full speed - The pulse length is 1.7 ms

' A PULSOUT value of 750 puts the motors in 'neutral' position and they won't move
' A PULSOUT value > 750 makes the servomotors go counterclockwise
' A value < 750 makes the motors go clockwise
' As the value decreases below 650 (full speed) the motors go slower
' As the value increases above 850 (full speed) the motors also go slower
' The #13 and #12, etc. after PULSOUT tells the BOE which slot the electrical impulse outputs to

PAUSE 50 '50 ms pause between pulses to each servomotor

NEXT

END

I believe you could also add a 'DO - LOOP' if needed. We're rookies so take this suggestion with a grain of salt (and two aspirin. But take the aspirin first.)

Let me now how it works, and keep at it.You'll get there,

Good morning,

Mark - ARLISS Team New Hampshire

erco · 2011-02-19 09:27

Ryan1234:

Moving 2 servos in the same direction is easy, just send the same pulsouts to 2 different pins. For 2 servos moving in opposite directions, try:

for w1=500 to 1000 step 5' w1 increases by 5 units each loop, range 500-1000 change value of '5' for different speeds
w2=1500-w1' w2 decreases by 5 units each loop, range 1000-500 (for same direction motion, change to w2=w1)
pulsout 0, w1' drive servo#1 on p0
pulsout 1,w2' drive servo#2 on p1
pause 20
next

Not too tough, right? Now it's your turn to code the return to the start.

Ryan1234 · 2011-02-21 08:27

It dose not seem to do anything with 2 servos...?
It works fine with the 1 servo setup... can you tell me why 1 works and 2 dose not..?

1 Servo program
' {$STAMP BS2}
' {$PBASIC 2.5}
ch0 VAR Byte
pw0 VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
SDAT CON 15
BAUD CON 396
COL VAR Nib
ROW VAR Nib
INIT:
RA =7
ch0 = 14
COL = 0
DEBUG "

SPEED SET ---"
DO WHILE (COL < 10)
FOR ROW = 0 TO 15
IF (ROW > 9) THEN EXIT
DEBUG CRSRXY, (COL * 8), ROW,
DEC COL, "/", DEC ROW, CR
NEXT
COL = COL + 1
IF (COL = 20) THEN GOSUB ENGSTOP '2 CYCLES + 1
DEBUG CLS
DEBUG "

SPEED SET---"
PW0 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW0 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
LOOP
WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 200
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH0, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN
ENGSTOP: 'ENGINE STOP
PW0 = 760: GOSUB WRSERVO1
PW0 = 780: GOSUB WRSERVO1
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
DEBUG CLS, "

SPEED SET

"
DEBUG "-- CYCLE COMPLETE -- ", DEC COL (0), "-- ENGINE STOP"
END

2 Servo program

' {$STAMP BS2}
' {$PBASIC 2.5}
ch0 VAR Byte
ch1 VAR Byte
pw0 VAR Word
pw1 VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
SDAT CON 15
BAUD CON 396
COL VAR Nib
ROW VAR Nib
INIT:
RA =7
ch0 = 14
ch1 = 15
COL = 0
DEBUG "

SPEED SET ---"
DO WHILE (COL < 10)
FOR ROW = 0 TO 15
IF (ROW > 9) THEN EXIT
DEBUG CRSRXY, (COL * 8), ROW,
DEC COL, "/", DEC ROW, CR
NEXT
COL = COL + 1
IF (COL = 20) THEN GOSUB ENGSTOP '2 CYCLES + 1
DEBUG CLS
DEBUG "

SPEED SET---"
PW0 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW0 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
LOOP
WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 200
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH0, CR]
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH1, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN
ENGSTOP: 'ENGINE STOP
PW0 = 760: GOSUB WRSERVO1
PW1 = 760: GOSUB WRSERVO1
PW0 = 780: GOSUB WRSERVO1
PW1 = 780: GOSUB WRSERVO1
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
DEBUG CLS, "

SPEED SET

"
DEBUG "-- CYCLE COMPLETE -- ", DEC COL (0), "-- ENGINE STOP"
END

can you help?

Franklin · 2011-02-21 08:50

It dose not seem to do anything with 2 servos...?

First off you need to 'wrap' your code in "code" tags so the formatting will stay intact or 'attach' your program to the post so it can be loaded and run by others. Second you need to explain just what it does and does not do.

Ryan1234 · 2011-02-21 09:04

OK, This program is moving both servos, but this issue now is that they are moving in order, they do not seem to be running independent, it is more than the 5ms delay. the programs are running in order and the delay, how do it get it start servo 1 the same time it starts servo 0? I can deal with the slight delay of the data being sent. I can get them moving at the same time if there is only 1 PW(#). Do they need to be set up in individal sub routines? I feel like i am almost there!

' {$STAMP BS2}
' {$PBASIC 2.5}
ch0 VAR Byte
ch1 VAR Byte
pw0 VAR Word
pw1 VAR Word
RA VAR Byte
X VAR Byte
BUFF VAR Byte(3)
SDAT CON 15
BAUD CON 396
COL VAR Nib
ROW VAR Nib
INIT:
RA =7
ch0 = 14
Ch1 = 15
COL = 0
DEBUG "

SPEED SET ---"
DO WHILE (COL < 10)
FOR ROW = 0 TO 15
IF (ROW > 9) THEN EXIT
DEBUG CRSRXY, (COL * 8), ROW,
DEC COL, "/", DEC ROW, CR
NEXT
COL = COL + 1
IF (COL = 3) THEN GOSUB ENGSTOP '2 CYCLES + 1
DEBUG CLS
DEBUG "

SPEED SET---"
PW0 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW0 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 723: GOSUB WRSERVO1 'OC IDLE - 15 SEC
PW1 = 750: GOSUB WRSERVO1 'OC IDLE - 15 SEC
LOOP
WRSERVO1: 'FASTEST SERVO 30 SECONDS (4100)
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
FOR X = 0 TO 6
PAUSE 300
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH0, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
NEXT
RETURN
ENGSTOP: 'ENGINE STOP
PW0 = 960: GOSUB WRSERVO1
PW1 = 960: GOSUB WRSERVO1
PW1 = 980: GOSUB WRSERVO1
PW0 = 980: GOSUB WRSERVO1
SEROUT Sdat, Baud+$8000,["!SC", ch0, ra, pw0.LOWBYTE, pw0.HIGHBYTE, CR]
SEROUT Sdat, Baud+$8000,["!SC", ch1, ra, pw1.LOWBYTE, pw1.HIGHBYTE, CR]
DEBUG CLS, "

SPEED SET

"
DEBUG "-- CYCLE COMPLETE -- ", DEC COL (0), "-- ENGINE STOP"
END

Ryan1234 · 2011-02-21 09:09

Here is the program..

Ryan1234 · 2011-02-21 09:11

This program is used to move 2 servos, using the basic stamp and servo controller. It seems to be working OK, This program is moving both servos,

but this issue now is that they are moving in order, they do not seem to be running independent, it is more than the 5ms delay. the programs are running in order and the delay, how do it get it start servo 1 the same time it starts servo 0? I can deal with the slight delay of the data being sent. I can get them moving at the same time if there is only 1 PW(#). Do they need to be set up in individal sub routines? I feel like i am almost there!

Ryan1234 · 2011-02-22 08:51

Upload program1.bs2Mike,

It seems that the servos are still having issues. the program runns in order and if i inter lace the to servos together in the program the second servo will not start until the pause is complete on the first servo. is there a better way to simulate a pause on the servos without affecting the timing on each other?

PW0 will not continue until PW1 has completed its cycle with the pause.

PW1 = 780: GOSUB WRIDEL1 'OC IDLE - 15 SEC
PW0 = 750: GOSUB WRIDEL1 'OC IDLE
PW0 = 670: GOSUB WRSERVO2_450 '9E
PW1 = 760: GOSUB WRIDEL1 'F8 5-7 MPH WARM - 15 SEC
PW0 = 749: GOSUB WRIDEL1 'F8
PW0 = 661: GOSUB WRSERVO2_450 '95 IDLE

WRIDEL1: 'IDEL 15 SECONDS
SEROUT SDAT, BAUD+$8000,["!SC", CH1, RA, PW1.LOWBYTE, PW1.HIGHBYTE, CR]
SEROUT SDAT, BAUD+$8000,["!SC", CH0, RA, PW0.LOWBYTE, PW0.HIGHBYTE, CR]
PAUSE 1800
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH1, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- IDEL 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
RETURN

WRSERVO2_450: 'FASTEST SERVO (450) 30 SECONDS (4100)
SEROUT SDAT, BAUD+$8000,["!SC", CH1, RA, PW1.LOWBYTE, PW1.HIGHBYTE, CR]
SEROUT SDAT, BAUD+$8000,["!SC", CH0, RA, PW0.LOWBYTE, PW0.HIGHBYTE, CR]
PAUSE 2000
SEROUT SDAT, BAUD+$8000,["!SCRSP", CH0, CR]
SERIN SDAT, BAUD, 1000, INIT,[STR BUFF\3]
DEBUG "CURRENT CYCLE -", DEC COL(0), "- SERVO 1-", DEC BUFF(1), " LOCATION - ", HEX2 BUFF(2), CR
RETURN

Mike G · 2011-02-22 11:44

There's nothing wrong with the servo controller. The PSC is just doing exactly what your logic is telling it to do.

Break your code into smaller reusable chunks. It will be a heck of a lot easier to debug.

You only need one of these methods.

Write_PSC:
  SEROUT SDAT, BAUD+$8000,["!SC", channel, ramp, servoPos.LOWBYTE, servoPos.HIGHBYTE, CR]
RETURN

Which can replace all the methods that look like this.

WRIDEL1: 'IDEL   15 SECONDS
  SEROUT SDAT, BAUD+$8000,["!SC", CH1, RA, PW1.LOWBYTE, PW1.HIGHBYTE, CR]
  SEROUT SDAT, BAUD+$8000,["!SC", CH0, RA, PW0.LOWBYTE, PW0.HIGHBYTE, CR]
  PAUSE  1800
  SEROUT SDAT, BAUD+$8000,["!SCRSP", CH1, CR]
  SERIN  SDAT, BAUD, 1000, INIT,[STR BUFF\3]
  DEBUG  "CURRENT CYCLE -", DEC COL(0), "- IDEL 1-", DEC BUFF(1), "  LOCATION - ", HEX2 BUFF(2), CR
  RETURN

Simply load up the variable and call the method. Similar to what you're doing with the positions... So here center 1 and 2. Wait for a second. Move the servos in opposite directions. Wait a second and loop forever.

MAIN:

  pscChannel = 0
  ramp = 1
  servoPos = 750
  GOSUB Write_PSC

  pscChannel = 1
  GOSUB Write_PSC

  PAUSE 1000


  pscChannel = 0
  ramp = 1
  servoPos = 300
  GOSUB Write_PSC

  pscChannel = 1
  servoPos = 1200
  GOSUB Write_PSC

  PAUSE 1000

GOTO MAIN

Write_PSC:
  SEROUT PSC,Baud,["!SC", pscChannel, ramp,
         servoPos.LOWBYTE, servoPos.HIGHBYTE, CR]
RETURN

Do the same kind of thing with all the

  SEROUT SDAT, BAUD+$8000,["!SCRSP", CH1, CR]
  SERIN  SDAT, BAUD, 1000, INIT,[STR BUFF\3]

Just put that logic in one method like

REPORT_POSITION:
  SEROUT SDAT, BAUD+$8000,["!SCRSP", channel, CR]
  SERIN  SDAT, BAUD, 1000, INIT,[STR BUFF\3]
  RETURN

Get all the debug and pause statements out of your methods and put them in their own spot too. This will make your code base smaller and concise. It will be much easier to assemble your main logic.

Finally, Consider using compiler directives so you can turn off debugging. Each DEBUG statement take a few ms to run depending on the data sent to the terminal.

Basic Stamp & servo controller with more than 1 servo- Please Help!

Comments