Can U make this D.A.I.R. program BETTER?

wperko

Hi,

First of all I am NOT an ace programmer ... I get by ... but this demo program for my D.A.I.R. Home Hobby Robot Kit is too big!

I need more variable space ... maybe more programming space even ... but since I'm not selling this program it's wide-open for stronger programmers to improve on it ... also to convert it for the Propeller and Arduino boards too ... besides any other µController boards out there ... the idea of the D.A.I.R. kit is that the end user prints their own robot parts, then assembles their own D.A.I.R. and using their favorite µController board will program their D.A.I.R. and share their results on my website and here in the Parallax forums as well as any other places they wish to share.

I have a crappy PBASIC program to do motions ... but it's a bunch of work to setup a sequence of movements;
http://www.youtube.com/watch?v=8WDjUoqKQaE

What I'm hoping to do is provide a PBASIC program for those who will print their own D.A.I.R. so they can put it to work easier and sooner. I'm also hoping other people will use an Arduino and Propeller to drive the D.A.I.R. and post their hardware/software solutions on my site to make it easier for others using Arduino and Propeller or other µControllers.

ITMT, this is my initial solution for sorting out the initial structure for a better PBASIC solution. Then once the program is capable of using all the required ports, positions and steps it can be better crunched into one large subroutine off the Main Body maybe.



Complete D.A.I.R. PBASIC Program
v0.0.0-DAIR-MotorControl.bs2

Left Arm Only D.A.I.R. PBASIC Program
v0.0.1L-DAIR-MotorControl.bs2

Right Arm Only D.A.I.R. PBASIC Program
v0.0.1R-DAIR-MotorControl.bs2



Complete D.A.I.R. PBASIC Program Listing:

' v0.0.0-DAIR-MotorControl.bs2
' Test the servo at three different position signals.
' {$STAMP BS2}
' {$PBASIC 2.5}

' Declare Variables & Constants
i VAR Word
WaistPort CON 0
Waistposition VAR Byte
Waistlimit VAR Word
Waiststep VAR Byte
' *******************************************************************
RightBodyShoulderPort CON 1
RightBodyShoulderposition VAR Byte
RightBodyShoulderlimit VAR Word
RightBodyShoulderstep VAR Byte
' ********************
RightArmShoulderPort CON 2
RightArmShoulderposition VAR Byte
RightArmShoulderlimit VAR Word
RightArmShoulderstep VAR Byte
' ********************
RightElbowPort CON 3
RightElbowposition VAR Byte
RightElbowlimit VAR Word
RightElbowstep VAR Byte
' ********************
RightWristPort CON 4
RightWristposition VAR Byte
RightWristlimit VAR Word
RightWriststep VAR Byte
' ********************
RightToolPort CON 5
RightToolposition VAR Byte
RightToollimit VAR Word
RightToolstep VAR Byte
' ********************
RightTool2Port CON 6
RightTool2position VAR Byte
RightTool2limit VAR Word
RightTool2step VAR Byte
' *******************************************************************
LeftBodyShoulderPort CON 7
LeftBodyShoulderposition VAR Byte
LeftBodyShoulderlimit VAR Word
LeftBodyShoulderstep VAR Byte
' ********************
LeftArmShoulderPort CON 8
LeftArmShoulderposition VAR Byte
LeftArmShoulderlimit VAR Word
LeftArmShoulderstep VAR Byte
' ********************
LeftElbowPort CON 9
LeftElbowposition VAR Byte
LeftElbowlimit VAR Word
LeftElbowstep VAR Byte
' ********************
LeftWristPort CON 10
LeftWristposition VAR Byte
LeftWristlimit VAR Word
LeftWriststep VAR Byte
' ********************
LeftToolPort CON 11
LeftToolposition VAR Byte
LeftToollimit VAR Word
LeftToolstep VAR Byte
' ********************
LeftTool2Port CON 12
LeftTool2position VAR Byte
LeftTool2limit VAR Word
LeftTool2step VAR Byte
' ********************

' Initialize Variables & Constants
i = 0
position1 CON 200
position2 CON 300
position3 CON 850
position4 CON 1000
position5 CON 1150

