Wonderful! As long as you're fully dedicated to the Parallax cause, Ken's wheeling & dealing on SX chips right now, especially in bulk. I think each one of your servos needs a full dozen redundant SX chips monitoring in failsafe fashion, like a NASA launch. 10 out of 12 uCs need to agree to go to the next step.
Comparing RN, BASIC Language, PBASIC
You're doing good and on the right track. The code to drive the gyros is all in BASIC so there's no learning curve in going from a review of PBASIC to RN's BASIC. The brain and heart of the ROBONOVA-1 is the ATMEL ATMega 128 processor (as a reference only), its internal memory supplemented by a 64K*8 EEPROM as storage for Robo-Script and Robo-Basic programs. It about time we develop these sensors for Stamps and Propellers.
The LISY300 Gyroscope Module is a single-axis yaw rate sensor providing up to 300°/s full scale rotation detection at up to 88 Hz. Useful in balancing robots or auto-pilot systems, the LISY300 Gyroscope Module can detect how many degrees it has turned on its planar axis allowing the host microcontroller to stabilize the platform or correct for drift.
' =========================================================================
'
' File...... LISY300 Test V1.0.bs2
' Purpose... Test LISY300 Gyroscope Module
' Author.... Parallax, Inc.
' E-mail.... support@parallax.com
' Started... 11-15-2009
' Updated...
'
' {$STAMP BS2}
' {$PBASIC 2.5}
'
' =========================================================================
' -----[ Program Description ]---------------------------------------------
' This program test the LISY300 Gyroscope Module (#27922) by returning the
' ADC value. The ADC is 10 bits and returns a value from 0 - 1023 at max
' ranges. Typically when the Gyroscope Module is sitting idle the return
' value should be close to 512 (+/- 24). As the Gyroscope rotates in a
' clockwise manner the values go down relative to the speed at which the
' gyroscope is rotating. Conversely when the gyroscope is rotating in a
' counter-clockwise manner the values go up relative to the speed at which
' the gyroscope is rotating.
' The sample code uses a 250uS pause to reduce display jitter, however when
' reading the ADC in an application you would not have this delay. Please
' read the documentation to determine how to make use of the data for
' calculating yaw.
' -----[ Revision History ]------------------------------------------------
' -----[ I/O Definitions ]-------------------------------------------------
Dout PIN 0 ' P0 <-- Dout (LISY300.2)
SCLK PIN 1 ' P1 --> SCLK (LISY300.4)
CSn PIN 2 ' P2 --> /CS (LISY300.5)
' -----[ Constants ]-------------------------------------------------------
Yes CON 1 ' Yes Constant
No CON 0 ' No Constant
' -----[ Variables ]-------------------------------------------------------
value VAR Word ' ADC Result Value
' -----[ EEPROM Data ]-----------------------------------------------------
' -----[ Initialization ]--------------------------------------------------
Initialization:
HIGH CSn
LOW SCLK
PAUSE 250
' -----[ Program Code ]----------------------------------------------------
Main:
DO
GOSUB Read_Gyro
DEBUG HOME, "ADC Value:", DEC5 value
PAUSE 250
LOOP
STOP
' -----[ Subroutines ]-----------------------------------------------------
Read_Gyro:
LOW CSn
SHIFTIN Dout, SCLK, MSBPOST, [value\13]
HIGH CSn
RETURN
Redundancy
I'm not aware of any Parallax projects with up to 12 Stamp microcontroller modules for maintaining complete accuracy through redundancy. For this hobby leg application, I wouldn't worry about it. When using Stamps, it's more of a matter of getting multiple things to run simultaneously with multiple modules. Erco's idea is a good one that's more suited to Propellers which already have eight processors inside. Each prop could be redundant unto others - with multiple Cogs doing the cross checking.
I will look into the latest thoughts, I just wanted to post a quick update:
I didn't get to work on this as much as I'd like over the weekend due to work, but I did spend a couple hours in data ==> equation matching. The best so far is y = 0.25ABS(x+3.4)+2 + COS(Pi * (a * xx + 7) / 3) + COS(Pi * xx) + COS(Pi * (2 * xx + 5) / 3) * COS(Pi * (2 * xx + 7) / 3). and fits pretty well for the hip. This equation thing is a pretty difficult task actually. I think no one commented on the idea because I didn't explain myself clearly, or everyone knew the idea was bogus.
I'm not sure how well this data based function will work, maybe I'm not smart enough for the pattern matching. I have a backup plan if that doesn't work....... or I could research and steal someone else's working code
I downloaded a program that was supposed to take data points and spit out a matching function... It worked.....technically..... but the function is useless for a motion profile.
I realize this is a lame update, but the legs are 90% complete, no power up yet, so I haven't fixed the servo arms to the joints. I'll need some kind of torso (box) to join them together and house the batteries and controls.... but that will have to wait.
At the moment, I'm doing some programming on the PC to work out my code, steps and a graphics simulation.... ok, ok, my "Simulation" is a set of moving lines to indicate thigh, calf and foot for each leg.... but just something to see it move
Programming details: I really think a routine based on a time constant and amplitude (for lack of a better word) is the way to go. I don't expect it to run marathons, but it'd be nice to select from shuffle, trudge or walk, not to mention amplitude (again I think) would be really handy for crossing thresholds.
Controls: Uh, there were some really good suggestions made earlier, but I haven't considered them in depth. I'd say at least one controller dedicated to the legs. The polulu controller looks real good.
One thing you may want to try, to avoid getting bogged down with equations, is simply teaching the motions. Think of the legs like the technique of stop motion where you position each step by step. One step leads to the next. This is the technique used with many humanoid robots.
Thanks Humanoido. I can use the data from post #22 on this thread for that. The intent of generating an equation was a fluid movement like a human rather than a point to point digital movement like most robots. That'll get me started though!
Update: I finished the initial "Simulation" last night which led to the discovery of something I consider very important. The foot to calf angle needs to be 75 degrees rather than the 90 degrees I previously thought. Unfortunately the leg is already built, so I'm going to see about separating the glue joint to insert a wedge. The exact angle is probably determined by the center of mass, however on my human subject this is a 75 degree angle...... since I do not have a DOF to cover this angle, it must be fixed at hopefully an ideal compromise over standing stability and foot lift while walking.
Another thing I was surprised about, the foot of my human subject does not lift much at all during a walk...... maybe 1 - 2" at the most.
Another thing I was surprised about, the foot of my human subject does not lift much at all during a walk...... maybe 1 - 2" at the most.
Most biped bot walk very differently than real humans. Asimo (surely among the highest-tech) has a very peculiar gait. If a guy walked into a bar with that style walk, he'd be quickly thrown out on his Asimo.
Most biped bot walk very differently than real humans. Asimo (surely among the highest-tech) has a very peculiar gait. If a guy walked into a bar with that style walk, he'd be quickly thrown out on his Asimo.
ERCO! I'm flagging that as "Inappropriate". Sheesh! Where's a moderator? .......Asimo..... HA! HA!
Alright, I would consider the simulation finished. The gait looks fantastic. I don't have anything to make a GIF animation so I guess I can't share.
The gait consists of six steps (which happens to be how fast my camera takes pictures), starting with step "Zero" which doubles as a standing position. The left and right legs are offset by three positions, so both legs use the same set of numbers. I have a floor drawn. The foot (toe) never crosses the floor, neither drags across the floor. It walks just like you would expect.
Now, the next step is I need to find an efficient way to interpolate between each of the six steps to make the walk. If I do something like this:
READ thigh position, calf position, foot position
PULSOUT thigh position
PULSOUT calf position
PULSOUT foot position
PAUSE some value
GOTO start
the servos will travel at full speed until reaching their endpoints..... all at different times depending on how far each has to travel. What I need is a way to throttle each according to the distance needed to move.
INCREMENT THIGH = (End position - Start position) / some constant
INCREMENT CALF = ......... etc
......
thigh position = current position + increment thigh
calf position = current .......
........
PULSOUT thigh position
PULSOUT calf pos.....
.....
So that works, is very processor intensive and will probably exceed the 20ms loop time I have available once calculations are done for six servos and update six servos.
The alternative is more data.... more positions for each step of the gait, 12 positions? 48 positions? at 1 WORD per position I doubt this is possible without external EEPROM. Worst of all, a canned gait may not be very adjustable.
I'll look at the Polulu controllers to see what they offer. If I can setup endpoint and traverse speed, that'll likely be all I need.
Try several (nested?) FOR/NEXT/STEP loops to move your servos slowly. Makes life easy. That's how my arm moves, although just one axis at a time: http://www.youtube.com/watch?v=-Z8lTSX4PHs
Most biped bot walk very differently than real humans. Asimo (surely among the highest-tech) has a very peculiar gait. If a guy walked into a bar with that style walk, he'd be quickly thrown...
ASIMO has a very steady walking gate which is undoubtedly more than we can say about people who frequent bars.
Alright, I would consider the simulation finished. The gait looks fantastic.
Congratulations! Celebration is in order for this great "step." Consider going the final mile and attaching an upper body to get a full humanoid. Plez. Your skillful results will be greatly appreciated by others.
I'm afraid I don't have much of an update, I've been working quite a lot and the weekend..... same ol' sad story right?
Anyways, I've written several programs to interpolate between the six set points on the gait, however I'm not happy with it just yet. I did run through the data sheet on the Polulu serial controller and as soon as money permits I'll be ordering it. I figure I'll get the serial version since that works with the BS I know how to program. It should also work with the Prop although I think I'll need a level shift? I guess the USB version works directly with the Prop??? In any case, I don't know how to program the Prop yet, so that will have to wait.
Update Oct2611
I wrote an elegant and very fast routine to interpolate 128 positions between the six steps of the gait. Calculations are intended to be performed during the idle time left over from the servo updates.. I loaded it into my simulation last night to test.... it worked wonderfully...... in ONE direction only! -DUH! I guess I was too wrapped up in the details and failed to consider the overall function of the routine.
Yea, one direction, but it DOES throttle each servo depending on delta angle needed, so both joints get to the next position at the same time.
So it interpolates points in one direction of travel (back / increasing angle), but falls through giving jerky motion forward (decreasing angle).... Looks like I still have more work to do eh?
Don't waste your money on a Polulu servo controller. Your Prop can control 32 servos with one cog (you'll be hard pressed to do anything else with it since it would use all the IO pins).
If you're going to build robots the Prop is the perfect controller to use.
I'll promise to help with code (this goes for erco too).
Your robot is calling out for a Propeller. Can't you hear it? I sure can!
I used to do some computer animation. I used digital models of Lego minifigures and made them walk. I didn't want to use the "key frame" method so I wrote an algorithm to move their legs, arms, hands (a little) and heads to give them a reasonably natural gait (within their limited degrees of freedom). Of course I didn't have to worry about things like gravity.
I'm pretty sure all the equations in the gait were sinusoidal. I'll look for the equations if you want. I doubt they do you much good since they had fewer degrees of freedom (minifig legs only bend at the hip).
Anyway, unwrap the Prop and get coding. I promise you'll be glad you did or I'll pay for your next too years of Parallax forum membership fees.
You too erco! (Don't let one rude propellerhead keep you from enjoying the wonders of Propdom.) YRSUTTP!
It should also work with the Prop although I think I'll need a level shift? I guess the USB version works directly with the Prop???
The Prop needs a series resitor to receive a 5V data signal. To output data nothing is usually required. Most 5V devices will read the 3.3V from a Prop as high.
The Prop as an USB host is still in the experimental stage. The USB servo controller also has a UART interface (to use with a Stamp or Prop).
Okay, I'll soften my stand on the serial servo controllers. They could be useful but I still don't think you need them when you have a Prop.
I'll promise to help with code (this goes for erco too).
Your robot is calling out for a Propeller. Can't you hear it? I sure can!
.....................I'm pretty sure all the equations in the gait were sinusoidal. I'll look for the equations if you want..............
Duane
Yeeeeeea, you're tempting me man. I didn't want to crack open the Prop and get discouraged trying to cram something else into a day..... but then I just got laid off my second job, so tis' a possibility at the least.
I really want to use the Prop and your are surely right about Robots = Prop. It's intimidating at the least.
I didn't see the sinusoidal relationship even though I really hoped that's what it would work out to be, for simplicity if nothing else. If you run across the equations, I'd like to study it.
I'm working on my interpolation stuff again. I can use that on any micro..... unless it doesn't work like I think.... then it was a good exercise I guess.
I'll read up on the Prop to see what program / compiler / editor / etc I need to start. I have the....the.... Prop clip??? I think that's it. So I have the hardware to start anyways.
This routine works fairly well actually..... problem is, is the starting and ending angle are smaller than a nine count difference ( 8/16 = 0 in integer math ) the servo won't move... I guess in the scheme of things, an error of eight on a servo is very little. The program was written in Basic, but could easily be ported to Pbasic. all numbers are WORD sized (unsigned INTEGERs in Basic). Division would be replaced with bitshifts in Pbasic. I think the code is fast enough for the Basic Stamp and six servos. We'll see.
It works by taking advantage of the oddities of an unsigned number (WORD) which allows me to go without a ton of IF - THEN checks for each case of position relationships. I like it!
I'll read up on the Prop to see what program / compiler / editor / etc I need to start. I have the....the.... Prop clip??? I think that's it. So I have the hardware to start anyways.
There is a lot of information in the Propeller Tool's help menu. The Propeller Education Kit (in help menu) goes through the basics.
What Propeller board to you have? A Prop Clip is used to connect a Propeller board to the computer via USB.
Let me know what kind of board you have and I'll throw together a little demo of using multiple servos. It's just about 1 AM here so the demo will likely be written tomorrow.
I should be able to find those equations I use to get Lego men to walk. I'm pretty sure I've seen a printout of the code lately.
The program generated code for the raytracing program POVRay. One of the animations was of a small army of Lego minifigures all walking together. It looked pretty cool, if I do say so myself.
Let me know what kind of board you have and I'll throw together a little demo of using multiple servos. It's just about 1 AM here so the demo will likely be written tomorrow.
Duane
Whew! It's a comfort to know I'm not the only person awake and browsing the forums at oh-dark-thirty! ;-)
Hey man, late to bed, late to rise is my motto I wonder why I'm always late to work?
Duane, I did not purchase a development board for the Prop. I'm funny like that I guess. I have always used the IC in a socket on a breadboard, so I have the EEPROM, crystal, Prop clip and DIP package Prop with the fancy pin label sticker. I have a shelf unit full of resistors, caps, diodes, regulators, etc and anything I need for the build.
I downloaded a very nice open source program last night called CamStudio. I'm working to upload a screen video to youtube now.
... OK, I downloaded the Prop program, manual and anything else that looked interesting. I'll start reading over lunch today (I hope)
The video is uploaded at: http://www.youtube.com/watch?v=I1JcbdPcQXQ
Don't expect to be entertained, it's purely educational, no spiffy music, no jokes.... just a stick man walking with the exact code and routines destined for the BS or <gulp> Prop.
It's C code. I'm going to paste the aplicable parts into the code block below. The step is broken into four parts. Each of the four parts had two sections to control the torso movement. Our torso moves forward a back a little with each step. The frequency of the torso movement is twice the step frequency since minifigs use the same movement with each step (right and left).
int WalkStyleI(double frame, double stepPeriod, double maxForwardsArmTheta,
double maxBackwardsArmTheta, double rightHandMinimum, double rightHandMaximum,
double leftHandMinimum, double leftHandMaximum, double rightHeadMaximum,
double leftHeadMaximum, double torsoForwardsMaximum,
double torsoBackwardsMaximum, double minVisorTheta, double maxVisorTheta)
{
/* assembles minifig using minifig coordinates
Changed name from WalkStyleVerticalThroughStuds to WalkStyleI on 7/20/04
The origin of this coordinate system is the leg joint.*/
/*Updated 11/20/04*/
int count1, count2;
//char proceed[20];
/*Maximums are when the hand is in front of the minifig and minimums are when their arms
are moved back.*/
//I need to fix the visor?
struct Xyz partTheta, distanceFromPivot;// distance from leg joint
struct Xyz visorPivot = {0, 7, 0};//{0, 8.3, 0.3};//With respect to the visor part origin
double headTheta, torsoTheta, pseudoTime;// pseudoTime goes backwards sometimes
maxBackwardsLegTheta = 2 * toeAngle;
maxForwardsLegTheta = 2 * heelAngle; /*(in degrees) positive angles*/
//angles that don't change during the step, these could be local variables.
rightHandTheta = 0;/*positive inwardhands, head, helmet and visor aren't going to move in
this style of walk*/
leftHandTheta = 0;// positive outwards
headTheta = 0;
helmetTheta = 0;
visorTheta = -70;/* -70 is all the way up, 0 is down, this angle makes sense to use
negative numbers*/
torsoTheta = 0;
/*printf("start of WalkStyleI function.\n");
printf(
"frame = %f, stepPeriod = %f, maxForwardsArmTheta = %f, maxBackwardsArmTheta = %f\n",
frame, stepPeriod, maxForwardsArmTheta,
maxBackwardsArmTheta);
printf("maxBackwardsArmTheta = %f, rightHandMinimum = %f, rightHandMaximum = %f\n",
maxBackwardsArmTheta, rightHandMinimum, rightHandMaximum);
printf("leftHandMinimum = %f, leftHandMaximum = %f, rightHeadMaximum = %f\n",
leftHandMinimum, leftHandMaximum, rightHeadMaximum);
printf("leftHeadMaximum = %f, torsoForwardsMaximum = %f, torsoBackwardsMaximum = %f\n",
leftHeadMaximum, torsoForwardsMaximum,
torsoBackwardsMaximum);
printf("minVisorTheta = %f, maxVisorTheta = %f\n",
minVisorTheta, maxVisorTheta);
printf ("enter any character to contine program\n");
scanf ("%s", proceed);*/
ResetMatrices();
if (frame / stepPeriod < .25)
/* First forth of the step right leg is forward and left arm is forward (both moving
back) pseudoTime is always 0 to stepPeriod/4. Rotation on right heel. Torso moving
forward.*/
{
pseudoTime = frame;
rightLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod)));
leftLegTheta = maxBackwardsLegTheta - ((16 * maxBackwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
rightArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
leftArmTheta = -1 * (maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod)));
rightHandTheta = rightHandMinimum + (rightHandMaximum - rightHandMinimum) * 2 * frame
/ stepPeriod;
leftHandTheta = leftHandMaximum - (leftHandMaximum - leftHandMinimum) * 2 * frame
/ stepPeriod;
headTheta = leftHeadMaximum + (rightHeadMaximum - leftHeadMaximum) * 2 * frame
/ stepPeriod;
visorTheta = minVisorTheta + (maxVisorTheta - minVisorTheta) * 2 * frame / stepPeriod;
if ( frame / stepPeriod < (1.0 / 8.0))
{
torsoTheta = (torsoForwardsMaximum + torsoBackwardsMaximum)/2 -
(torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * frame / stepPeriod;
}
else
{
torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum -
torsoBackwardsMaximum) * 4 * (frame - stepPeriod / 8.0) / stepPeriod;
}
/*printf ("leftHandTheta = %f\n", leftHandTheta);
printf ("enter any character to contine (end)program\n");
scanf ("%s", debugProceed);
exit(0);*/
}
else if (frame / stepPeriod < .5)
/* Second forth of the step right leg is back and left arm is back (both still moving
back) left foot is forward moving forward. Rotation on right toe. Lean should start in
middle between extremes
and be moving back.*/
{
pseudoTime = .5 * stepPeriod - frame;
rightLegTheta = maxBackwardsLegTheta - ((16 * maxBackwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
leftLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod)));
rightArmTheta = -1 *(maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime *
pseudoTime)/ (stepPeriod * stepPeriod)));
leftArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
rightHandTheta = rightHandMinimum + (rightHandMaximum - rightHandMinimum) * 2 *
frame / stepPeriod;
leftHandTheta = leftHandMaximum - (leftHandMaximum - leftHandMinimum) * 2 * frame /
stepPeriod;//in degrees
headTheta = leftHeadMaximum + (rightHeadMaximum - leftHeadMaximum) * 2 * frame /
stepPeriod;
visorTheta = minVisorTheta + (maxVisorTheta - minVisorTheta) * 2 * frame / stepPeriod;
if (frame / stepPeriod < (3.0 / 8.0))
{
torsoTheta = torsoBackwardsMaximum +
(torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - stepPeriod /
8.0) / stepPeriod;
}
else
{
torsoTheta = torsoForwardsMaximum - (torsoForwardsMaximum -
torsoBackwardsMaximum) * 4 * (frame - 3.0 * stepPeriod / 8.0) / stepPeriod;
}
}
else if (frame / stepPeriod < .75)
/* Third forth of the step right leg and left arm are back but moving forward.
Rotation on left heel. Left leg forward */
{
pseudoTime = frame - .5 * stepPeriod;
rightLegTheta = maxBackwardsLegTheta -((16 * maxBackwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
leftLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod)));
rightArmTheta = -1 * (maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod)));
leftArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
rightHandTheta = rightHandMaximum - (rightHandMaximum - rightHandMinimum) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
leftHandTheta = leftHandMinimum + (leftHandMaximum - leftHandMinimum) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
headTheta = rightHeadMaximum - (rightHeadMaximum - leftHeadMaximum) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
visorTheta = maxVisorTheta - (maxVisorTheta - minVisorTheta) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
if (frame / stepPeriod < (5.0 / 8.0))
{
torsoTheta = torsoForwardsMaximum - (torsoForwardsMaximum -
torsoBackwardsMaximum) * 4 * (frame - 3.0 * stepPeriod / 8.0) / stepPeriod;
}
else
{
torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum -
torsoBackwardsMaximum) * 4 * (frame - 5.0 * stepPeriod / 8.0) / stepPeriod;
}
}
else
/* Forth forth of the step right leg and left arm are forward and moving forward.
Rotation of left toe. Left leg back */
{
pseudoTime = stepPeriod - frame;
rightLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod)));
leftLegTheta = maxBackwardsLegTheta - ((16 * maxBackwardsLegTheta * pseudoTime *
pseudoTime) / (stepPeriod * stepPeriod));
rightArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime *
pseudoTime)/ (stepPeriod * stepPeriod));
leftArmTheta = -1 * (maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime *
pseudoTime)/ (stepPeriod * stepPeriod)));
rightHandTheta = rightHandMaximum - (rightHandMaximum - rightHandMinimum) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
leftHandTheta = leftHandMinimum + (leftHandMaximum - leftHandMinimum) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
headTheta = rightHeadMaximum - (rightHeadMaximum - leftHeadMaximum) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
visorTheta = maxVisorTheta - (maxVisorTheta - minVisorTheta) * 2 *
(frame - .5 * stepPeriod) / stepPeriod;
if (frame / stepPeriod < (7.0 / 8.0))
{
torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum -
torsoBackwardsMaximum) * 4 * (frame - 5.0 * stepPeriod / 8.0) / stepPeriod;
}
else
{
torsoTheta = torsoForwardsMaximum - (torsoForwardsMaximum -
torsoBackwardsMaximum) * 4 * (frame - 7.0 * stepPeriod / 8.0) / stepPeriod;
}
}
I'll try to find the values used in the code.
static double toeAngle = 21.4477363271, heelAngle = 17.8188889146;
//toeAngle = atan(11/28)*degreeConversion, heelAngle = atan(9/28)*degreeConversion;//in degrees
static double rToe = 30.083217913, rHeel = 29.4108823397;
// distance from leg joint to heel and toe, rToe = sqrt(905), rHeel= sqrt(865)
static double shoulderAngle = 9.782
struct WalkingParameter //used for each minifig
{
/*Updated 11/9/04 added turnSlowdownFactor*/
//The first parameters are used in WalkStyleI function.
//double maxBackwardsLegTheta;//default 2*toeAngle
//double maxForwardsLegTheta;//default 2*heelAngle /*(in degrees) positive angles*/
double stepPeriod;//default 40 (This looks good when rendered at 30 frames per second.)
double frame;
double maxForwardsArmTheta;// default heelAngle
double maxBackwardsArmTheta;//default toeAngle
double rightHandMaximum;// default 45
double rightHandMinimum;// default -15
double leftHandMaximum;// default -45
double leftHandMinimum;// default 15
/*Maximums are when the hand is in front of the minifig and minimums are when their arms
are moved back.*/
double leftHeadMaximum; //default -10
double rightHeadMaximum; //default 10
double MinVisorTheta;//(zero = closed, -70 = open)default -70
double MaxVisorTheta;
double torsoForwardsMaximum; //default -10
double torsoBackwardsMaximum;/*default 20, when a leg is forward minifig should lean back
this far.*/
//The following are used in the ChangeMinifigCoordingatesToStepCoordinatesBouce function.
double bounce;//default 100
struct Xyz maxZLean;//default { 0, 0, 15};Just side to side lean for now.
/*The minifig will lean side to side and front to back from this point by the maxZLean
amount.*/
//The following are used in the RotateStepCoordinates function.
double turnSlowdownFactor;//default 1.5
//The following are used in the ChangeStepCoordinatesToWorldCoordinates function.
//The following are used in the main function.
struct Xyz walkAngle;
struct Xyz stepCenter;
struct Xyz startNorth;
struct Xyz startWest;
struct Xyz startSouth;
struct Xyz startEast;
double turnYAngle;
double turnIncrementTheta;
};
static struct Xyz handArmDifference = { 5.202, 18.752, -10.321};
struct Xyz visor, helmet, head, torso, rightArm, rightHand, leftArm, leftHand;
struct Xyz airtanks, rightLeg, leftLeg, waist, legJoint;
struct Xyz cameraLocation, light1, light2, light3, cameraLookAt, lightPointAt;
static double wristAngle = -28.8281500231;
// atan(handArmDifference.z/handArmDifference.y) * degreeConversion
static double armpitTheta = 15.5045682803; /* make negative for right hand*/
// atan(handArmDifference.x / handArmDifference.y) * degreeConversion
There's also some code that moves the minifig with respect to the ground. Since minifigs don't have ankles, I pivoted them on their toes and heels to give them a more natural looking walk. I'll post this additional code once I get my internet working.
My internet is on the fritz so attach files to my posts right now.
I also can't upload (or download) video. I have several videos of these minifigs (vertual) walking.
I rounded up some servos so I can write a little test program to show you (and erco if he's watching) how to add servos to a Prop project.
Darn it. I wont be able to attach the Spin files. I think I've going to try to get my internet working before writting the servo code.
I have a new DSL modem but I have to call tech support to configure it properly.
Wow, thanks Duane. I'll pull the code off to dissect it. It's been a couple years since I've used C, but it shouldn't be a problem.
I dunno how far back you've read, but my legs will not have ankles either (in the normal sense) so this code may be more applicable than I though. I do have a left/right pivot on the foot to transfer the center of mass.
Okay, I haven't fixed my internet problem yet but working on some servo code was more fun than crimping RJ-45 connectors onto CAT5e cable.
Here's some code for oscillating 16 servos. Some of the servos oscillate at twice the frequency of the others.
DAT fileName byte "Servo111027h" ' I like to have the name of the program
' displayed in a terminal window.
CON{{
111027a start working on a sevo demo.
27d modify equations.
27e keep servo #15 centered.
27f increase _FramesPerPeriod (slow down servos).
27g add displayPosition array so frame numbers match position
values.
27h fix byte sized frame problem.
}}
CON
_Clkmode = xtal1 + pll16x
_Xinfreq = 5_000_000
_DebugBaud = 9600
_MaxPulse = 2000
_MinPulse = 1000
_Stop = 1500
_Center = _Stop
_ServosInUse = 16
_FramesPerPeriod = 300
_PositionsPerRow = 4
VAR
long time, timeOld, frameInterval, debugStack[100]
word servoPosition[_ServosInUse], maxPosition[_ServosInUse], minPosition[_ServosInUse]
word displayPosition[_ServosInUse], frameCount, frameCountOld
byte servoPin[_ServosInUse]
OBJ
Debug : "Simple_Serial"
Servo : "Servo32v7"
PUB Setup | localIndex
repeat localIndex from 0 to _ServosInUse - 1 ' Set pin numbers. They don't need to be consecutive.
servoPosition[localIndex] := _Center ' You could set each pin individually.
servoPin[localIndex] := localIndex
maxPosition[localIndex] := _MaxPulse ' Set all the Max and Mins the same for now.
minPosition[localIndex] := _MinPulse
maxPosition[0] := _MaxPulse - 100 ' Change some of the Max and Mins.
minPosition[0] := _MinPulse + 100
minPosition[1] := _MinPulse + 150
maxPosition[2] := _MaxPulse - 150
maxPosition[3] := _MaxPulse - 200
minPosition[3] := _MinPulse + 200
frameInterval := clkfreq / 50
frameCount := 0
cognew(DebugLoop, @debugStack) ' starts a cog 6 left
waitcnt(clkfreq / 20 + cnt)
Servo.Start ' starts a cog 5 left
waitcnt(clkfreq / 20 + cnt)
'Servo.Ramp '<-OPTIONAL 'Start Background Ramping ' starts a cog not used so still 5 cogs left
MainLoop
PUB MainLoop | localIndex, pseudoPercent, nextFrameTime
'' This method is being executed in cog #0.
nextFrameTime := cnt + frameInterval
repeat
waitcnt(nextFrameTime)
nextFrameTime += frameInterval
pseudoPercent := ((frameCount * 100) / _FramesPerPeriod ) ' How far into the cycle are we?
if pseudoPercent < 50
repeat localIndex from 12 to 14
servoPosition[localIndex] := minPosition[localIndex] + (2 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 8 to 11
servoPosition[localIndex] := maxPosition[localIndex] - (2 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100
if pseudoPercent < 25
repeat localIndex from 0 to 4
servoPosition[localIndex] := minPosition[localIndex] + (4 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 5 to 7
servoPosition[localIndex] := maxPosition[localIndex] - (4 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100
else
repeat localIndex from 5 to 7
servoPosition[localIndex] := minPosition[localIndex] + (4 * (pseudoPercent - 25) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 0 to 4
servoPosition[localIndex] := maxPosition[localIndex] - (4 * (pseudoPercent - 25) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
else
repeat localIndex from 8 to 11
servoPosition[localIndex] := minPosition[localIndex] + (2 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 12 to 14
servoPosition[localIndex] := maxPosition[localIndex] - (2 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
if pseudoPercent < 75
repeat localIndex from 0 to 4
servoPosition[localIndex] := minPosition[localIndex] + (4 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 5 to 7
servoPosition[localIndex] := maxPosition[localIndex] - (4 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
else
repeat localIndex from 5 to 7
servoPosition[localIndex] := minPosition[localIndex] + (4 * (pseudoPercent - 75) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 0 to 4
servoPosition[localIndex] := maxPosition[localIndex] - (4 * (pseudoPercent - 75) * (maxPosition[localIndex] - minPosition[localIndex])) / 100
repeat localIndex from 0 to _ServosInUse - 1
Servo.Set(servoPin[localIndex], servoPosition[localIndex])
if frameCount == _FramesPerPeriod - 1
frameCount := 0
else
frameCount++
PUB DebugLoop | localIndex
'' We'll use a separate loop to send debug information
'' so it doesn't slow down the servo calculations.
'' It is likely that not all frames will be displayed
'' since the data can't be transmitted fast enough to
'' keep up with how quickly the seroPosition data is
'' changed.
Debug.init(31, 30, _DebugBaud)
Debug.str(string(13, "Debug Started "))
Debug.str(string(13, "fileName = "))
Debug.str(@fileName)
Debug.tx(13)
frameCountOld := _FramesPerPeriod + 1 ' not a legitamate frame so frameCount is sure not to be equal to it.
repeat
repeat while frameCount == frameCountOld
frameCountOld := frameCount
wordmove(@displayPosition, @servoPosition, _ServosInUse)
' move the servoPosition values so they don't change while they
' are being displayed.
Debug.str(string(13, 13, "Frame # "))
Dec(frameCountOld)
Debug.str(string(" Servo Positions"))
'Debug.tx(13)
repeat localIndex from 0 to _ServosInUse - 1
if (localIndex // _PositionsPerRow) == 0
Debug.tx(13)
Debug.str(string("SP["))
else
Debug.str(string(", SP["))
Dec(localIndex)
Debug.str(string("] = "))
Dec(displayPosition[localIndex])
PUB Dec(value) | i, x
'' Since Simple_Serial doesn't have a dec method.
'' Copied from FullDuplexSerial
'' Print a decimal number
x := value == NEGX 'Check for max negative
if value < 0
value := ||(value+x) 'If negative, make positive; adjust for max negative
Debug.tx("-") 'and output sign
i := 1_000_000_000 'Initialize divisor
repeat 10 'Loop for 10 digits
if value => i
Debug.tx(value / i + "0" + x*(i == 1)) 'If non-zero digit, output digit; adjust for max negative
value //= i 'and digit from value
result~~ 'flag non-zero found
elseif result or i == 1
Debug.tx("0") 'If zero digit (or only digit) output it
i /= 10 'Update divisor
That can't be scarier than PBASIC code to oscillate 16 servos. Right?
The oscillations in this code was linear. The code I posted early to make the minifig walk was different than this code. I believe the minfig code used the equations from an oscillating pendulum (hence sinusoidal).
I didn't try to translate the pendulum oscillation equations into Spin yet. I just wanted to give an example of moving multiple servos with a Propeller. While the code produces signals for 16 servos, I only connected 8 servos. Still 8 servos oscillating back and forth is still fun.
Servo #15 doesn't oscillate. It's set to the center position. I used this to adjust a couple of continuous rotations servos I had.
Hopefully this large post will make through my bad internet connection.
Apparently you have also infected my internet connection at home. Lucky for me I had downloaded the Prop stuff at work, I so read the manual through last night (I did not read the 200pg of code instructions )
I took quite a lot of notes and questions I need to find the answers for. The Prop is quite a bit different than anything I've programmed so far, but we'll manage I hope.
WHOA! Duane, you da man! I can actually halfway (now) read through and understand what your code does. Thank you sir. The nicest thing about the Prop over the BS for this particular project is the internal clock.
I have no doubt the Prop will handle all I want to do for the biped. I also have no doubt you've single handedly postponed it's completion until I can get my mind wrapped around the Prop. <sigh> all for the best though and it's a good excuse to finally open the package of Prop stuff..
Feel free to ask about any code you don't understand.
I tried to edit my post with code that didn't wrap around but it was too much for my internect connection to handle.
The method "Dec" is taken from the object (program) FullDuplexSerial. There are a couple of lines in the Dec method I don't understand. But the rest of the program I should be able to explain.
I'm basically treating the robot as an animated object. Instead of 30 frames per second I use 50 frames per second since that's the fastest many servos can be updated.
As part of the "Setup" method I add the line "frameInterval := clkfreq / 50". frameInterval then holds the number of clock cycles in one 50th of a second (or 20 ms).
Before the repeat statement in "MainLoop" is the line "nextFrameTime := cnt + frameInterval". This sets nextFrameTime equal to the current time (held in cnt) plus 20ms. This will be the time the first loop will be executed.
The command in "MainLoop" that causes the Prop to wait for the next 20 ms interval is "waitcnt(nextFrameTime)". Once this next time interval is reached the loop continues. I imediately add another 20 ms to "nextFrameTime" with the statement "nextFrameTime += frameInterval". This is a short hand for "nextFrameTime := nextFrameTime + frameInterval". This sets up the "nextFrameTime" so it will have the value we want it to have at the beginning of the next loop.
This adding 20 ms to "nextFrameTime" keeps the loop syncronized to every 20 ms. You would have a problem if the loop took longer than 20 ms. If the loop took longer than 20 ms, "waitcnt(nextFrameTime)" would wait for the system clock to roller over (this takes about 53 seconds @ 80MHz).
Let me know if there's a portion of the code you'd like be to explain.
Comments
You're doing good and on the right track. The code to drive the gyros is all in BASIC so there's no learning curve in going from a review of PBASIC to RN's BASIC. The brain and heart of the ROBONOVA-1 is the ATMEL ATMega 128 processor (as a reference only), its internal memory supplemented by a 64K*8 EEPROM as storage for Robo-Script and Robo-Basic programs. It about time we develop these sensors for Stamps and Propellers.
http://www.robotadvice.com/hitec-robonova-1_robot.html
Parallax has a gyro you should look at, complete with code for the BASIC Stamp.
http://www.parallax.com/Store/Sensors/AllSensors/tabid/760/CategoryID/46/List/0/SortField/0/catpageindex/3/Level/a/ProductID/588/Default.aspx
The LISY300 Gyroscope Module is a single-axis yaw rate sensor providing up to 300°/s full scale rotation detection at up to 88 Hz. Useful in balancing robots or auto-pilot systems, the LISY300 Gyroscope Module can detect how many degrees it has turned on its planar axis allowing the host microcontroller to stabilize the platform or correct for drift.
' ========================================================================= ' ' File...... LISY300 Test V1.0.bs2 ' Purpose... Test LISY300 Gyroscope Module ' Author.... Parallax, Inc. ' E-mail.... support@parallax.com ' Started... 11-15-2009 ' Updated... ' ' {$STAMP BS2} ' {$PBASIC 2.5} ' ' ========================================================================= ' -----[ Program Description ]--------------------------------------------- ' This program test the LISY300 Gyroscope Module (#27922) by returning the ' ADC value. The ADC is 10 bits and returns a value from 0 - 1023 at max ' ranges. Typically when the Gyroscope Module is sitting idle the return ' value should be close to 512 (+/- 24). As the Gyroscope rotates in a ' clockwise manner the values go down relative to the speed at which the ' gyroscope is rotating. Conversely when the gyroscope is rotating in a ' counter-clockwise manner the values go up relative to the speed at which ' the gyroscope is rotating. ' The sample code uses a 250uS pause to reduce display jitter, however when ' reading the ADC in an application you would not have this delay. Please ' read the documentation to determine how to make use of the data for ' calculating yaw. ' -----[ Revision History ]------------------------------------------------ ' -----[ I/O Definitions ]------------------------------------------------- Dout PIN 0 ' P0 <-- Dout (LISY300.2) SCLK PIN 1 ' P1 --> SCLK (LISY300.4) CSn PIN 2 ' P2 --> /CS (LISY300.5) ' -----[ Constants ]------------------------------------------------------- Yes CON 1 ' Yes Constant No CON 0 ' No Constant ' -----[ Variables ]------------------------------------------------------- value VAR Word ' ADC Result Value ' -----[ EEPROM Data ]----------------------------------------------------- ' -----[ Initialization ]-------------------------------------------------- Initialization: HIGH CSn LOW SCLK PAUSE 250 ' -----[ Program Code ]---------------------------------------------------- Main: DO GOSUB Read_Gyro DEBUG HOME, "ADC Value:", DEC5 value PAUSE 250 LOOP STOP ' -----[ Subroutines ]----------------------------------------------------- Read_Gyro: LOW CSn SHIFTIN Dout, SCLK, MSBPOST, [value\13] HIGH CSn RETURNI'm not aware of any Parallax projects with up to 12 Stamp microcontroller modules for maintaining complete accuracy through redundancy. For this hobby leg application, I wouldn't worry about it. When using Stamps, it's more of a matter of getting multiple things to run simultaneously with multiple modules. Erco's idea is a good one that's more suited to Propellers which already have eight processors inside. Each prop could be redundant unto others - with multiple Cogs doing the cross checking.
I didn't get to work on this as much as I'd like over the weekend due to work, but I did spend a couple hours in data ==> equation matching. The best so far is y = 0.25ABS(x+3.4)+2 + COS(Pi * (a * xx + 7) / 3) + COS(Pi * xx) + COS(Pi * (2 * xx + 5) / 3) * COS(Pi * (2 * xx + 7) / 3). and fits pretty well for the hip. This equation thing is a pretty difficult task actually. I think no one commented on the idea because I didn't explain myself clearly, or everyone knew the idea was bogus.
I'm not sure how well this data based function will work, maybe I'm not smart enough for the pattern matching. I have a backup plan if that doesn't work....... or I could research and steal someone else's working code
I downloaded a program that was supposed to take data points and spit out a matching function... It worked.....technically..... but the function is useless for a motion profile.
At the moment, I'm doing some programming on the PC to work out my code, steps and a graphics simulation.... ok, ok, my "Simulation" is a set of moving lines to indicate thigh, calf and foot for each leg.... but just something to see it move
Programming details: I really think a routine based on a time constant and amplitude (for lack of a better word) is the way to go. I don't expect it to run marathons, but it'd be nice to select from shuffle, trudge or walk, not to mention amplitude (again I think) would be really handy for crossing thresholds.
Controls: Uh, there were some really good suggestions made earlier, but I haven't considered them in depth. I'd say at least one controller dedicated to the legs. The polulu controller looks real good.
Update: I finished the initial "Simulation" last night which led to the discovery of something I consider very important. The foot to calf angle needs to be 75 degrees rather than the 90 degrees I previously thought. Unfortunately the leg is already built, so I'm going to see about separating the glue joint to insert a wedge. The exact angle is probably determined by the center of mass, however on my human subject this is a 75 degree angle...... since I do not have a DOF to cover this angle, it must be fixed at hopefully an ideal compromise over standing stability and foot lift while walking.
Another thing I was surprised about, the foot of my human subject does not lift much at all during a walk...... maybe 1 - 2" at the most.
Most biped bot walk very differently than real humans. Asimo (surely among the highest-tech) has a very peculiar gait. If a guy walked into a bar with that style walk, he'd be quickly thrown out on his Asimo.
ERCO! I'm flagging that as "Inappropriate". Sheesh! Where's a moderator? .......Asimo..... HA! HA!
You say tomato...
The gait consists of six steps (which happens to be how fast my camera takes pictures), starting with step "Zero" which doubles as a standing position. The left and right legs are offset by three positions, so both legs use the same set of numbers. I have a floor drawn. The foot (toe) never crosses the floor, neither drags across the floor. It walks just like you would expect.
Now, the next step is I need to find an efficient way to interpolate between each of the six steps to make the walk. If I do something like this:
READ thigh position, calf position, foot position
PULSOUT thigh position
PULSOUT calf position
PULSOUT foot position
PAUSE some value
GOTO start
the servos will travel at full speed until reaching their endpoints..... all at different times depending on how far each has to travel. What I need is a way to throttle each according to the distance needed to move.
INCREMENT THIGH = (End position - Start position) / some constant
INCREMENT CALF = ......... etc
......
thigh position = current position + increment thigh
calf position = current .......
........
PULSOUT thigh position
PULSOUT calf pos.....
.....
So that works, is very processor intensive and will probably exceed the 20ms loop time I have available once calculations are done for six servos and update six servos.
The alternative is more data.... more positions for each step of the gait, 12 positions? 48 positions? at 1 WORD per position I doubt this is possible without external EEPROM. Worst of all, a canned gait may not be very adjustable.
I'll look at the Polulu controllers to see what they offer. If I can setup endpoint and traverse speed, that'll likely be all I need.
I'm afraid I don't have much of an update, I've been working quite a lot and the weekend..... same ol' sad story right?
Anyways, I've written several programs to interpolate between the six set points on the gait, however I'm not happy with it just yet. I did run through the data sheet on the Polulu serial controller and as soon as money permits I'll be ordering it. I figure I'll get the serial version since that works with the BS I know how to program. It should also work with the Prop although I think I'll need a level shift? I guess the USB version works directly with the Prop??? In any case, I don't know how to program the Prop yet, so that will have to wait.
I wrote an elegant and very fast routine to interpolate 128 positions between the six steps of the gait. Calculations are intended to be performed during the idle time left over from the servo updates.. I loaded it into my simulation last night to test.... it worked wonderfully...... in ONE direction only! -DUH! I guess I was too wrapped up in the details and failed to consider the overall function of the routine.
Yea, one direction, but it DOES throttle each servo depending on delta angle needed, so both joints get to the next position at the same time.
So it interpolates points in one direction of travel (back / increasing angle), but falls through giving jerky motion forward (decreasing angle).... Looks like I still have more work to do eh?
Should I have admitted that?
Don't waste your money on a Polulu servo controller. Your Prop can control 32 servos with one cog (you'll be hard pressed to do anything else with it since it would use all the IO pins).
If you're going to build robots the Prop is the perfect controller to use.
I'll promise to help with code (this goes for erco too).
Your robot is calling out for a Propeller. Can't you hear it? I sure can!
I used to do some computer animation. I used digital models of Lego minifigures and made them walk. I didn't want to use the "key frame" method so I wrote an algorithm to move their legs, arms, hands (a little) and heads to give them a reasonably natural gait (within their limited degrees of freedom). Of course I didn't have to worry about things like gravity.
I'm pretty sure all the equations in the gait were sinusoidal. I'll look for the equations if you want. I doubt they do you much good since they had fewer degrees of freedom (minifig legs only bend at the hip).
Anyway, unwrap the Prop and get coding. I promise you'll be glad you did or I'll pay for your next too years of Parallax forum membership fees.
You too erco! (Don't let one rude propellerhead keep you from enjoying the wonders of Propdom.) YRSUTTP!
Duane
The Prop needs a series resitor to receive a 5V data signal. To output data nothing is usually required. Most 5V devices will read the 3.3V from a Prop as high.
The Prop as an USB host is still in the experimental stage. The USB servo controller also has a UART interface (to use with a Stamp or Prop).
Okay, I'll soften my stand on the serial servo controllers. They could be useful but I still don't think you need them when you have a Prop.
Duane
dmmadd!
Yeeeeeea, you're tempting me man. I didn't want to crack open the Prop and get discouraged trying to cram something else into a day..... but then I just got laid off my second job, so tis' a possibility at the least.
I really want to use the Prop and your are surely right about Robots = Prop. It's intimidating at the least.
I didn't see the sinusoidal relationship even though I really hoped that's what it would work out to be, for simplicity if nothing else. If you run across the equations, I'd like to study it.
I'm working on my interpolation stuff again. I can use that on any micro..... unless it doesn't work like I think.... then it was a good exercise I guess.
I'll read up on the Prop to see what program / compiler / editor / etc I need to start. I have the....the.... Prop clip??? I think that's it. So I have the hardware to start anyways.
It works by taking advantage of the oddities of an unsigned number (WORD) which allows me to go without a ton of IF - THEN checks for each case of position relationships. I like it!
pos1 = 1500 pos1a = 750 pos2 = 750 pos2a = 1500 inc1 = (pos1a - pos1) / 16 inc2 = (pos2a - pos2) / 16 PRINT "Start1="; pos1, "End1="; pos1a, "Increment="; inc1 PRINT "Start2="; pos2, "End2="; pos2a, "Increment="; inc2 PRINT FOR i = 0 TO 16 _LIMIT 5 PRINT pos1, pos2 pos1 = pos1 + inc1 pos2 = pos2 + inc2 NEXT iYou'll want to download the Propeller Tool v.13 from the Propeller download page.
There is a lot of information in the Propeller Tool's help menu. The Propeller Education Kit (in help menu) goes through the basics.
What Propeller board to you have? A Prop Clip is used to connect a Propeller board to the computer via USB.
Let me know what kind of board you have and I'll throw together a little demo of using multiple servos. It's just about 1 AM here so the demo will likely be written tomorrow.
I should be able to find those equations I use to get Lego men to walk. I'm pretty sure I've seen a printout of the code lately.
The program generated code for the raytracing program POVRay. One of the animations was of a small army of Lego minifigures all walking together. It looked pretty cool, if I do say so myself.
Duane
Whew! It's a comfort to know I'm not the only person awake and browsing the forums at oh-dark-thirty! ;-)
Amanda
Duane, I did not purchase a development board for the Prop. I'm funny like that I guess. I have always used the IC in a socket on a breadboard, so I have the EEPROM, crystal, Prop clip and DIP package Prop with the fancy pin label sticker. I have a shelf unit full of resistors, caps, diodes, regulators, etc and anything I need for the build.
I downloaded a very nice open source program last night called CamStudio. I'm working to upload a screen video to youtube now.
... OK, I downloaded the Prop program, manual and anything else that looked interesting. I'll start reading over lunch today (I hope)
The video is uploaded at: http://www.youtube.com/watch?v=I1JcbdPcQXQ
Don't expect to be entertained, it's purely educational, no spiffy music, no jokes.... just a stick man walking with the exact code and routines destined for the BS or <gulp> Prop.
It's C code. I'm going to paste the aplicable parts into the code block below. The step is broken into four parts. Each of the four parts had two sections to control the torso movement. Our torso moves forward a back a little with each step. The frequency of the torso movement is twice the step frequency since minifigs use the same movement with each step (right and left).
int WalkStyleI(double frame, double stepPeriod, double maxForwardsArmTheta, double maxBackwardsArmTheta, double rightHandMinimum, double rightHandMaximum, double leftHandMinimum, double leftHandMaximum, double rightHeadMaximum, double leftHeadMaximum, double torsoForwardsMaximum, double torsoBackwardsMaximum, double minVisorTheta, double maxVisorTheta) { /* assembles minifig using minifig coordinates Changed name from WalkStyleVerticalThroughStuds to WalkStyleI on 7/20/04 The origin of this coordinate system is the leg joint.*/ /*Updated 11/20/04*/ int count1, count2; //char proceed[20]; /*Maximums are when the hand is in front of the minifig and minimums are when their arms are moved back.*/ //I need to fix the visor? struct Xyz partTheta, distanceFromPivot;// distance from leg joint struct Xyz visorPivot = {0, 7, 0};//{0, 8.3, 0.3};//With respect to the visor part origin double headTheta, torsoTheta, pseudoTime;// pseudoTime goes backwards sometimes maxBackwardsLegTheta = 2 * toeAngle; maxForwardsLegTheta = 2 * heelAngle; /*(in degrees) positive angles*/ //angles that don't change during the step, these could be local variables. rightHandTheta = 0;/*positive inwardhands, head, helmet and visor aren't going to move in this style of walk*/ leftHandTheta = 0;// positive outwards headTheta = 0; helmetTheta = 0; visorTheta = -70;/* -70 is all the way up, 0 is down, this angle makes sense to use negative numbers*/ torsoTheta = 0; /*printf("start of WalkStyleI function.\n"); printf( "frame = %f, stepPeriod = %f, maxForwardsArmTheta = %f, maxBackwardsArmTheta = %f\n", frame, stepPeriod, maxForwardsArmTheta, maxBackwardsArmTheta); printf("maxBackwardsArmTheta = %f, rightHandMinimum = %f, rightHandMaximum = %f\n", maxBackwardsArmTheta, rightHandMinimum, rightHandMaximum); printf("leftHandMinimum = %f, leftHandMaximum = %f, rightHeadMaximum = %f\n", leftHandMinimum, leftHandMaximum, rightHeadMaximum); printf("leftHeadMaximum = %f, torsoForwardsMaximum = %f, torsoBackwardsMaximum = %f\n", leftHeadMaximum, torsoForwardsMaximum, torsoBackwardsMaximum); printf("minVisorTheta = %f, maxVisorTheta = %f\n", minVisorTheta, maxVisorTheta); printf ("enter any character to contine program\n"); scanf ("%s", proceed);*/ ResetMatrices(); if (frame / stepPeriod < .25) /* First forth of the step right leg is forward and left arm is forward (both moving back) pseudoTime is always 0 to stepPeriod/4. Rotation on right heel. Torso moving forward.*/ { pseudoTime = frame; rightLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod))); leftLegTheta = maxBackwardsLegTheta - ((16 * maxBackwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); rightArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); leftArmTheta = -1 * (maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod))); rightHandTheta = rightHandMinimum + (rightHandMaximum - rightHandMinimum) * 2 * frame / stepPeriod; leftHandTheta = leftHandMaximum - (leftHandMaximum - leftHandMinimum) * 2 * frame / stepPeriod; headTheta = leftHeadMaximum + (rightHeadMaximum - leftHeadMaximum) * 2 * frame / stepPeriod; visorTheta = minVisorTheta + (maxVisorTheta - minVisorTheta) * 2 * frame / stepPeriod; if ( frame / stepPeriod < (1.0 / 8.0)) { torsoTheta = (torsoForwardsMaximum + torsoBackwardsMaximum)/2 - (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * frame / stepPeriod; } else { torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - stepPeriod / 8.0) / stepPeriod; } /*printf ("leftHandTheta = %f\n", leftHandTheta); printf ("enter any character to contine (end)program\n"); scanf ("%s", debugProceed); exit(0);*/ } else if (frame / stepPeriod < .5) /* Second forth of the step right leg is back and left arm is back (both still moving back) left foot is forward moving forward. Rotation on right toe. Lean should start in middle between extremes and be moving back.*/ { pseudoTime = .5 * stepPeriod - frame; rightLegTheta = maxBackwardsLegTheta - ((16 * maxBackwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); leftLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod))); rightArmTheta = -1 *(maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime * pseudoTime)/ (stepPeriod * stepPeriod))); leftArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); rightHandTheta = rightHandMinimum + (rightHandMaximum - rightHandMinimum) * 2 * frame / stepPeriod; leftHandTheta = leftHandMaximum - (leftHandMaximum - leftHandMinimum) * 2 * frame / stepPeriod;//in degrees headTheta = leftHeadMaximum + (rightHeadMaximum - leftHeadMaximum) * 2 * frame / stepPeriod; visorTheta = minVisorTheta + (maxVisorTheta - minVisorTheta) * 2 * frame / stepPeriod; if (frame / stepPeriod < (3.0 / 8.0)) { torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - stepPeriod / 8.0) / stepPeriod; } else { torsoTheta = torsoForwardsMaximum - (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - 3.0 * stepPeriod / 8.0) / stepPeriod; } } else if (frame / stepPeriod < .75) /* Third forth of the step right leg and left arm are back but moving forward. Rotation on left heel. Left leg forward */ { pseudoTime = frame - .5 * stepPeriod; rightLegTheta = maxBackwardsLegTheta -((16 * maxBackwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); leftLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod))); rightArmTheta = -1 * (maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod))); leftArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); rightHandTheta = rightHandMaximum - (rightHandMaximum - rightHandMinimum) * 2 * (frame - .5 * stepPeriod) / stepPeriod; leftHandTheta = leftHandMinimum + (leftHandMaximum - leftHandMinimum) * 2 * (frame - .5 * stepPeriod) / stepPeriod; headTheta = rightHeadMaximum - (rightHeadMaximum - leftHeadMaximum) * 2 * (frame - .5 * stepPeriod) / stepPeriod; visorTheta = maxVisorTheta - (maxVisorTheta - minVisorTheta) * 2 * (frame - .5 * stepPeriod) / stepPeriod; if (frame / stepPeriod < (5.0 / 8.0)) { torsoTheta = torsoForwardsMaximum - (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - 3.0 * stepPeriod / 8.0) / stepPeriod; } else { torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - 5.0 * stepPeriod / 8.0) / stepPeriod; } } else /* Forth forth of the step right leg and left arm are forward and moving forward. Rotation of left toe. Left leg back */ { pseudoTime = stepPeriod - frame; rightLegTheta = -1 * (maxForwardsLegTheta - ((16 * maxForwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod))); leftLegTheta = maxBackwardsLegTheta - ((16 * maxBackwardsLegTheta * pseudoTime * pseudoTime) / (stepPeriod * stepPeriod)); rightArmTheta = maxBackwardsArmTheta - ((16 * maxBackwardsArmTheta * pseudoTime * pseudoTime)/ (stepPeriod * stepPeriod)); leftArmTheta = -1 * (maxForwardsArmTheta - ((16 * maxForwardsArmTheta * pseudoTime * pseudoTime)/ (stepPeriod * stepPeriod))); rightHandTheta = rightHandMaximum - (rightHandMaximum - rightHandMinimum) * 2 * (frame - .5 * stepPeriod) / stepPeriod; leftHandTheta = leftHandMinimum + (leftHandMaximum - leftHandMinimum) * 2 * (frame - .5 * stepPeriod) / stepPeriod; headTheta = rightHeadMaximum - (rightHeadMaximum - leftHeadMaximum) * 2 * (frame - .5 * stepPeriod) / stepPeriod; visorTheta = maxVisorTheta - (maxVisorTheta - minVisorTheta) * 2 * (frame - .5 * stepPeriod) / stepPeriod; if (frame / stepPeriod < (7.0 / 8.0)) { torsoTheta = torsoBackwardsMaximum + (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - 5.0 * stepPeriod / 8.0) / stepPeriod; } else { torsoTheta = torsoForwardsMaximum - (torsoForwardsMaximum - torsoBackwardsMaximum) * 4 * (frame - 7.0 * stepPeriod / 8.0) / stepPeriod; } }I'll try to find the values used in the code.
static double toeAngle = 21.4477363271, heelAngle = 17.8188889146; //toeAngle = atan(11/28)*degreeConversion, heelAngle = atan(9/28)*degreeConversion;//in degrees static double rToe = 30.083217913, rHeel = 29.4108823397; // distance from leg joint to heel and toe, rToe = sqrt(905), rHeel= sqrt(865) static double shoulderAngle = 9.782 struct WalkingParameter //used for each minifig { /*Updated 11/9/04 added turnSlowdownFactor*/ //The first parameters are used in WalkStyleI function. //double maxBackwardsLegTheta;//default 2*toeAngle //double maxForwardsLegTheta;//default 2*heelAngle /*(in degrees) positive angles*/ double stepPeriod;//default 40 (This looks good when rendered at 30 frames per second.) double frame; double maxForwardsArmTheta;// default heelAngle double maxBackwardsArmTheta;//default toeAngle double rightHandMaximum;// default 45 double rightHandMinimum;// default -15 double leftHandMaximum;// default -45 double leftHandMinimum;// default 15 /*Maximums are when the hand is in front of the minifig and minimums are when their arms are moved back.*/ double leftHeadMaximum; //default -10 double rightHeadMaximum; //default 10 double MinVisorTheta;//(zero = closed, -70 = open)default -70 double MaxVisorTheta; double torsoForwardsMaximum; //default -10 double torsoBackwardsMaximum;/*default 20, when a leg is forward minifig should lean back this far.*/ //The following are used in the ChangeMinifigCoordingatesToStepCoordinatesBouce function. double bounce;//default 100 struct Xyz maxZLean;//default { 0, 0, 15};Just side to side lean for now. /*The minifig will lean side to side and front to back from this point by the maxZLean amount.*/ //The following are used in the RotateStepCoordinates function. double turnSlowdownFactor;//default 1.5 //The following are used in the ChangeStepCoordinatesToWorldCoordinates function. //The following are used in the main function. struct Xyz walkAngle; struct Xyz stepCenter; struct Xyz startNorth; struct Xyz startWest; struct Xyz startSouth; struct Xyz startEast; double turnYAngle; double turnIncrementTheta; }; static struct Xyz handArmDifference = { 5.202, 18.752, -10.321}; struct Xyz visor, helmet, head, torso, rightArm, rightHand, leftArm, leftHand; struct Xyz airtanks, rightLeg, leftLeg, waist, legJoint; struct Xyz cameraLocation, light1, light2, light3, cameraLookAt, lightPointAt; static double wristAngle = -28.8281500231; // atan(handArmDifference.z/handArmDifference.y) * degreeConversion static double armpitTheta = 15.5045682803; /* make negative for right hand*/ // atan(handArmDifference.x / handArmDifference.y) * degreeConversionThere's also some code that moves the minifig with respect to the ground. Since minifigs don't have ankles, I pivoted them on their toes and heels to give them a more natural looking walk. I'll post this additional code once I get my internet working.
My internet is on the fritz so attach files to my posts right now.
I also can't upload (or download) video. I have several videos of these minifigs (vertual) walking.
I rounded up some servos so I can write a little test program to show you (and erco if he's watching) how to add servos to a Prop project.
Darn it. I wont be able to attach the Spin files. I think I've going to try to get my internet working before writting the servo code.
I have a new DSL modem but I have to call tech support to configure it properly.
Duane
I dunno how far back you've read, but my legs will not have ankles either (in the normal sense) so this code may be more applicable than I though. I do have a left/right pivot on the foot to transfer the center of mass.
Here's some code for oscillating 16 servos. Some of the servos oscillate at twice the frequency of the others.
DAT fileName byte "Servo111027h" ' I like to have the name of the program ' displayed in a terminal window. CON{{ 111027a start working on a sevo demo. 27d modify equations. 27e keep servo #15 centered. 27f increase _FramesPerPeriod (slow down servos). 27g add displayPosition array so frame numbers match position values. 27h fix byte sized frame problem. }} CON _Clkmode = xtal1 + pll16x _Xinfreq = 5_000_000 _DebugBaud = 9600 _MaxPulse = 2000 _MinPulse = 1000 _Stop = 1500 _Center = _Stop _ServosInUse = 16 _FramesPerPeriod = 300 _PositionsPerRow = 4 VAR long time, timeOld, frameInterval, debugStack[100] word servoPosition[_ServosInUse], maxPosition[_ServosInUse], minPosition[_ServosInUse] word displayPosition[_ServosInUse], frameCount, frameCountOld byte servoPin[_ServosInUse] OBJ Debug : "Simple_Serial" Servo : "Servo32v7" PUB Setup | localIndex repeat localIndex from 0 to _ServosInUse - 1 ' Set pin numbers. They don't need to be consecutive. servoPosition[localIndex] := _Center ' You could set each pin individually. servoPin[localIndex] := localIndex maxPosition[localIndex] := _MaxPulse ' Set all the Max and Mins the same for now. minPosition[localIndex] := _MinPulse maxPosition[0] := _MaxPulse - 100 ' Change some of the Max and Mins. minPosition[0] := _MinPulse + 100 minPosition[1] := _MinPulse + 150 maxPosition[2] := _MaxPulse - 150 maxPosition[3] := _MaxPulse - 200 minPosition[3] := _MinPulse + 200 frameInterval := clkfreq / 50 frameCount := 0 cognew(DebugLoop, @debugStack) ' starts a cog 6 left waitcnt(clkfreq / 20 + cnt) Servo.Start ' starts a cog 5 left waitcnt(clkfreq / 20 + cnt) 'Servo.Ramp '<-OPTIONAL 'Start Background Ramping ' starts a cog not used so still 5 cogs left MainLoop PUB MainLoop | localIndex, pseudoPercent, nextFrameTime '' This method is being executed in cog #0. nextFrameTime := cnt + frameInterval repeat waitcnt(nextFrameTime) nextFrameTime += frameInterval pseudoPercent := ((frameCount * 100) / _FramesPerPeriod ) ' How far into the cycle are we? if pseudoPercent < 50 repeat localIndex from 12 to 14 servoPosition[localIndex] := minPosition[localIndex] + (2 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 8 to 11 servoPosition[localIndex] := maxPosition[localIndex] - (2 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100 if pseudoPercent < 25 repeat localIndex from 0 to 4 servoPosition[localIndex] := minPosition[localIndex] + (4 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 5 to 7 servoPosition[localIndex] := maxPosition[localIndex] - (4 * pseudoPercent * (maxPosition[localIndex] - minPosition[localIndex])) / 100 else repeat localIndex from 5 to 7 servoPosition[localIndex] := minPosition[localIndex] + (4 * (pseudoPercent - 25) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 0 to 4 servoPosition[localIndex] := maxPosition[localIndex] - (4 * (pseudoPercent - 25) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 else repeat localIndex from 8 to 11 servoPosition[localIndex] := minPosition[localIndex] + (2 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 12 to 14 servoPosition[localIndex] := maxPosition[localIndex] - (2 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 if pseudoPercent < 75 repeat localIndex from 0 to 4 servoPosition[localIndex] := minPosition[localIndex] + (4 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 5 to 7 servoPosition[localIndex] := maxPosition[localIndex] - (4 * (pseudoPercent - 50) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 else repeat localIndex from 5 to 7 servoPosition[localIndex] := minPosition[localIndex] + (4 * (pseudoPercent - 75) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 0 to 4 servoPosition[localIndex] := maxPosition[localIndex] - (4 * (pseudoPercent - 75) * (maxPosition[localIndex] - minPosition[localIndex])) / 100 repeat localIndex from 0 to _ServosInUse - 1 Servo.Set(servoPin[localIndex], servoPosition[localIndex]) if frameCount == _FramesPerPeriod - 1 frameCount := 0 else frameCount++ PUB DebugLoop | localIndex '' We'll use a separate loop to send debug information '' so it doesn't slow down the servo calculations. '' It is likely that not all frames will be displayed '' since the data can't be transmitted fast enough to '' keep up with how quickly the seroPosition data is '' changed. Debug.init(31, 30, _DebugBaud) Debug.str(string(13, "Debug Started ")) Debug.str(string(13, "fileName = ")) Debug.str(@fileName) Debug.tx(13) frameCountOld := _FramesPerPeriod + 1 ' not a legitamate frame so frameCount is sure not to be equal to it. repeat repeat while frameCount == frameCountOld frameCountOld := frameCount wordmove(@displayPosition, @servoPosition, _ServosInUse) ' move the servoPosition values so they don't change while they ' are being displayed. Debug.str(string(13, 13, "Frame # ")) Dec(frameCountOld) Debug.str(string(" Servo Positions")) 'Debug.tx(13) repeat localIndex from 0 to _ServosInUse - 1 if (localIndex // _PositionsPerRow) == 0 Debug.tx(13) Debug.str(string("SP[")) else Debug.str(string(", SP[")) Dec(localIndex) Debug.str(string("] = ")) Dec(displayPosition[localIndex]) PUB Dec(value) | i, x '' Since Simple_Serial doesn't have a dec method. '' Copied from FullDuplexSerial '' Print a decimal number x := value == NEGX 'Check for max negative if value < 0 value := ||(value+x) 'If negative, make positive; adjust for max negative Debug.tx("-") 'and output sign i := 1_000_000_000 'Initialize divisor repeat 10 'Loop for 10 digits if value => i Debug.tx(value / i + "0" + x*(i == 1)) 'If non-zero digit, output digit; adjust for max negative value //= i 'and digit from value result~~ 'flag non-zero found elseif result or i == 1 Debug.tx("0") 'If zero digit (or only digit) output it i /= 10 'Update divisorHere's what the output looks like.
That can't be scarier than PBASIC code to oscillate 16 servos. Right?
The oscillations in this code was linear. The code I posted early to make the minifig walk was different than this code. I believe the minfig code used the equations from an oscillating pendulum (hence sinusoidal).
I didn't try to translate the pendulum oscillation equations into Spin yet. I just wanted to give an example of moving multiple servos with a Propeller. While the code produces signals for 16 servos, I only connected 8 servos. Still 8 servos oscillating back and forth is still fun.
Servo #15 doesn't oscillate. It's set to the center position. I used this to adjust a couple of continuous rotations servos I had.
Hopefully this large post will make through my bad internet connection.
Duane
I took quite a lot of notes and questions I need to find the answers for. The Prop is quite a bit different than anything I've programmed so far, but we'll manage I hope.
WHOA! Duane, you da man! I can actually halfway (now) read through and understand what your code does. Thank you sir. The nicest thing about the Prop over the BS for this particular project is the internal clock.
I have no doubt the Prop will handle all I want to do for the biped. I also have no doubt you've single handedly postponed it's completion until I can get my mind wrapped around the Prop. <sigh> all for the best though
I tried to edit my post with code that didn't wrap around but it was too much for my internect connection to handle.
The method "Dec" is taken from the object (program) FullDuplexSerial. There are a couple of lines in the Dec method I don't understand. But the rest of the program I should be able to explain.
I'm basically treating the robot as an animated object. Instead of 30 frames per second I use 50 frames per second since that's the fastest many servos can be updated.
As part of the "Setup" method I add the line "frameInterval := clkfreq / 50". frameInterval then holds the number of clock cycles in one 50th of a second (or 20 ms).
Before the repeat statement in "MainLoop" is the line "nextFrameTime := cnt + frameInterval". This sets nextFrameTime equal to the current time (held in cnt) plus 20ms. This will be the time the first loop will be executed.
The command in "MainLoop" that causes the Prop to wait for the next 20 ms interval is "waitcnt(nextFrameTime)". Once this next time interval is reached the loop continues. I imediately add another 20 ms to "nextFrameTime" with the statement "nextFrameTime += frameInterval". This is a short hand for "nextFrameTime := nextFrameTime + frameInterval". This sets up the "nextFrameTime" so it will have the value we want it to have at the beginning of the next loop.
This adding 20 ms to "nextFrameTime" keeps the loop syncronized to every 20 ms. You would have a problem if the loop took longer than 20 ms. If the loop took longer than 20 ms, "waitcnt(nextFrameTime)" would wait for the system clock to roller over (this takes about 53 seconds @ 80MHz).
Let me know if there's a portion of the code you'd like be to explain.
Duane