' HX12K FullRight = 375, Center = 900, FullLeft = 1100
' Futaba Std FullRight = 200, Center = 850, FullLeft = 1150
' HiTec HS422 Std FullLeft = 250, Center = 725, FullRight = 1200
' ********************
Waistposition = 200 ' Where to move the joint to
Waistlimit = 1150 ' Goto Position
' ********************
RightBodyShoulderposition = 200 ' Where to move the joint to
RightBodyShoulderlimit = 1150 ' Goto Position
' ********************
RightArmShoulderposition = 200 ' Where to move the joint to
RightArmShoulderlimit = 1150 ' Goto Position
' ********************
RightElbowposition = 200 ' Where to move the joint to
RightElbowlimit = 1150 ' Goto Position
' ********************
RightWristposition = 200 ' Where to move the joint to
RightWristlimit = 1150 ' Goto Position
' ********************
RightToolposition = 200 ' Where to move the joint to
RightToollimit = 1150 ' Goto Position
' ********************
RightTool2position = 200 ' Where to move the joint to
RightTool2limit = 1150 ' Goto Position
' *******************************************************************
LeftBodyShoulderposition = 200 ' Where to move the joint to
LeftBodyShoulderlimit = 1150 ' Goto Position
' ********************
LeftArmShoulderposition = 200 ' Where to move the joint to
LeftArmShoulderlimit = 1150 ' Goto Position
' ********************
LeftElbowposition = 200 ' Where to move the joint to
LeftElbowlimit = 1150 ' Goto Position
' ********************
LeftWristposition = 200 ' Where to move the joint to
LeftWristlimit = 1150 ' Goto Position
' ********************
LeftToolposition = 200 ' Where to move the joint to
LeftToollimit = 1150 ' Goto Position
' ********************
LeftTool2position = 200 ' Where to move the joint to
LeftTool2limit = 1150 ' Goto Position
' ********************


' *******************************************************************
' * * * * * Main Body * * * * * *
' *******************************************************************
GOSUB init
PAUSE 500

' *******************************************************************
' Example Position & Speed Declarations to Run
' *******************************************************************
RightBodyShoulderposition = i
RightBodyShoulderlimit = position5
RightBodyShoulderstep = 2
GOSUB RightBodyShoulderMotor
PAUSE 500

RightBodyShoulderposition = i
RightBodyShoulderlimit = position3
RightBodyShoulderstep = 4
GOSUB RightBodyShoulderMotor
PAUSE 500
RightBodyShoulderposition = i
RightBodyShoulderlimit = position5
RightBodyShoulderstep = 2
GOSUB RightBodyShoulderMotor
PAUSE 500
RightBodyShoulderposition = i
RightBodyShoulderlimit = position2
RightBodyShoulderstep = 10
GOSUB RightBodyShoulderMotor
PAUSE 500
RightBodyShoulderposition = i
RightBodyShoulderlimit = position4
RightBodyShoulderstep = 5
GOSUB RightBodyShoulderMotor
' *******************************************************************
RightArmShoulderposition = i
RightArmShoulderlimit = position5
RightArmShoulderstep = 2
GOSUB RightArmShoulderMotor
PAUSE 500

RightArmShoulderposition = i
RightArmShoulderlimit = position3
RightArmShoulderstep = 4
GOSUB RightArmShoulderMotor
PAUSE 500
RightArmShoulderposition = i
RightArmShoulderlimit = position5
RightArmShoulderstep = 2
GOSUB RightArmShoulderMotor
PAUSE 500
RightArmShoulderposition = i
RightArmShoulderlimit = position2
RightArmShoulderstep = 10
GOSUB RightArmShoulderMotor
PAUSE 500
RightArmShoulderposition = i
RightArmShoulderlimit = position4
RightArmShoulderstep = 5
GOSUB RightArmShoulderMotor


END


' *******************************************************************
' Subroutines
' *******************************************************************
init:
FOR i = 0 TO 100 STEP 1
PULSOUT WaistPort, 200
PULSOUT RightBodyShoulderPort, 200
PULSOUT RightArmShoulderPort, 200
PULSOUT RightElbowPort, 200
PULSOUT RightWristPort, 200
PULSOUT RightToolPort, 200
PULSOUT RightTool2Port, 200
' *******************************************************************
PULSOUT LeftBodyShoulderPort, 200
PULSOUT LeftArmShoulderPort, 200
PULSOUT LeftElbowPort, 200
PULSOUT LeftWristPort, 200
PULSOUT LeftToolPort, 200
PULSOUT LeftTool2Port, 200
PAUSE 20
NEXT
RETURN

WaistMotor:
FOR i = Waistposition TO Waistlimit STEP Waiststep
PULSOUT WaistPort, i
PAUSE 20
NEXT
RETURN

RightArmShoulderMotor:
FOR i = RightArmShoulderposition TO RightArmShoulderlimit STEP RightArmShoulderstep
PULSOUT RightArmShoulderPort, i
PAUSE 20
NEXT
RETURN

RightElbowMotor:
FOR i = RightElbowposition TO RightElbowlimit STEP RightElbowstep
PULSOUT RightElbowPort, i
PAUSE 20
NEXT
RETURN

RightWristMotor:
FOR i = RightWristposition TO RightWristlimit STEP RightWriststep
PULSOUT RightWristPort, i
PAUSE 20
NEXT
RETURN

RightToolMotor:
FOR i = RightToolposition TO RightToollimit STEP RightToolstep
PULSOUT RightToolPort, i
PAUSE 20
NEXT
RETURN

RightTool2Motor:
FOR i = RightTool2position TO RightTool2limit STEP RightTool2step
PULSOUT RightTool2Port, i
PAUSE 20
NEXT
RETURN

' *******************************************************************
LeftBodyShoulderMotor:
FOR i = LeftBodyShoulderposition TO LeftBodyShoulderlimit STEP LeftBodyShoulderstep
PULSOUT LeftBodyShoulderPort, i
PAUSE 20
NEXT
RETURN

LeftArmShoulderMotor:
FOR i = LeftArmShoulderposition TO LeftArmShoulderlimit STEP LeftArmShoulderstep
PULSOUT LeftArmShoulderPort, i
PAUSE 20
NEXT
RETURN

LeftElbowMotor:
FOR i = LeftElbowposition TO LeftElbowlimit STEP LeftElbowstep
PULSOUT LeftElbowPort, i
PAUSE 20
NEXT
RETURN

LeftWristMotor:
FOR i = LeftWristposition TO LeftWristlimit STEP LeftWriststep
PULSOUT LeftWristPort, i
PAUSE 20
NEXT
RETURN

LeftToolMotor:
FOR i = LeftToolposition TO LeftToollimit STEP LeftToolstep
PULSOUT LeftToolPort, i
PAUSE 20
NEXT
RETURN

LeftTool2Motor:
FOR i = LeftTool2position TO LeftTool2limit STEP LeftTool2step
PULSOUT LeftTool2Port, i
PAUSE 20
NEXT
RETURN

END


The D.A.I.R. Project webpage: http://www.brainless.org/DAIR-Kit.html

...

Publison

And you can also include the .bs2 program as an attachment.

This would make it much easier to read and debug.

Duane Degn

Do I count 13 servos?

That a bit much for Basic Stamp to handle. Even still the program has issues even if it only had a single servo.

The code:

[COLOR=#333333]RightBodyShoulderposition = i[/COLOR]
[COLOR=#333333]RightBodyShoulderlimit = position5[/COLOR]
[COLOR=#333333]RightBodyShoulderstep = 2[/COLOR]
[COLOR=#333333]GOSUB RightBodyShoulderMotor[/COLOR]
[COLOR=#333333]PAUSE 500[/COLOR]

Like many other sections of code this includes a 500ms pause. Servos need to be refreshed about every 20ms. If you want the robot to hold a servo position 500ms you'll need to send it about 25 pulses (to each servo in use) during the 500ms.

The Prop holds all the answers for your troubles. It has much more memory than a BS2 and takes care of driving the servos in the background so you don't need to worry about refreshing pulses.

I helped someone with a robotic fish project a while back. They were using a BS1. Man it made me appreciate the Prop. You can drive multiple servos with a basic stamp but it requires a keeping track of the state of the various servos and each time through the loop (which hopefully doesn't take much longer than 20ms) you send a pulse to each of the servos.

All your subroutine only pulse one of the servos. Your robot's other servos wont maintain torque while the program moves an individual servo.

Again this wouldn't be a problem with the Prop. Even an Arduino (heaven forbid) will handle the servos for you in the background.

Sorry to say, but if you want to keep using the BS2 with your robot, your program needs a major overhaul.

wperko

Hi,

The code works. Look at the subroutines and you'll see the code is done right. I've been testing each module and they all work perfectly. But it's too much for one BS2 so I figured if there's a good programmer they can maybe make it better. I don't need feedback that isn't a complete solution. I'm not changing my code unless I see a real solution to compress the code ... e.g., using an array for the position settings and variables etc...

If you don't have a real solution don't keep posting theories and dreams. It's annoying and distracts people from the real thread.

wperko

Hi,

The programs ARE all attached via a simple link.

Click on one of the links and you get to download the attachment.

Mike Green

The code doesn't work. Each module works individually and the problems you're running into are a result of combining them in one program. It's not just a matter of "compressing" the code in space. You also have to compress the code in time and the BS2 is neither fast enough nor is capable of overlapping operations to the extent that the servos will work properly. It's not a matter of having a "real solution" or not. It's a matter first of understanding the timing constraints you're working under. In particular, the minimum range for servo pulses is 1ms to 2ms with some servos ranging from 0.5ms to 2.5ms for a full range of motion. Servos have to see a pulse once every 20ms or they shut themselves off until the next pulse comes along. A "shut off" servo will slip and will exhibit some vibration as it turns on and off repeatedly. If you're going to limit the range of motion to keep the 13 servos turned on or if you can tolerate intermittent servo control, please say so. It's a major design decision. Note that you have to reduce your PAUSEs to compensate for multiple servos being controlled. In fact, you really should dynamically change the PAUSEs as the pulse width changes when you're controlling multiple servos.

As Duane mentioned, you really need to take a step back and define functionally what you want to accomplish with the finished program in terms of movement. If you're really just interested in moving one servo at a time and you don't need to keep them from slipping ... that's one design. If you're really just interested in moving one servo at a time and you do need to keep the others from slipping ... that's another design. If you need to move several servos simultaneously ... that's yet another design. Frankly, I agree with Duane. A BS2 is unlikely to be adequate for the task. You need to offload some of the work to an external servo controller.

Duane Degn

wperko wrote: »

The code works. Look at the subroutines and you'll see the code is done right. I've been testing each module and they all work perfectly.

If a "module" is a subroutine like "RightArmShoulderMotor" then yes, the module looks like it works correctly for the one servo it is moving. However all the other servos are not being powered while the single servo is being positioned. Apparently none of the servos in your Dual Arm Industrial Robot require holding torque above that provided by the static friction of the idle servos or you would likely already have noticed this issue.

However if a "module" is one of the attached BS2 programs then I disagree with your definition of "works". I personally don't consider a program which leaves the majority of the servos in a robot idle as working code.

wperko wrote: »

But it's too much for one BS2 so I figured if there's a good programmer they can maybe make it better. I don't need feedback that isn't a complete solution.

It's actually unusual to receive feedback in the form a "complete solution". Generally people point out problems they see and possibly offer suggestions on ways the problems may be overcome (which is what I thought I had done).

wperko wrote: »

I'm not changing my code unless I see a real solution to compress the code ... e.g., using an array for the position settings and variables etc...

There may be ways of reducing the code size. With the current code this could be done by eliminating many of the individual servo subroutines and have a subroutine "MoveServo" instead of a separate movement subroutine for each servo. Since you're only moving one servo at a time I don't see the need for 13 separate subroutines.

Even if you streamline the code this way you'll still have the (in my opinion) critical flaw of not sending multiple servos pulses to hold their positions.

I found the robotic fish thread I mentioned previously. I'm not sure what possessed me to pretty much write all the code for the guy but I like to think it's a pretty good example of how to control multiple servos with a Basic Stamp while also monitoring sensors. The final code is in post #53 of the thread. I suppose I wrote the code because I had a dream of the robotic fish living up to its potential. The fish actually worked pretty well. I can't find a video of it anymore though.

In order to drive multiple servos at once with a Basic Stamp you need to have each servo move a small amount through each loop of the program. In order to move both the "fin" and "rudder" servos at the same time in the robotic fish, I had to keep track of the state of the two servos.

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:

Depending on the "state" of the servo (which direction it was moving) the code would branch to the appropriate code to update its position.

Here's the "rudderCheckFromLeft" code (the BS1 doesn't use subroutines).

rudderCheckFromLeft:
  rudderPosition = rudderPosition + RudderStep
  IF rudderPosition >= CenterRudderP THEN stopRudderFL
  GOTO afterRudderCheck
stopRudderFL:
  rudderState = RudderCenteredS
  rudderPosition = CenterRudderP
  GOTO afterRudderCheck

I'm not sure how many servos could be controlled this way with a BS2. You might be able to control all the servos in a single arm to allow more complex movements but 13 servos would be much too many to control this way.

As I theorized previously, controlling multiple servos with a BS1 really made be appreciate the Propeller. The Propeller can control up to 32 servos at once.

You could even use a Propeller as a slave to the BS2. Parallax sells a servo controller which uses a Propeller chip. I believe the servo controller can control up to 16 servos. A less expensive option would be to use a QuickStart board as a slave. A slave Propeller could control up to 28 servos. I say 28 instead of 32 since it's a good idea not to attach servos to the four I/O pins the Propeller uses at bootup.

The BS2 could send serial commands to the Propeller with the desired servo positions and the Prop would take care of the 50Hz refresh signals the servos require to maintain torque.

wperko wrote: »

If you don't have a real solution don't keep posting theories and dreams.

What's a real solution?

I'm afraid, I'll just keep on theorizing on how to make robots better and posting my dreams of robots living up to their potential.

wperko wrote: »

It's annoying and distracts people from the real thread.

Sorry you chose to be annoyed. I generally appreciate it when people try to help me. Isn't the "real thread" about making your Dual Arm Industrial Robot code better? I thought that's what my post was about.

Genetix

Use PIN for the Port assignments instead of CON. CON is for constant values and PIN is for assigning the BS2 I/O Pins.
The BS2 only has 26 bytes of RAM and yet you declare 24 Byte variables and 12 Word Variables, which requires 48 bytes of RAM.

wperko

Hi,

Wow actual help ... Okay, I did that ... changed the CON to a PIN ... does that save variable space?

RightBodyShoulderPort PIN 1
RightBodyShoulderposition VAR Byte
RightBodyShoulderlimit VAR Word
RightBodyShoulderstep VAR Byte

... I really hope the core of the program can be shrunk to all fit in one BS2.

... thanks ...

Mike Green

No, changing CON to PIN doesn't save variable or program space. It's a convenience thing and it makes programs easier to understand. The reason for it is that there are two ways to describe an I/O pin. Some statements expect a pin number while other statements expect a predefined name like IN0 or OUT5. You can take care of the first case using CON to give a name to a particular number. The second case is tougher and you can't combine them. Enter the PIN declaration. This allows a single name to be used 3 different ways to represent an I/O pin. If you declare "RightBodyShoulderPort PIN 1", the you can use RightBodyShoulderPort anywhere a pin number is expected, like in a PULSOUT statement. You can also use RightBodyShoulderPort where a variable name like OUT1 is expected or where IN1 might be used and the compiler "knows" what to do.

In terms of saving variable space, what you have to do is to use the same variable at multiple places in the program where it makes sense. For example, lets say you have variable X that's used in a subroutine somewhere in the program and another variable Y that's used in a different subroutine elsewhere in the program and, most importantly, X and Y never both contain a value that's needed in some other section of code. In that case, you can declare X and Y to be two different names for the same piece of storage and you've saved some variable space. Look at the section of the "Basic Stamp Syntax and Reference Manual" on Aliases and Modifiers starting around page 89.

wperko

Hi,

Then nesting loops to keep torque on all the servos can easily be arranged. My example above is to describe the scheme of things to improve the program design.

Little tips like Genetix are what I'm looking for ...

I'm sorry I didn't post the demo video of my crappier code running all the servos on one arm ... I know how to do that ... I'm just trying to find a PBASIC solution that is easier for the next kit builder to get going sooner than later so they can understand how to program movements more easily.

Check out these videos: http://www.youtube.com/watch?v=8WDjUoqKQaE

http://www.youtube.com/watch?v=xGApv4Xw9cU

wperko

Hi,

My current D.A.I.R. has 7 servo motors in it and I'm able to move them all simultaneously with the BS2 HomeWork Board ... I expect to add three more servo motors to complete the basic D.A.I.R. and maybe another one or two servo motors to add a gripper on one or two arms. Some people may want to add more servos for more DOF, but it's really not needed.
http://www.youtube.com/watch?v=8WDjUoqKQaE

The code sample above is to show where I'm trying to go ... as I state it's not a fully functional program because it's simply too big as is for a BS2. But the concept I'm trying to achieve making a program that an end-user can more easily get started with using their D.A.I.R. after building it.

Mike Green

7 servo motors I can believe. It's right at the edge of what a BS2 can do. At one end of the range of motion, that's 14ms just for the pulse time plus about 250us each for PULSOUT statement overhead plus about 1000us for the FOR/NEXT statement (and no PAUSE). 3 additional servos will push this over the edge with a loop time in the range of 13ms to 23ms with the latter well beyond the 20ms limit of R/C servos.

wperko

So I should switch to a BOE with a BS2p chip on it? 12,000 instructions/sec. over the ~4,000 instructions/sec. of the standard BS2 ... or 3x faster.

My other arm parts are at a friends house 500-miles away yet, but once I get them I'll have a total of 9 servo motors running my D.A.I.R. I don't have any plans to add another "wrist" or "tool2" joint. But the software cut down four servo controls still requires more variable space than available. That's the primary hurdle to making it easier for others to build and "operationally program" their own D.A.I.R. kit.

Mike Green

Switching to a faster Stamp can help a little, but you're still limited by the fact that the Stamp can issue only one control pulse at a time and the control pulses each can take as long as 2.5ms (2.5 x 9 = 22.5ms). If you're going to spend the money for a change, I'd go for an external servo controller like the Propeller Servo Controller. The main advantage of this is it's "set and forget". You send a desired servo position to it and a ramp speed and it takes care of all the intermediate positions as well as keeping the servo locked at the final position. It can do this for up to 16 servos simultaneously. It uses a very fast microcontroller to do the timing for 8 servo control pulses simultaneously which a Stamp can't do.

Duane Degn

wperko wrote: »

If you don't have a real solution don't keep posting theories and dreams. It's annoying and distracts people from the real thread.

Oh what a pleasant sentiment. It's almost refreshing after having so many "thank you" and "that did the trick" kind of replies.

As I mentioned previously:

Duane Degn wrote: »

There may be ways of reducing the code size. With the current code this could be done by eliminating many of the individual servo subroutines and have a subroutine "MoveServo" instead of a separate movement subroutine for each servo. Since you're only moving one servo at a time I don't see the need for 13 separate subroutines.

Apparently you really do want the code to just move a single servo at a time. This makes compressing your code much easier. Instead of a single side taking up all the variable space of the BS2, if you use my annoying suggestion above, you could have a BS2 memory like this.



I wasn't really sure what you wanted the servos to do since you use the variable "i" to set your "position" variables which are byte sized variables.

GOSUB initPAUSE 500


' *******************************************************************
'            Example Position & Speed Declarations to Run
' *******************************************************************
LeftBodyShoulderposition = i
LeftBodyShoulderlimit = position5
LeftBodyShoulderstep = 2
GOSUB LeftBodyShoulderMotor
PAUSE 500

In the code above, the line "LeftBodyShoulderposition = i" sets the byte sized variable "LeftBodyShoulderposition" to 101. The subroutine "init" leaves the variable "i" set to 101. I didn't think that is what you really intended. There are several times when the position variable is set to a truncated value of 'i" ("i" is appropriately a word). This would cause the servo to move from a relative high pulse value to 127. I would have thought these harsh glitches would have been noticeable in "worked perfectly" code.

I wasn't really sure which position you wanted the servos to start in. If all four movements are identical, then the code could be reduced in size a bit more.

Apparently I'm a slow learner but I've learned from my past mistakes and this time I present the only help appropriate to offer "a complete solution".

' v0.0.1L-DAIR-MotorControl.bs2' Test the servo at three different position signals.
' {$STAMP BS2}
' {$PBASIC 2.5}


' Declare Variables & Constants
  i VAR Word
  WaistPort CON 0
  RightBodyShoulderPort CON 1
  RightArmShoulderPort CON 2
  RightElbowPort CON 3
  RightWristPort CON 4
  RightToolPort CON 5
  RightTool2Port CON 6
  LeftBodyShoulderPort CON 7
  LeftArmShoulderPort CON 8
  LeftElbowPort CON 9
  LeftWristPort CON 10
  LeftToolPort CON 11
  LeftTool2Port CON 12


  activePort VAR Byte
  activeStep VAR Word
  startPosition VAR Word
  endposition VAR Word
  fixI VAR Word


' Initialize Variables & Constants
  i = 0
  position1 CON 200
  position2 CON 300
  position3 CON 850
  position4 CON 1000
  position5 CON 1150


  ' HX12K FullRight = 375, Center = 900, FullLeft = 1100
  ' Futaba Std FullRight = 200, Center = 850, FullLeft = 1150
  ' HiTec HS422 Std FullLeft = 250, Center = 725, FullRight = 1200


' *******************************************************************
'        * * * * *           Main Body           * * * * *          *
' *******************************************************************
  GOSUB init
  PAUSE 500


' *******************************************************************
'            Example Position & Speed Declarations to Run
' *******************************************************************


  activePort = RightBodyShoulderPort
  startPosition = Position1
  endPosition = position5
  activeStep = 2
  GOSUB MoveSingleServo
  PAUSE 500
  GOSUB Pattern5_3s4_5s2_2s10_4s5
' *******************************************************************


  activePort = RightArmShoulderPort
  startPosition = Position4
  endPosition = position5
  activeStep = 2
  GOSUB MoveSingleServo
  PAUSE 500
  GOSUB Pattern5_3s4_5s2_2s10_4s5
' *******************************************************************


  activePort = LeftBodyShoulderPort
  startPosition = Position1
  endPosition = position5
  activeStep = 2
  GOSUB MoveSingleServo
  PAUSE 500
  GOSUB Pattern5_3s4_5s2_2s10_4s5
' *******************************************************************


  activePort = LeftArmShoulderPort
  startPosition = position4
  endPosition = position5
  activeStep = 2
  GOSUB MoveSingleServo
  PAUSE 500
  GOSUB Pattern5_3s4_5s2_2s10_4s5


END




' *******************************************************************
'                          Subroutines
' *******************************************************************
init:
  FOR i = 0 TO 100 STEP 1
    PULSOUT WaistPort, position1
    PULSOUT RightBodyShoulderPort, position1
    PULSOUT RightArmShoulderPort, position1
    PULSOUT RightElbowPort, position1
    PULSOUT RightWristPort, position1
    PULSOUT RightToolPort, position1
    PULSOUT RightTool2Port, position1
  ' *******************************************************************
    PULSOUT LeftBodyShoulderPort, position1
    PULSOUT LeftArmShoulderPort, position1
    PULSOUT LeftElbowPort, position1
    PULSOUT LeftWristPort, position1
    PULSOUT LeftToolPort, position1
    PULSOUT LeftTool2Port, position1
    PAUSE 2
  NEXT
  RETURN


Pattern5_3s4_5s2_2s10_4s5:
  startPosition = Position5
  endPosition = position3
  activeStep = 4
  GOSUB MoveSingleServo
  PAUSE 500
  startPosition = i
  endPosition = position5
  activeStep = 2
  GOSUB MoveSingleServo
  PAUSE 500
  startPosition = i
  endPosition = position2
  activeStep = 10
  GOSUB MoveSingleServo
  PAUSE 500
  startPosition = i
  endPosition = position2
  activeStep = 10
  GOSUB MoveSingleServo
  PAUSE 500
RETURN


MoveSingleServo:
  IF startPosition < endPosition THEN
    fixI = activeStep
  ELSE
    fixI = activeStep * -1
  ENDIF
  FOR i = startPosition TO endPosition STEP activeStep
    PULSOUT activePort, i
    PAUSE 18
  NEXT
  i = i - fixI
  RETURN


END

I think I've been possessed again.

wperko

Hi,

I've grabbed this from the original message;
<quote>
Hi,

First of all I am NOT an ace programmer ... I get by ... but this demo program for my D.A.I.R. Home Hobby Robot Kit is too big!

I need more variable space ... maybe more programming space even ...
<end quote>

The rest of the program listing is a demonstration (the code is spread-out wide) of how I'm trying to figure out how to make a sequencer that's easy for an end user to add sequences of movements to the D.A.I.R.

I'm sorry that message didn't seem to get through to some.

So far Genetix and Mike Green had some good suggestions ... I already added the PIN instead of CON to the program. But still found I don't have enough variable space. So I thought maybe I should change the pin assignment to a variable Byte ... then for each motor just add 1 or 2 or 3 etc... so there's one variable for all motors while the statements would be something like "PULSOUT (motor+3), position" ... but while being a neat trick, it may not be easy for the end-user to understand, who is just beginning to learn.

<quote>
I have a crappy PBASIC program to do motions ... but it's a bunch of work to setup a sequence of movements;
http://www.youtube.com/watch?v=8WDjUoqKQaE
<end quote>

So far the code I'm using to actually run my D.A.I.R. with all the joints active together works ... it just seems to cumbersome for an initial demo program to get younger end-users programming sequences. I figured if I got weird results from the motor movements I'd trim the PAUSE 20 in the FOR/NEXT loops. I would hope that dynamic trimming really shouldn't be needed since all motors need to be held ON in all loops.

Also, even that PBASIC program limits the joints to 11 servo motors ... the timing seems to work okay for the servo motors although other servo motor brands may need stricter timing output. But as you can see I only had to comment out two of the variables to get my beginning D.A.I.R. demo program working well with the HX12K servo motors.
' DAIR-v1.0.0.bs2
' Test the servo at three different position signals.
' {$STAMP BS2}
' {$PBASIC 2.5}
' ******************************************************************

' ******************************************************************
' HX12K FullRight = 375, Center = 900, FullLeft = 1100
' Futaba Std FullRight = 200, Center = 850, FullLeft = 1150
' HiTec HS422 Std FullRight = 1200, Center = 725, FullLeft = 250
' ******************************************************************

' ******************************************************************
' Declare & Initialize Variables, Constants and Ports
RightBodyMotor PIN 0 ' Right Body Soulder Motor
RightArmMotor PIN 1 ' Right Arm Soulder Motor
RightElbowMotor PIN 2 ' Right Elbow Motor
RightWristMotor PIN 3 ' Right Wrist Motor
RightHandMotor PIN 4 ' Right Lower Wrist Continuous Rotation Servo
RightToolMotor PIN 5 ' Right Tool Motor

WaistMotor PIN 6 ' Waist - Continuous Rotation Servo

LeftBodyMotor PIN 7 ' Left Body Soulder Motor
LeftArmMotor PIN 8 ' Left Arm Soulder Motor
LeftElbowMotor PIN 9 ' Left Elbow Motor
LeftWristMotor PIN 10 ' Left Wrist Motor
LeftHandMotor PIN 11 ' Left Lower Wrist Continuous Rotation Servo
LeftToolMotor PIN 12 ' Left Tool Motor

counter1 VAR Word
counter2 VAR Word

RightBodyPosition VAR Word
RightArmPosition VAR Word
RightElbowPosition VAR Word
RightWristPosition VAR Word
RightHandPosition VAR Word
'** RightTool VAR Word

Waist VAR Word

LeftBodyPosition VAR Word
LeftArmPosition VAR Word
LeftElbowPosition VAR Word
LeftWristPosition VAR Word
LeftHandPosition VAR Word
'** LeftTool VAR Word

It's a working program, but just requires more work on the end-users side to sequence their D.A.I.R. kit.

http://www.brainless.org/DAIR-Kit.html

http://www.brainless.org/MultiMedia/DAIR-Kit/DAIR-v1.0.0.bs2

Now I guess I need to save up a few bucks and try the Propeller Servo Controller ...

Mike Green

I still don't understand why you've got all the xxxPosition variables when you're really moving only one servo at a time. You'd need only the variables startFrom, moveTo, servoPin, stepSize, and i:

moveServo:
   for i = startFrom to moveTo step stepSize
      pulsout servoPin, i
      pause 20
   next
   return

This would be called like this:

startFrom = 500
moveTo = 1000
stepSize = 4
servoPin = LeftElbowMotor
gosub moveServo

Another thing to think about for sequencing would be to use the program EEPROM to store movements. Look at the description of the DATA and READ statements in the Stamp Manual. You could store the above movement in 5 bytes of EEPROM (with a variable pause between movements). To avoid needing words for the positions, you could store the positions in a byte by subtracting 200 from them and dividing by 4 (for a range of 200 to 1200 or 0.4ms to 2.4ms).

address = 0   ' by default, DATA stores starting at address 0
gosub processPacket
' ...
processPacket:
   read address, startFrom   'get first value
   startFrom = startFrom * 4 + 200   'correct for fitting into a byte
   read address+1, moveTo   'same thing for second value
   moveTo = moveTo * 4 + 200
   read address+2,stepSize   'this can be stored directly
   read address+3,servoPin   'this can be stored directly
   read address+4,delay   'delay time
   delay = delay * 100   'units of 100ms
   if stepSize = 0 then return   'stepSize of zero marks end of sequence
   for i = startFrom to moveTo step stepSize   ' do requested movement
      pulsout servoPin,i
      pause 20
   next
   address = address + 5   'on to next packet of information
   pause delay   'after specified delay
   goto processPacket

Here would be a typical packet:

data 0,237,4,LeftElbowMotor,0   ' move LeftElbow from 200 to 1148 in steps of 4, no delay
data 230,5,4,RightElbowMotor,10   ' move RightElbow from 1120 to 220 in steps of 4, delay 1s afterwards
data 0,0,0,0,0   ' marks end of sequence