PDA

View Full Version : Meet Hi No Hikari



darkback2
10-13-2010, 12:11 AM
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/plm02-0.jpg

This is my latest project spurred on by Tybes' post on PLM design.

My wife got a bunch of AX-12s over the summer planning to build a robot. Well her busy schedule made building a robot all but impossible. so I decided to put those robot parts to good use. :-p

I designed all of the brackets using turboCAD, cut the parts out using a bandsaw and drill press. I then bent them using a bench vice and hammer. you can see the dings in all of the parts. The only parts left are the 3 DOF arms. Because we are on a tight budget I have to wait until next month to get the servos for those. I'm not sure about the weight. Adding arms might put it over the limit for AX-12s, and I can't really swing 18Fs right now. I have teflon for the bottoms of the feet, though I'm not sure I'm going to make those right away. I sort of want to see it moving first.

If possible I will be saving up to build an RX-24F version over the next two years... of course by then the RX-24Fs will be old hat and we will be using something really incredible.

Here are some pics and stats. I'll try to post more video as I get things up and running.

Current stats:

Height 18.25 inches
Legs 12.75 inches
Current weight 1.76 kg :-(

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/plm02-1.jpg

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/plm02-3.jpg

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/plm022.jpg

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/plm01-0.jpg


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

Here are the PDF files if you want to build one yourself. I'll update them periodically. (http://forums.trossenrobotics.com/datacenter/members/darkback2-1499/)

Connor
10-13-2010, 10:19 AM
DB, I'm a little concerned about how far apart you have the hips.. That looks like it might be a little to much.. you may need to bring them in to keep from overloading the ankle role servos...

darkback2
10-13-2010, 11:30 AM
DB, I'm a little concerned about how far apart you have the hips.. That looks like it might be a little to much.. you may need to bring them in to keep from overloading the ankle role servos...

I hear you there. The hips are currently 11cm wide at the center of the servo. I'm looking at making the feet smaller because right now they are redonculous. I was also considering switching up to AX-18s in the future though I'm not really into strong arming my way around solving a problem.

I'm hoping to get it wired today and I'll do some tests to see if I need smaller hips, and conversly feet. I was originally hoping to keep the payload down, but there are no ways around the laws of physics. :-)

DB

DresnerRobotics
10-13-2010, 11:49 AM
I'd keep those feet as is, you're going to be unpleasantly surprised at how hard stable walking gaits are to develop.

The hips are a bit wide but it looks great!!

darkback2
10-13-2010, 12:18 PM
Part of my hope is that I can use all 4 corner leg servos so both ankles and both hips to tilt the bot and shift its weight and sort of shuffle walk. I know that still puts a strain on the ankles, so we'll have to see how bad it is after its wired. In any case banging out smaller hips wouldn't take that long. The feet are 13cm * 8cm so they are really big, but big means stable. :-)

RobotAtlas
10-13-2010, 02:23 PM
While the leg design is great, I was really amazed at this:


My wife got a bunch of AX-12s over the summer planning to build a robot.

Matt
10-13-2010, 03:13 PM
Um, what the hell is slithering through the background? LOL

sthmck
10-13-2010, 03:16 PM
Um, what the hell is slithering through the background? LOL

Julius Squeezer- the python code snake

gdubb2
10-13-2010, 04:20 PM
DB.. That's looking great.. and congrats on keeping Maya so busy that you were able to abscond with the servos..Brilliant..

Gary

animeunplugged
10-13-2010, 05:08 PM
Awesome!!! Now all you need is a head and a body!

Stobs
10-13-2010, 05:38 PM
That's awesome DB, love it - very interested in seeing how this progresses! And I'm with RobotNV & Gary, literally LOL'd when I read that bit! :)

Stobs/Paul

cire
10-13-2010, 10:01 PM
Good job on acquiring the servos :p

It looks very solid, nice work.

DresnerRobotics
10-13-2010, 10:14 PM
I think that "I stole these robot servos from my wife" might be an internet first. :D

Stobs
10-13-2010, 10:39 PM
I think that "I stole these robot servos from my wife" might be an internet first. :D

Maybe he should qualify that with how many nights he had to sleep on the couch over that one! :veryhappy:

jes1510
10-14-2010, 07:25 PM
I dig it dude!

darkback2
10-15-2010, 12:48 AM
So first off...thanks for the kind words. My mom is out visiting, and saw Hikari doing the little side to side wobble bit. She wants one for her desk at work. I told her how much it would cost...to just sit on her desk...she wrote me a check. So Hi No Hikari will be getting a sister for christmas.

We had parent teacher conferences today so I had plenty of time to sit there working on my robot. That was until I realized i forgot to unlock my door, or turn on the lights in my classroom...In anycase. Here is some video.

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

DB

elaughlin
10-15-2010, 05:55 AM
It looked all innocent in the video just wobbling back and forth, then it starts turning and looks incredibly powerful. Nice job.

darkback2
10-19-2010, 11:03 PM
So I was looking through the rules for humanoid and found this

"2.2.3. The robot shall have a head"

What is the advantage of not having a head if the robot is nowhere near 120cm. I can see it if the robot is 120 cm without a head, but if the robot is say 30 or 40 CM? And what counts as a head? Is the robot disqualified if the head falls off?

Still waiting on UPS with Hikari's arms. They were supposed to get here today, but because of miss management they did a loop around chicago and won't get here till thursday. :-(

Oh well...Guess I'll have to play with X in the mean time. :-) Wait...X doesn't have a head either. darn it.

DB

gdubb2
10-20-2010, 10:35 AM
DB.. just for the "Fun" factor, I made a little head mount for Bheka when it a humanoid. It was just a piece of dowel that just fit inside a finger puppet, available at toy/party stores. Interchangable heads that were cute..stupid.. and.. fit the bill.

Got a whole bag of finger puppets for a couple of $$..
Gary

DresnerRobotics
10-20-2010, 11:04 AM
So I was looking through the rules for humanoid and found this

"2.2.3. The robot shall have a head"

What is the advantage of not having a head if the robot is nowhere near 120cm. I can see it if the robot is 120 cm without a head, but if the robot is say 30 or 40 CM? And what counts as a head? Is the robot disqualified if the head falls off?

Still waiting on UPS with Hikari's arms. They were supposed to get here today, but because of miss management they did a loop around chicago and won't get here till thursday. :-(

Oh well...Guess I'll have to play with X in the mean time. :-) Wait...X doesn't have a head either. darn it.

DB

A lot of competitors will just tape a Dixie cup to the top for a head. They didn't give me any problems at KRGF for not having a head either.

animeunplugged
10-21-2010, 10:19 AM
Please tell me that they aleast spay painted the Dixie cup ?

darkback2
10-21-2010, 11:38 AM
I'm going to have to check with Dave, but I believe the segment above where the arm servos are would count as a head if I tape some googly eyes to the front of it. I'm not really sure what the classification of a head is, but given that the brains (the arbotix) are in there, what more could anyone ask for.

DB

DresnerRobotics
10-21-2010, 02:01 PM
Please tell me that they aleast spay painted the Dixie cup ?

Some people build for function prior to aesthetics.

darkback2
10-22-2010, 12:44 AM
So the brown truck paid a visit today, and Hi No Hikari got a set of arms. Disaster though because Hikari is about 250 grams overweight. This makes Hikari a middle weight and puts it up against bots like Giger...

The cool part is I get to make a second Hikari. The current plan is to upgrade the hips and ankles using AX-18Fs, Cut some out of the arm, and top portion, and feet in the hopes of loosing the weight.

Here are some picks.
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hinohikari-1.jpg
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hinohikari-0.jpg

I'll try to get some motions done tomorrow and some video this weekend.

DB

DresnerRobotics
10-22-2010, 08:42 AM
I can tell you from experience, 250g of weight is a LOT to shed via cutouts. You'd need to drop 3 servos as well, at least. What are you using for a battery? Milspec lipo might be in your future.

darkback2
10-22-2010, 10:12 AM
Switching to a mill spec lipo, thinner metal for the feet, cutting chunks out of the body, and getting aluminum screws for the ankle and leg brackets...Maybe I'll tie some baloons to the top if all else fails. :-) Actually if all else fails I'll be switching all of the servos for RX-24Fs and 28s in the ankles and hips...but that is more of a long term plan...which really makes the whole thing mute...SO instead maybe I'll just let Hikari 01 get her butt kicked and hope that Hikari 02, the 24F version does a bit better.

lnxfergy
10-22-2010, 10:28 AM
I can tell you from experience, 250g of weight is a LOT to shed via cutouts. You'd need to drop 3 servos as well, at least. What are you using for a battery? Milspec lipo might be in your future.

Tybs -- you don't build your bots out of 1/8 thick scrap metal though... :veryhappy:

-Fergs

darkback2
10-22-2010, 10:47 AM
Unfortunately no...with the exception of the feet this bot ia all aircraft grade aluminum .004 thick I think. I'm coming to the conclusion that this bot is just going to be an overweight looser.

DB

Connor
10-22-2010, 10:52 AM
Unfortunately no...with the exception of the feet this bot ia all aircraft grade aluminum .004 thick I think. I'm coming to the conclusion that this bot is just going to be an overweight looser.

DB

Turn the Holes into slots DB..

O O
O O
O O
O O

into

(==)
(==)
(==)
(==)

and put a few holes & slots in the torso too.

cire
10-22-2010, 05:30 PM
I heard drilling holes in your battery is the best way to lose weight!

Honestly though, you won't lose much drilling holes in your brackets, aluminum is only 2.7g / cm^3 , you can do a quick calculation to see how much you'll lose per hole. Its a good backup if you are really close to the limit, but at 250g you will obviously need to take another approach. I tend to see drilling holes as something you do after your bot is done and you are only slightly over.

mannyr7
10-22-2010, 08:45 PM
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hinohikari-1.jpg

DB

I know you're using clean aluminum, but this shot makes it look like old rusty steel. I like that effect! Awesome work, DB!

Upgrayd
10-22-2010, 09:03 PM
Great looking bot DB. Looking forward to seeing some more videos.

How about using brackets similar to the these (http://www.trossenrobotics.com/store/p/6181-Bioloid-Frame-F3.aspx) to attach the the butt to butt servos in each each leg segment to save some weight? Would remove the need of the four large brackets you have now.

darkback2
10-23-2010, 02:53 AM
Thanks for the kind words and advice everyone. I'm pretty convinced hitting the 2kg mark is out of the question for this guy. The good news is over the next two years I can upgrade all of the servos to AX-18Fs. In the second one which I hope to have done by christmas, I'll start out with AX-18fs for the ankles and hips. For now the AX-12s will have to do for the legs.

Spent the evening coming up with some moves. I'll get the side steps pretty soon, turning seams to work, and I even have a face down get up sequence. After recording the video I made a back down roll over sequence. This was actually relatively hard given how wide this bot is. With the arms she is 10 inches at the shoulder.


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

DB

darkback2
10-31-2010, 07:21 PM
http://www.youtube.com/watch?v=BgCzs4HUgAc

So I've been working over the past few days trying to make Hi No Hikari more stable, and to increase her range of movements. I'm still not at all happy with my forward walk. It is more of a shuffle, and it isn't very good because she tends to go in a circle to the right.

I am running up against the limitations of the servos. Thermal shut down happens after about 6 minutes of use, and I can't let the legs get too far apart. I'm also trying to incorporate bouncing to help with motions. Seams like it is working well for side stepping, but it makes moving around a bit slower because the robot has to prep for a jump before each move.

I don't think this current inception will ever qualify for robo-one, but she can get up from both face down and face up.

Any suggestions on the walking gate are welcome.

Thanks.

DB

DresnerRobotics
11-01-2010, 08:24 AM
If youre running into overheating try bending the knees less. Also, your forward walking gait doesnt even appear to be lifting the legs off the ground at all. A 'shuffle' gait as you see most humanoid kung fu fighters doing will actually still lift the foot off the ground, it's just very slight and quick. Lessening your hip distance will help with lifting the legs.

darkback2
11-01-2010, 08:31 AM
Thanks for the help Tybes.

I am going to try rewriting the walking/leaning poses from scratch this afternoon. I was going for more of a shuffle than a walk, but I do need to actually lift the feet in order to qualify for Robo-one styled fighting. I am hoping if things work out that I can simply transfer the gates over to a stronger RX-28/24F based PLM bot in the future.

Center to center the hips are 9 cm apart. When standing upright the feet are 1 cm apart. Should I go for narrower feet? When I moved them closer they got caught up on each other.

Now that I look at it, most of everyone else's feet stick out to the outside.

DB

darkback2
11-01-2010, 10:23 PM
So I shot a video of Hikari walking for trouble shooting purposes yesterday, but ended up not needing it because Tybes pointed out a big part of what I was doing wrong. I still can't get her to lift a leg without falling over sideways when the walking gets faster, but things are going a lot smoother now.


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

UncleBob
11-02-2010, 12:20 AM
I posted a comment to your vid. Your gait looks almost fine. You just need to speed up everything by 3x. Also remove the pause in between moves.

DresnerRobotics
11-07-2010, 07:32 AM
So I shot a video of Hikari walking for trouble shooting purposes yesterday, but ended up not needing it because Tybes pointed out a big part of what I was doing wrong. I still can't get her to lift a leg without falling over sideways when the walking gets faster, but things are going a lot smoother now.


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

You need to speed things up considerably, shuffle gaits are usually pretty fast. Also keep in mind that a shuffle gait doesn't mean you can't lift your feet up off the ground, you actually have to lift them otherwise they'll just drag and cause you to veer off course. Again, if you're running into overheating, don't walk with such a bent-knee default position.

Feet should have a lot more width on the outside than on the inside, actually the most recent feet I've made only have enough room to mount the servo on the inside edge, the rest of the width protrudes outward.

darkback2
11-21-2010, 06:15 PM
http://www.youtube.com/watch?v=GBDanljlVoU

So its 1995 and the movie strange days comes out. I'm instantly in love with the idea of recording memories. A few years later I'm at CalArts, and I watch a guy named David Rosenboom controlling a computer playing music using an EEG device. Fast foward to about 3 years ago. I've been making robots for a while when I stumble across a device that has the potential to allow me to control a robot using my mind.

Ok, so that was two years ago, and really I have had about 2 years of frustration. There was the long wait after ordering the device, and when it finally got here I was pretty much un-able to get it to do anything. It sat on my shelf collecting dust for a long time after that.


http://www.youtube.com/watch?v=5GX8moQzOOM

A few months ago I stumbled across a really lame anime series Angelic Layer. In the series people use mind controllers to control robot dolls that fight in an arena. That sort of sparked renewed interest on my part. Hopefully at some point I'll be better able to control Hikari using the Emotiv Epoc

I need to edit the code so that it plays the whole sequence every time the key is pressed. At current the key has to be held and it loops through the sequence. Any help would be appreciated.

Here is the code:

#include <ax12.h>
#include <BioloidController.h>
#include "iPose01.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);

void setup()
{
// need to start the serial port
Serial.begin(38400);

// recommended pause
delay(1000);

// stand up slowly
bioloid.loadPose(Up);
bioloid.readPose();
bioloid.interpolateSetup(500);

while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}

void loop()
{
// check for new messages over the serial port
int x = Serial.read()
;
if( x >= 0 )
{
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));

// only process if we aren't currently doing a sequence
if(bioloid.playing == 0)
{
if(x== 'w') //if I press w walk forward
{
bioloid.playSeq(LRLean);
}
else if(x== 's') //if I press s stop moving
{
bioloid.playSeq(UP);
}
else if(x== 'x') //if I press s stop moving
{
bioloid.loadPose(Rest01);
}
else if(x== 'g') //if I press s get up
{
bioloid.playSeq(facedown);
}
else if(x== 'q') //if I press q turn left
{
bioloid.playSeq(LeftTurn);
}
else if(x== 'a') //if I press a sidestep left
{
bioloid.playSeq(SideLT);
}
else if(x== 'e') //if I press e turn right
{
bioloid.playSeq(RightTurn);
}
else if(x== 'd')//if I press d sidestep right
{
bioloid.playSeq(SideRT);
}

else if(x== 'j') //if I press j attack left
{
bioloid.playSeq(LA);
}

else if(x== 'l') //if I press l attack right
{
bioloid.playSeq(RA);
}
else if(x== 'k') //if I press k right punch
{
bioloid.playSeq(RightPunch);
}
}

if(bioloid.playing > 0)
{
// continue sequence
bioloid.play();
}

}
}

lnxfergy
11-21-2010, 10:09 PM
Move the second to last } bracket up above the "if bioloid.playing > 0" statement. This way, it will execute very time, even in the absence of command inputs.




} // this was down below

if(bioloid.playing > 0)
{
// continue sequence
bioloid.play();
}

// I removed a closing bracket from here
}-Fergs

darkback2
11-21-2010, 11:27 PM
Thanks Fergs.

So I updated the code and everything works a whole lot better...Probably helps that my headache is gone. Here is the new vid.


http://www.youtube.com/watch?v=1DNqBAa4Nkk

DB

darkback2
11-24-2010, 02:55 AM
So I spent the day getting Hikari to work with my Ipad. I'm using Touch OSC to send commands to my PC, and then I wrote a MAX/MSP patch that converts the Ipad commands into key commands that Hikari can respond to. The patch is set up so that I can also control Hikari directly from the computer Keyboard. This way as I expand the detections with the Emotiv Epoc and Emokey the program will integrate in the same way.


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

There is a weird loss of power in the legs. I also had to revamp the BackToFront sequence. The old one kept getting hung up. I had to be careful with the new one because the arms get into a position where they can pull the servo wires out.

DB

darkback2
11-25-2010, 12:43 AM
I added some dance moves...I'll be cutting a new left leg. Currently the left leg has a bit too much play, and is therefor not as predictable as the right. given that I broke my last bandsaw blade, and tomorrow is a holiday I don't think I'll be able to get to it until friday...Then again, there is no school so...

http://www.youtube.com/watch?v=kX-g3Vo5X2M

darkback2
11-28-2010, 01:25 PM
So, inspired by Darwin-Op, and a minor lack of funds I'm considering adding a roboard to Hikari. I figure that way I could mount a camera on her and possibly get aspirations of doing robo-cup stuff...Of course I would need to get her walking properly first... which seams like a big deal...

I'm still working out the legs issues also. I rebuilt the left leg today, and for some reason it stands about 3 millimeters shorter than the right. I'm thinking lifts would be an easy sollution, but probably a dumb one. I'm also still having momentary weakness in the left leg that is probably coming from a shorted wire momentarily loosing its connection. When the left leg gives out all of the servos flash...

new idea...wireless servos. :)

DB

Xevel
11-28-2010, 02:34 PM
This project is looking great. Can't wait to see it walk (I bet you too ^^).

lnxfergy
11-28-2010, 05:37 PM
When the left leg gives out all of the servos flash...

Servos flashing == servos resetting. So, the voltage drops below ~7V at some point for that to happen. Two possibilities: if just that leg flashes, then you might have a loose wire (either ground or 12V), if ALL of the servos flash, then it may be either a loose connection to the battery, or that your battery/voltage source can't source enough current for the robot -- how are you powering the bot?

-Fergs

darkback2
11-28-2010, 06:00 PM
I already know its the bad wire problem. I sort of need to get a round to it in order to fix it because it started out as such an intermittent problem...Now of course it's gotten worse and worse over time meaning I probably need to get a round to it before I play with the robot again.

DB

darkback2
12-05-2010, 01:34 AM
So Inspired by all the talk about the Darwin OP I figured I should work on making Hikari both more intelligent and capable. So, I'm jumping in with both feet and mounting a roboard computer Under Hikari's battery. I just got the computer in the mail yesterday and will be working tomorrow to make a metal plate for mounting the hardware. The current goal is to have three PLM robots with cameras mounted. I'll upload the PDF files for cutting out the mounting plate as soon as its done.

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/poop2.jpg

In the mean time I spent today making a second copy of Hikari. I cut out most of the brackets a few weeks ago and have been waiting for the parts for assembly.

Here are the leg brackets laid out next to Hikari
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/poop1.jpg

Things Starting to go together.
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/poop3.jpg

Legs and body done next to Hikari for comparison.
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/poop4.jpg

Unfortunately this second bot is not mine. Its a gift that my mom commissioned for my dad. I have the arms cut out. I am waiting to get four more servos so that I can get them mounted up.

I also replaced all of the wires in Hikari's legs, and replaced the hip and ankle servos with AX-18s. Things are moving a whole lot better. I'm sort of in love with AX-18s for this project. They seam to give me just enough power while still leaving room for mounting the PC under the battery without it getting in the way of the legs.

As for the voltage problem It was in part a bad wire, and in part the powersupply. When I am running the bot plugged in I loose power to all of the servos and they reset themselves when the bot moves too quickly. Its sort of like the robot has narcolepsy. When I'm running off of the battery I was getting a similar problem but only in one leg. Since switching out the wires the problem leg has gone away.

Here is a quick video showing Hikari walking with the new AX-18s in her hips and ankles. I also test carrying a 1/2 pound weight to see the effect of adding the roboard.


http://www.youtube.com/watch?v=oMmTeUe-BnU

Here are a couple pics of the roboard all mounted Up.
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/roboard_mount1.jpg

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/roboard_mount2.jpg

Thanks.

DB

darkback2
12-05-2010, 01:48 AM
Here is a link to the code I developed for this robot. I had a lot of help with that part.

The Specific post of the current code.
Code Post (http://forums.trossenrobotics.com/showpost.php?p=44430&postcount=20)

The thread showing its development.
Entire Thread (http://forums.trossenrobotics.com/showthread.php?t=4434)

UncleBob
12-06-2010, 12:14 AM
How you cut the parts and replicate it? If you in my area I would help you get some cool walking gait.

darkback2
12-06-2010, 12:52 AM
Hey Uncle Bob

I start out by drawing all of the parts using turbocad. I draw them flat leaving 2mm between each side which gets lost in the bend. I glue the paper print out onto sheet metal using rubber cement. I used to use spray on adhesive, but rubber cement peels off cleanly without having to use solvent.

Then I cut the parts out using a band saw. I use a fine tooth metal blade.

I drill all of the holes as marked on the paper and sand the edges with an angle grinder with a sanding wheel on it.

Finally I use a vice and sheet metal pliers to bend the metal. I finish the bends with a flat hammer.

With the second Hikari I used an orbital sander to scuff the metal which also hides the hammer marks.

I made a tutorial about how I do it. (http://forums.trossenrobotics.com/tutorials/how-to-diy-128/building-a-mech-using-dbs-bracket-system-3099/?catid=searchresults&searchid=10040)

As for help making walking gates, I live in Washington State in the US. I have used some of your tutorials to try to get things working. Any further help would be greatly appreciated.

DB

elaughlin
12-06-2010, 05:47 PM
Ahhh robotics friends all over the world coming together right here on the Trossen Forum.

Really like Hikari so far DB. I also think its awesome that family members want to purchase your works! Mine just looks at me strangely. Keep it up sir. Can't wait to see your bots at Mech Warfare.

Also wanted to add - I saw your tutorial a long time about about cutting out sheet metal pieces. I used it to cut out my small brackets, just tweaked it a little bit. I don't know what the cost of spray adhesive is or the rubber cement, but for about 10$ I bought a package of peel and stick paper from staples. I printed out my designs onto that paper, peeled off the back and stuck it on the sheet metal. There was some residue when taking it off in the end, but just water and a little soap took it off easily.

darkback2
12-10-2010, 01:03 AM
So the good news is I managed to finish cutting out and fitting the parts for Hi No Hikari's Twin.
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/2010-12-09_20-54-27_37.jpg

I also have an Arbotix Commander that I plan to run with this one. That way it won't require a computer and all the other good stuff in order to make it work. Here is the code I came up with with a lot of help from Fergs. I had trouble getting it going, but with Ferg's help I got it running. Turns out I forgot to start the serial port. The code posted works like a charm.



#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "iPose01.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

void setup(){

//open the serial port
command.begin(38400);
// stand up slowly
delay(100); // recommended pause
bioloid.loadPose(Up);
bioloid.readPose();
bioloid.interpolateSetup(500);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
}

void loop(){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence
if(command.walkV > 50){
// walk forward
bioloid.playSeq(WalkForward03);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(rest01);
}else if(command.walkH > 50){
// turn left
bioloid.playSeq(SideLT);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(SideRT);
}else if(command.lookH > 50){
// turn right
bioloid.playSeq(LeftTurn);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(RightTurn);
//Here go the button commands
}if(command.buttons&BUT_R1) {
// right punch
bioloid.playSeq(RightPunch);
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(FFrontAttack);
}if(command.buttons&BUT_R3) {
//Drop down and punch
bioloid.playSeq(RestPunchCombo);
}if(command.buttons&BUT_L6) {
//Soccer Kick
bioloid.playSeq(RightKick);
}if(command.buttons&BUT_L5) {
//Get up from face down
bioloid.playSeq(facedown);
}if(command.buttons&BUT_L4) {
//Roll Over (still not working well)
bioloid.playSeq(backtofront);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RA);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LA);
}
}
}// end else

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
} //end loop
Not sure what is not working here.

I'm really happy with how repeatable this process has been. This is the first time I have ever gotten to rebuild the same robot. Things went together very smoothly, and I came up with a new way to route the wires so that they won't be as likely to get hooked on stuff or torn. I replicated this method with the old Hi No Hikari.

DB

darkback2
12-11-2010, 10:38 PM
Spent the day coming up with image tracking software.

Its still a bit jittery, and there is some error which makes the robot get the message telling it to dance every once in a while. So, a lot of stuff needs tracking down. I live pretty far to the north, and the light in the room got bad pretty early. I spliced a movie of a red dot dancing around for testing purposes. Here is a screen shot.

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/ballsearch.jpg

Here is a shot of the camera mounted on Hi No Hikari. The camera is removable, so I can mount it for robocup and take it off for kung fu. In reality I sort of prefer the robot without the camera, but it fits nicely.

http://sphotos.ak.fbcdn.net/hphotos-ak-snc4/hs754.snc4/65413_1655784867184_1013961601_31826722_5970669_n. jpg (http://www.facebook.com/photo.php?pid=31822525&id=1013961601)

DB

Stobs
12-17-2010, 08:54 PM
I don't know, looks kinda cool with the camrea as well - reminds me a bit of the Thor mech (http://www.sarna.net/wiki/Summoner_(Thor)) :)

darkback2
12-17-2010, 09:05 PM
So I'm in the process of getting 12 RX-28s. If I can get them and at least 2 RX-24fs, then I'll make a PLM mech version of this bot...which will probably look a lot like the thor mech you posted. Will I be done on time? don't know.

DB

darkback2
12-19-2010, 02:21 PM
This project, actually all of my projects suffered a minor setback 3 days ago. I managed to destroy my computer and had to start over reinstalling everything and getting it all working again. I somehow managed to back up a lot of my data, and the fact that I post quite a bit on this forum allowed me to simply copy and past a lot of the work I have done from the forum back onto my new computer. I think I've worked out the last of the kinks, and I am finally back to where I was three days ago. :-)

DB

UncleBob
12-20-2010, 12:22 AM
Hikari is featured in www.tuaw.com

darkback2
12-20-2010, 02:27 AM
Wow! Thanks for the heads up!

So a quick google search turned up a bunch more mentions of Hikari. oddly it is mostly on cell phone sites, and only focused on the iPad controller stuff I did. But still...cool!

darkback2
12-21-2010, 02:56 AM
The video of me controlling hikari using an Ipad is up over 6000 views...I think it almost doubled in one day.

UncleBob
12-21-2010, 06:02 PM
6000 views is a lot. Well done.

master_of_robots
12-22-2010, 02:28 PM
The video of me controlling hikari using an Ipad is up over 6000 views...I think it almost doubled in one day.

Where can this video be found?

darkback2
12-22-2010, 06:29 PM
Hey Master_of_Robots

The video can be found here:

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

There are also other videos showing different things I've done with the robot.

DB

darkback2
12-27-2010, 12:28 PM
So I was watching a video or reading a post by UnkleBob and he said something about how when a walkers servos aren't strong enough you have to be really exact with the gate positions. Given how heavy Hikari is I figured the AX-18s in the hips just weren't up to the challenge.

I swapped them out for two AX-12s. I'm thinking I'll use the two AX-18s in the shoulders.

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hikariupgrade_-_4.jpg
Here you can see the double hip brackets.

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hikariupgrade_-_1.jpg


http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hikariupgrade_-_2.jpg
Here is a close up. The battery fits nicely between the two hip servos. I could probably stand to make the double servo brackets about 2mms taller. now it sort of bows out a bet. The battery is a nice tight fit.

Here is a video. One of the wires in the left hip is bad. As the wire bounces the servos seam to loose power. I have to loom up another wire to replace it with.
http://www.youtube.com/watch?v=YTysfiWXkyM

DB

DresnerRobotics
12-27-2010, 12:41 PM
I'd double up the ankles as well, because those are now the weakpoint in your legs (and you've added more weight with the additional servos).

darkback2
12-27-2010, 12:46 PM
I'd double up the ankles as well, because those are now the weakpoint in your legs (and you've added more weight with the additional servos).

Might end up doing that. I'm going to hold off for now. Mostly because I am running out of AX-12s. I'm also worried about adding too much weight to the feet and canceling out the increase in strength at the hips. Its all about the experiment at this point, so I may go the double double route in the future.

DB

DresnerRobotics
12-27-2010, 02:15 PM
Personally I'd have upgraded the ankles prior to the hips. Ankles are always a weak point. I'm in the school of though that if you're going to upgrade one joint, might as well upgrade all the weight bearing joints. Otherwise you get left with weak points.

I went through this exact problem with my RX-24F-now RX-28 PLM bot.

UncleBob
12-28-2010, 12:24 AM
Actually I am going to an experiment and post the result on YouTube. I have a idea that actually robot do not need ankle servos to walk and turn. Of course need one if want to stand on one foot. I will pull the servo wires from the ankle and test these motions and show u. It does not need ankle because if walking quickly the body weight do most of the works.

RobotAtlas
12-28-2010, 07:19 PM
Did you see how Darwin-OP does doubling of servos in hips and ancles?
It does not make those brakets twice as wide - extra servo just "hangs" on the back.
I can think of two reasons to do it this way:
1. Reduce weight of brackets (big one)
2. Shorter brackets are sturdier.

http://1.bp.blogspot.com/_Y2RcOfpufTg/TO6r_w0ifQI/AAAAAAAAAkM/fwBPRfXukkk/s1600/screenshot.jpg

lnxfergy
12-28-2010, 07:29 PM
Did you see how Darwin-OP does doubling of servos in hips and ancles?
It does not make those brakets twice as wide - extra servo just "hangs" on the back.
I can think of two reasons to do it this way:
1. Reduce weight of brackets (big one)
2. Shorter brackets are sturdier.



They're not actually doubled up -- it's still a single hip/ankle servo in the standard 5/6-servo leg they just have a shorter bracket by hanging the servo off the back.

-Fergs

darkback2
12-29-2010, 12:28 AM
I'm going to have to take some time to seek out all of the gremlins that have crept up in this bot. I think the design is eating the wiring, or maybe the wires are just too long. The ankle servos seam to be holding up well, and don't seam to be giving out at all.

When I do get thermal shutdown its coming from the various PLM servos. I've straightened up the legs a bit which should help with that.

I'm also getting failed read errors. It isn't saying any specific errors...so I need to go through and replace all of the wires or something. :(

I'm dialing in the walking. and starting to work on the "cotton candy attack motions." I should probably work on getting up sequences while I'm at it.

DB

darkback2
12-31-2010, 02:09 AM
I made a vision package (http://forums.trossenrobotics.com/datacenter/software-4/hikarivision-169/) that I hope to use with Hikari at some point in the future You can use it to track faces, colors, or selected points. I was originally planning to add a roboard. It simply won't have enough power to do what I want given my current skill set. I will probably add a fit PC to the RX-28 version which is about 2 years away.

DB

darkback2
12-31-2010, 09:38 PM
Here is a video showing the new motions and the vision software I am hoping to run on a PC mounted on Hikari.

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

The software is available for download in the data center. It sends its data out as a UDP signal that you can route to an IP address.

DB

DresnerRobotics
01-02-2011, 11:50 AM
Bigger feet! Still gotta lift those feet off the ground for your forward walk, otherwise it won't go anywhere on a surface that isn't perfectly smooth, and tbh it's not moving much even on those hardwood floors. You're moving your feet about 1/3-1/2 as much as they need to be. Not trying to criticize, just providing some feedback. I haven't really seen your forward walking gait change any considerable amount since you started the project, and that seems to be the area that needs the most work.

darkback2
01-02-2011, 12:10 PM
Bigger feet! Still gotta lift those feet off the ground for your forward walk, otherwise it won't go anywhere on a surface that isn't perfectly smooth, and tbh it's not moving much even on those hardwood floors. You're moving your feet about 1/3-1/2 as much as they need to be. Not trying to criticize, just providing some feedback. I haven't really seen your forward walking gait change any considerable amount since you started the project, and that seems to be the area that needs the most work.

Thanks for the feedback. I appreciate it, and always have. I'm just starting out at this (building biped gates from scratch) so I can use all the feedback I can get. What your not seeing are all of the complete and total failures that have been tried along the way.

As for lifting the feet, Its been difficult to balance lifting the feet vs falling over. This is probably the best argument for doubling the ankle servos or coming up with something stronger. I could try larger feet, and may still in the future, but that is all down the road for another day.

DB

DresnerRobotics
01-02-2011, 12:38 PM
Thanks for the feedback. I appreciate it, and always have. I'm just starting out at this (building biped gates from scratch) so I can use all the feedback I can get. What your not seeing are all of the complete and total failures that have been tried along the way.

As for lifting the feet, Its been difficult to balance lifting the feet vs falling over. This is probably the best argument for doubling the ankle servos or coming up with something stronger. I could try larger feet, and may still in the future, but that is all down the road for another day.

DB

And balance lifting/falling over can many times be solved by using bigger feet. Let me put it this way, your feet are MUCH smaller than even Giger's previous revision- and that was a 25" tall near 6kg robot. Hikari isn't that much smaller, but I'd honestly suggest even doubling her foot size. The way the competition rulesets in korea work, the lighter bots get more leeway in footsize.

Work with larger feet and step down the size eventually once you get the gaits down. Think of them as training wheels. Starting small and working your way up is backwards in terms of development.

UncleBob
01-03-2011, 12:55 AM
Darkside

Can u tell me more about your vision setup from an end consumer point of view. Can I pull the view cam from my pc. Buy a wifi board plug in the USB and run your or some other easy to use software?

darkback2
01-03-2011, 04:27 AM
The software currently is set up to pull the video from whatever camera is hooked up to your PC. I have a mac that has a built in camera. I can run windows on it. If I start up the software without an external camera plugged in, it uses the built in camera by default. I should probably add a menu to the grab option. I should probably also compile the software as a macintosh application. It is really basic. The software is not currently set up to send the video via wifi or internet, but I could and probably should add that too as an update.

The information as in where the detected color, face, or tracking information is sent out as a UDP message I think you just type in an IP address to send the info to. This is so that you can send the information to some other program that can receive UDP messages.

The software uses Open CV (open computer vision.)

Everyone is of course welcome to use the software. It is available for download from the data center. I'll try to put it together as a modularized package where you can only use the parts that you want. I should probably also try to figure out how to get it to work with a wifi camera too.

Hope this helps.

DB

darkback2
01-03-2011, 09:20 PM
http://forums.trossenrobotics.com/datacenter/software-4/simple-color-tracking-patch-170/

This is the link to the color tracking part of the software.

http://forums.trossenrobotics.com/datacenter/software-4/clicktracking-extraction-171/

This is the link to the click tracking part of the software.

DB

UncleBob
01-05-2011, 05:15 AM
Cheers let me play around during the weekend with my Mac isight

darkback2
01-18-2011, 12:49 AM
So for those of you who don't know I have been strongly pressured into entering Mech Warfare this year. I don't want to carry three robots to Robogames, so I'm bringing X, and a Hikari. I'm steeling tyb's backpack idea and I'm making Hikari convertable. The guns will mount under the arms, and the scoring stuff and power stuff will go in a backpack that can be easily removed. So far I have made the mount for the trendnet camera which can be removed using two wing nuts.

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hikariupgrade_-_5.jpg

http://forums.trossenrobotics.com/gallery/files/1/4/9/9/hikariupgrade_-_6.jpg
So far forward walking works, Turning is a bit unstable.

DB

DresnerRobotics
01-18-2011, 04:09 AM
I think brow beaten is perfectly accurate :D

What are friends for if not to push you to a challenge every once in awhile eh?

(I totally pulled the 'why don't you tell your schoolkids you're a quitter, huh?' card) buahahahahaha

darkback2
01-20-2011, 11:29 PM
So the camera scoring system mount is done. I have to finish wiring it up tomorrow. I'm a bit worried about the fact that the scoring system, camera, and servos are sharing a battery, but it is what it is. Here is a video showing the conversion. If I try side stepping with the mech warfare bits on the legs give out, so side stepping is out of the question. I still have to add the guns, but those will plug directly into the arbotix.

http://www.youtube.com/watch?v=F-hJjk_fiSk
Front View
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/2011-01-20_19-26-00_610.jpg
Back View
http://forums.trossenrobotics.com/gallery/files/1/4/9/9/2011-01-20_19-28-40_829.jpg

DB

DresnerRobotics
01-21-2011, 08:53 AM
What type of battery are you using?

darkback2
01-21-2011, 12:18 PM
I'm using the thunderpower 11.1 volt 2100 mah battery. The legs do a whole lot less giving out when its plugged into the battery. The wall plug I use most of the time for gate development and testing is the standard bioloid plug.

I may try to come up with a way to wire capacitors into the top of each limb's power supply in the hopes that that will reduce the instantaneous draw when I move all 21 servos at once under extreme load. These are relatively long legs for AX-12s.

Over time I'll swap up to RX-24Fs and RX-28s at least in the legs...course that brings up its own set of problems. :-)

DB

DresnerRobotics
01-21-2011, 02:56 PM
Do they give out at ALL when plugged in via battery?

You're going to need a pretty beefy wall supply to power that many servos on a spike.

darkback2
05-22-2011, 12:31 AM
http://www.youtube.com/watch?v=DdNs__U0MgI
So it has been a really long time since I posted to this thread, and for that I am sorry...well...sort of sorry. I have been focusing on getting my own personal website up and running...

So Hikari has seen some upgrades since we last talked.

First off, Upgraded legs I got to a point where I was running into thermal shutdown issues for every couple of minutes of run time. So I decided to upgrade the leg servos. I'm now running RX-28s in the legs. For anyone who is wondering...RX-28s are made of awesome.

The hips are now RX-24 Fs. So I will have to see. Running RX-servos all below the waste has made Hikari much neater in terms of wiring, and also allowed me to add servo side brackets to the sides of the hips and the bottoms of the arms. This will make it easier to add the current guns, and the new guns down the road.

I am hoping to upgrade the guns in the next few weeks. I have also had this crazy idea that I need to add some sort of skin.

I sort of also want to get the roboard added so that I can think of either running MAX/MSP or ROS. I'm not sure I have room in my brain for ROS, though all the young people are doing it, and Hikari is a great platform for it...

This robot is teaching me a lot.

DB

gdubb2
05-22-2011, 09:44 AM
Looking good, I especially liked the "fall on your face" routine.. must be a defensive move..

Gary

darkback2
05-22-2011, 09:57 AM
Ha!

Actually it works as a push move if the other robot is in front of you...the other robot stops you from falling over.

I can catch it if I am quick enough by returning to the start pose before it falls over...then it just wobbles. It was supposed to be a bit of an homage to Tyb's light weight 24f fighter.

On another note I'm thinking of using 2 of the AX-12s from the hips as grippers at the ends of the arms, and 2 more for arm rotation...

DB

Stobs
05-23-2011, 10:44 PM
Very nicely done DB, bot'ss looking awesome! :D

darkback2
05-25-2011, 11:22 AM
Thanks Stobs.

Oddly I am running into a design problem that I'm not sure I can muscle my way around. The servos are spending too much time at near stall torque drawing a ton of energy from the battery. This is limiting my run time to about 10 minutes when fully loaded for Mech Warfare. After that the voltage drops and the ankles start giving out. I can stretch the time by a bit by pausing to rest every minute or so.

I had hoped to upgrade the guns, but I am over budget on weight...I might be able to if I drop to just one gun...

I also have plans of adding springs to the ankles to help stiffen things up a bit. Given how over budget this bot is the thought of adding more servos or upgrading the ankles to RX-64s is just out of the question.

Oddly I have to come to grips with the fact that this bot will not be very competitive in its current configuration. I suppose I could always convert it into a quad... Nah.

DB

tom_chang79
05-25-2011, 02:07 PM
looking good darkback... He even has a twin! :)

Slugman
05-25-2011, 07:04 PM
As I've just decided to convert my AX12 Hex to a Biped, my first thought was "Yikes! - RX 24s with PLM legs & he's STILL having performance issues!"
Looking at the pictures though.... Well, um, sorry DB, but I think Hi No Hikari is, um, *cough* a bit chunky.... OK, lets be honest - He's fat. :tongue: Look at all that nice shiny steel!

IMHO, from an armchair expert, if you want a biped to be competative in MW, you might look at Giger, who from all accounts, is fairly well placed in the standings. Giger doesn't actually walk or turn very fast, & has a fair bit of backlash, compared to most of the Hexes, but he can pivot the upper torso & aim well. :happy:

darkback2
05-25-2011, 10:22 PM
Hey Slugman,

So Hikari's frame is made of Aluminum not steel, and yeah...she is a bit on the heavy side for a biped, but most of the weight stems from the camera guns target system larger battery and so on. Without all of that, in Kung fu mode she is about 2.5 KG. Given that she stands almost two feet tall...

that said, Giger uses EX-106s in the legs.

DB

kamondelious
05-25-2011, 10:25 PM
As I've just decided to convert my AX12 Hex to a Biped, my first thought was "Yikes! - RX 24s with PLM legs & he's STILL having performance issues!"
Looking at the pictures though.... Well, um, sorry DB, but I think Hi No Hikari is, um, *cough* a bit chunky.... OK, lets be honest - He's fat. :tongue: Look at all that nice shiny steel!

I dunno that fat is a very good word choice. Heavy, yeah. Phat for sure. Shiny, unquestionably. How do you get fat? Heavy does not equal fat, that's why the body mass index (BMI) rating is not a good indicator of your physical well being. Doesn't look to me from any of the videos I've seen that Hi No Hikari has any issues moving her mass around. Have you seen the body slam video? Seems to me like the added weight is a secret weapon. Sounds to me like the compliance needs more tweaking, possibly an adjustment to the torque settings or maybe even double checking that the servos in the PLM aren't fighting against each other.

:D

Slugman
05-26-2011, 01:59 AM
Lol - & I was thinking that my comment about Gigers movement might raise the ire of people. :veryhappy: All praise the mighty Giger, BTW.
I was using "fat" in terms of having extra weight that was not actually required. I just ran the sums on what my biped should/might weigh though, & it comes in at just above 2kg without wires, so it looks like I might be ankle deep in trouble with my ankle servos.

I always wondered about the PLM legs though, as the typical leg position is usually bent to allow fast lifting of the foot, but that means the servos are under load, while a non-PLM leg is straight, so less strain on the knees & hips, & therefore less power needed just to stand still. kamondelious could have it right, but could it just be a posture problem? Or maybe a combo of the two - PLM servo error + posture? I don't want to drag the PLM discussion into DBs thread though. :happy:

darkback2
05-26-2011, 08:10 AM
@ Slugman,

No worries. A PLM discussion is more than appropriate for this thread given that it is a PLM bot. I know that I am making trade offs in making this a PLM bot. I could drop the whole PLM thing and shorten the legs by half...

As for the trade offs of standing taller vs shorter...If I stand Hikari all the way up she is almost two feet tall and very unstable. Crouching to 90 degrees at the knees lowers her center of gravity and makes her much less tippy. Yes it does put a strain on the knees, but I would rather have a shorter battery life than a robot that is constantly falling over.

Here is where I think I can claim a definite advantage from using a PLM design. Hikari started out using AX-12s with AX-18s in the ankles, and could walk. Not well mind you, and she was in thermal shutdown every 2 minutes or so when fully loaded, but an almost 24 inch bot carrying most of the extra weight for mech warfare was able to walk using just AX servos...That was what sold me on PLM.

Slugman
05-26-2011, 05:13 PM
Yeah, the height of PLM has kept me away from it so far. Have tried to work out how to make it shorter, but no PLM combo I have found has been able to get the thigh or shin shorter than about 10cm (4 inches), apart from using a double-width servo, which isn't PLM. I had wondered why anyone would use one RX24 when they could use 2 AX12s in PLM - cheaper with more torque, but now I know - Total length, & reduction in servo range!

DresnerRobotics
05-26-2011, 05:18 PM
As I've just decided to convert my AX12 Hex to a Biped, my first thought was "Yikes! - RX 24s with PLM legs & he's STILL having performance issues!"
Looking at the pictures though.... Well, um, sorry DB, but I think Hi No Hikari is, um, *cough* a bit chunky.... OK, lets be honest - He's fat. :tongue: Look at all that nice shiny steel!

IMHO, from an armchair expert, if you want a biped to be competative in MW, you might look at Giger, who from all accounts, is fairly well placed in the standings. Giger doesn't actually walk or turn very fast, & has a fair bit of backlash, compared to most of the Hexes, but he can pivot the upper torso & aim well. :happy:

Sadly, Giger is the fastest of any MW biped entry yet, and he uses $5600 in servos in his legs alone.

YouTube - Giger v4 Walking Progress Part 3

darkback2
05-26-2011, 09:11 PM
Giger is awesome. :-(

Noog
05-27-2011, 09:53 AM
Giger is awesome. :-(

Don't sell yourself short either. Hikari is awesome! =)

I think Hikari and Giger are both mechanically sound, but the real struggle now is the software control and gaits.

darkback2
05-27-2011, 11:07 AM
I guess the key thing for us all to remember is the fact that Hikari is really only 8 months old, and is my my first real scratch build biped. The fact that she walks at all at this point is probably something I should view as a major accomplishment.

Where reality sets in is in with the fact that Hikari is a multi-purpose robot. She has competed in both mech warfare and kung-fu. I will also be adding a PC and sensors for doing AI work in the next few months...

DB

DresnerRobotics
05-28-2011, 07:02 PM
I guess the key thing for us all to remember is the fact that Hikari is really only 8 months old, and is my my first real scratch build biped. The fact that she walks at all at this point is probably something I should view as a major accomplishment.

Where reality sets in is in with the fact that Hikari is a multi-purpose robot. She has competed in both mech warfare and kung-fu. I will also be adding a PC and sensors for doing AI work in the next few months...

DB

I wasn't trying to draw a comparison at all. I was just commenting on the state of Mech Warfare bipeds; the unfortunate reality in that cost effective servo technology just isn't here yet.

darkback2
05-28-2011, 07:41 PM
I wasn't trying to draw a comparison at all. I was just commenting on the state of Mech Warfare bipeds; the unfortunate reality in that cost effective servo technology just isn't here yet.

I wasn't responding to you...I was responding to Noog...so...No worries.

In actuality I do draw comparisons between Giger and Hikari. Giger like capability is a "someday goal" for hikari. Think of bots like Giger, Rook's Pawn, Crunk and the like as inpiration. I would really like to get Hikari walking as smoothly as Giger, and realistically I hope to someday have a version of Hikari with a similar payload capacity. I have a lot of respect for you, Andrew, in seeing Giger through to the level where he is now, and hope to someday have something similar to hang my hat on.

I was just pointing out the fact that Hikari is relatively young, and that I am proud of how far I have come personally as a roboticist. Not long ago I was trying to build a hexapod using 5 phidget 4s and a bunch of MG-995s....

DB

DresnerRobotics
06-03-2011, 09:39 PM
Not long ago I was trying to build a hexapod using 5 phidget 4s and a bunch of MG-995s....

DB

It's okay, I caught Matt trying to do this when I first started working here. :p

darkback2
06-21-2011, 12:21 AM
OK...so it has been a long time since I have updated this thread. Robogames 2011 was a lot of fun...and in the same breath it was disappointing. Hikari's feet stuck to the mech warfare arena and her ankles were twisted by the force of her trying to walk. Hardcore was fun, but I only won because the demented baby bot's gun exploded, and kung-fu...well lets just say I felt bad and had to apologize to the guy who kicked the snot out of me for not making it interesting.

When I got home I made some changes that I hope pay off this next time around.

1) New feet and shoes. 1 pair for high traction situations, and another for low traction...At least I hope that is how it works. The feet are bigger which should make tipping over a bit harder.

2) I replaced the rubber bands in the legs with springs. This actually gives the robot the ability to jump which is pretty cool, though it is a bit less stable.

3) New arms...There was something about Hikari...I couldn't put my finger on it, but there was something wrong with her that was driving me crazy. I finally figured out that her arms were simply too short. I lengthened them up and all is well in that department.

4) New Guns. Tybes and Cire had these AEG gearbox guns. They seamed like all the rage, and I quickly saw that I was totally out gunned. These things pack a punch and have a really high rate of fire. Now, after a few weeks of trial and error, I got a couple hopups printed out that fit the guns, and build some hoppers that mount under the arms. I even managed to keep the small barrels that Gdubb machined for me. It took 4 prints before we got it right. Special thanks to the Engineering class at the high school where I work for printing the boxes out for me and making the adjustments as we went on. I posted the parts on the trossen database.

Now the issue is the weight of BBs. I only have room for 200 in each gun. I have them set to automatically shut off after 1 second of shooting, and they get out about 3 shots every pull...so...I have less than 100 shots per 15 minute match. I guess I better make them count. I'm thinking of setting things up so that I have 2 different rates of fire. So a 1 off and then an empty my gun mode...Not really sure how that will work, but I will probably need to stretch things out a bit.

I also want to set up a shot counter so that I know when I am getting low on ammo.

I think it might be advisable to give bipeds the ability to reload 7 minutes into a match...but that is more a bit of me whining which I really can't stand for.

You can see pictures of my progress here.

https://picasaweb.google.com/117645864319745324150/DropBox?authkey=Gv1sRgCOfYpNiurri-uQE

DB

Stobs
08-13-2011, 08:55 PM
Just saw your new vid on YT - HNH's lookin' good with the new threads DB! ;)

darkback2
08-18-2011, 10:24 AM
3404 34053406

Hi No Hikari trimmed out for Mech Warfare, Kung Fu, and as a PC based humanoid.

It has been a while since I have updated this thread. I have been publishing a lot about Hikari, only I have been doing so over at my personal website (http://www.teammoistpanties.com).

Making a biped has proven to be a real challenge. I've gone through several different foot designs (each bigger than the previous,) and several servo upgrades (AX-12s to RX-28s in the legs.)

So finally I made one upgrade which seams to have made all the difference in the world. I upgraded Hikari's ankles. I am now running dual RX-28s in the ankles. Earlier I was having trouble because Hikari's ankle servos would overshoot the position and not have the strength to recover. She would tip over sideways, especially on rough surfaces. By both truly lifting the feet, and doubling up the ankle servos (eliminates servo backlash and increases torque) I think I have solved the majority of the problem.

Before:

http://www.youtube.com/watch?v=DdNs__U0MgI
You can see that Hikari isn't really lifting her feet when walking. The shuffling works ok on smooth surfaces, but on rough surfaces the robot tips forward when she tried to shuffle her feet. With the week servos she also tipped over to the side frequently.

After:

http://www.youtube.com/watch?v=XrG5TqMXBcM
In this video Hikari is clearly lifting her feet off of the ground. This allows her to walk on surfaces like plywood without much trouble. It also makes the walking smoother and straiter.

I added a really slow walking gate for when Hikari is in full mech warfare dress. I'll be spending the next few weeks trying to get her walking gates to be more and more stable so that she can carry an increased payload. I may have to have her drop down and rest turning off her servos periodically to prevent overheating.

I also managed to add a backwards walking sequence. I realized that I couldn't just run the forward walk backwards...(it has to do with shifting weight before you lift your foot...

DB

DresnerRobotics
08-18-2011, 10:57 AM
Dude. This is a massive improvement. Awesome!

I'd slow down that side step if you're taking that large of steps, otherwise go with shorter quicker sidestep motions.

darkback2
08-18-2011, 11:14 AM
Funny you say that...On a smooth surface the current side step works fine...I need to tune it for rough surfaces..or make smaller steps. Funny but I am finally in a position to take your advice. Up until now Hikari just wasn't strong enough to do things like lift her feet...

DresnerRobotics
08-18-2011, 12:31 PM
I've told you for a long time those ankles were a problem, good to see 'em fixed. :P

If you want mobility for kung-fu, a quick short-step side-step movement is key. Take a look at my Giger Fu videos for a prime example of what that side step should look like, mine was based on what I saw the top 'bots in Korea doing. Literally there is only 2 inches or so of lateral movement on each foot, and I used a total of 2 key frames for each motion.

helishaun
08-18-2011, 03:16 PM
:happy: I think it is fantastic that you were able to greatly improve your robot’s walking gait. Congratulations! I hope to see more.

Gertlex
08-18-2011, 04:45 PM
Looked pretty good... but the sidestepping was a bit nervewracking ;)

elaughlin
08-23-2011, 05:51 AM
I found a competitor for your cross-dressing robot event, DB. You are going to need to add luscious hair once Hikari has a head I guess...


http://www.youtube.com/watch?v=XxNEy-ijjIU

I think Hikari would kick its a$$ though.

darkback2
08-31-2011, 08:39 PM
I got to go down to FONCO last week to help out with the MORAV (http://www.MORAV.net) project. That place is freaking awesome! If your not familiar with the MORAV project then get the graphic novel and familiarize yourself. FONCO makes miniatures, models, and other cool as heck things including the Mech Warfare buildings. Lets just say it was an awesome four days. The last day I got to work on a part for Hikari. My wife and I made her a helmet/cockpit for Mech Warfare. Now I really need to make a vacuum former of my own. Here is what I got done. I didn't get to take any pictures at FONCO...these were taken at my house.

Here is the CAD file I worked from to make the foam slug.
3445

Here is one side of the slug. I made it in two halves which I then glued together. I vacuum formed over it twice to try to hide some of the flaws in the original.
3446

I painted it.
3447

Then I cut out the hole in the front for the vacuum formed acrylic screen.
3448

Finally here it is on Hikari.
3449
DB

gdubb2
08-31-2011, 09:26 PM
Looks great DB.
You are having entirely too much fun this summer..You're not going to want to go back to teaching..

Vac formers are fun.. I made mine using the elements from an old theater pizza oven, and an old shop vac.. works great.

Gary

cire
08-31-2011, 10:25 PM
I like it, but please tell me it is polycarbonate, not acrylic? We both know what happened last time..

I wish I had summer vacation...

elaughlin
08-31-2011, 11:35 PM
What happened with acrylic???

darkback2
08-31-2011, 11:50 PM
sorry...it is acrylic, that was all Fon had laying around. I'm going to make a new one using the mold once work starts up again. I'll make it out of thicker plastic and I'll try to find some polycarb that I can make the face plate out of.

The acrylic lens shield I used last year shattered when Andrew shot it. Its actually sort of cool...

DB

cire
09-01-2011, 05:26 PM
What happened with acrylic???

http://img824.imageshack.us/img824/1026/1000615t.jpg

Acrylic is strong but very brittle, especially in impact loading situations, like with airsoft BB's. Polycarbonate on the other hand, is what it used for bullet proof glass, and is very impact resistant.

elaughlin
09-01-2011, 10:49 PM
Haha. That picture is great.

darkback2
09-01-2011, 10:52 PM
Hikari's ankle servos just quit again. That makes twice in one summer. Luckily I have 2 back ups that I can use while I send these ones back to robotis...I'm not sure what I am doing wrong to make this happen. I have replaced the wires and checked the code.... :-(

darkback2
09-02-2011, 01:13 AM
Hikari can walk and turn on carpet! This is important because the floor in Mech Warfare tends to be rather high in traction. I still have to get this to work with Hikari fully loaded for mech warfare...

As for the ankle servos...Thank goodness Fergs suggested that I pick up a couple spares. I just did a time test...45 seconds to walk a foot in mech warfare mode...I think I'll have to speed that up if I plan on qualifying. course there is always side stepping...


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

Gertlex
09-04-2011, 10:37 PM
DB, you should put airbags in the dress so Hikari doesn't fall over ;D

darkback2
09-06-2011, 01:19 AM
If I told you that I was seriously considering mounting the guns on a walker would you make fun of me? Actually...I was thinking a walker with the guns mounted on it, and maybe some balloons to help with the weight problems...

DresnerRobotics
09-06-2011, 10:46 AM
I would suggest loosening up your compliance margins if you're running into overheating/shutdown issues.

darkback2
09-10-2011, 09:55 PM
Thanks for reminding me about the compliance fix. I have that in the hikari key program but not the commander one. I've updated it to make it better.

I've found some problems that mean I probably won't be dressing up Hikari again any time soon. The hip brackets were torn nearly all the way through. I'm not sure when this started happening, but it was making the legs behave differently enough that problems started creeping into the walking gates, especially the side steps and jump turns. So she would sidestep well enough to the right, only to not be able to side step to the left at all. It was really frustrating until I looked up her skirt and found the problem.

Well...I've only got 7 months and 1 week and 3 days to fix the problem...Better get on it!

Db

Toshi
09-11-2011, 04:18 PM
What an amazing project!! :) I admit I didnt read all 13 pages, but I did read a lot. A Biped seems to be amazingly complex, but very impressive. Good luck with getting it working! I cant wait to see more videos :D

Gertlex
09-13-2011, 11:32 AM
Show us some pics of these failed brackets when you get a chance :)

darkback2
09-14-2011, 09:29 PM
34733474

So As you can see, Hikari's hip brackets were nearly torn all the way through. I couldn't see this happening, nor do I know when it happened because of the dress she wears...So...for a little while at least I'm going to have to let her run around naked to "air things out". In anycase, I have some professionally machined brackets coming in a few weeks. Not being one to wait...I made some new brackets out of aluminum that is twice as thick while at work yesterday. Here they are installed.

3475
A part of me feels silly for thinking the old brackets would last any time at all. The new ones are much beefier, though I may re-make them with gussets between the different bends to help strengthen them a bit more. I'll have to really carefully check over my code tonight, and I'll try to get some video of her walking some time in the next few days. All in all things are looking up though. Hopefully this will solve some of the problems that I was having.

DB

darkback2
09-17-2011, 01:10 PM
Here is my commander mode code...I'll have to clean it up a bit, but I think people can tell what is going on.


#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04a.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();
int mode;
int panh;
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables
int mode = 0;
int panh = 512;
int pany = 512;
// crouch down slowly (currently seams the AX-12 side goes slow RX side goes really fast)
bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
for (int i = 1; i < 21; i++) { // set the compliance of all servos
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);
}
//end new stuff from jess
}

void loop(){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//change modes by pressing button R1
if(command.buttons&BUT_R1) {
// taunt as visual indicator
mode += 1;
//resets count bringing the mode back to 0
if(mode == 4){
mode = 0;
//this does not appear to be happening...When you get to mode 0 you are also supposed to crouch.
bioloid.playSeq(Defence01);}
bioloid.playSeq(Taunt01);
}
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//Push position
bioloid.playSeq(Push01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L6) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L5) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L4) {
//Neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Gunplay);
}if(command.buttons&BUT_L6) {
//Collaps
bioloid.playSeq(Collaps01);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L4) {
//Start getting up
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
if(mode == 3){
int panh = 512;
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(slowwalk01);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Qrt01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Qlt01);
}
// connect the head turn to the right thumb
else if (command.lookH > 50){
panh += 50;
SetPosition(23, panh);
}else if (command.lookH < -50){
panh += -50;
SetPosition(23, panh);

//Here go the button commands
}if(command.buttons&BUT_R2) {
//shoot right gun
ax12SetRegister2(25, AX_GOAL_SPEED_L,312);
}if(command.buttons&BUT_R3) {
//turn off right gun
ax12SetRegister2(25,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L6) {
//shoot left gun
ax12SetRegister2(26, AX_GOAL_SPEED_L,362);
}if(command.buttons&BUT_L5) {
//turn off left gun
ax12SetRegister2(26, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Side step right
bioloid.playSeq(Sr01);
}if(command.buttons&BUT_LT) {
//Side step left
bioloid.playSeq(Sl01);
}
}
}
}// end else

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
} //end loop

darkback2
11-06-2011, 08:55 PM
So one of the perks of being a FIRST robotics mentor is that I get access to some of the sweet stuff like a copy of Autodesk inventor. A while ago I installed a roboard in Hikari, but I ended up having more problems with it than it was ever going to be worth. That in part had to do with my own personal limitations as a programmer...I'm sure the roboard is a perfectly good SBC. In anycase in a fit of frustration I put it in a box and sent it away.

Now all the cool guys are installing FIT-PCs in their robots...so I figured why not. Especially when I found out how light the fit is once it is taken out of its case. I then got to learning Autodesk and designed a FIT-PC/Arbotix box for Hikari. I sent it out to Big Blue Saw and got the real deal a few days later...talk about cool!So the pros...

The roboard was a funny shape, and required weird wires hanging all over the place. This made it so that I had to put it in a really big backpack sort of setup. The fit, while having a larger footprint than the roboard is a lot cleaner. I was able to fit it into one square box that is only slightly bigger than the hips were. (4 RX-24fs in a square formation) This puts the weight of the Fit directly over the feet, while the backpack with the roboard sort of hung off of the back which changed the robots COG a bit making it less stable while walking.

This also cleaned up the wiring. Before the Arbotix was in the head, above the waste rotation. This meant that there were a lot of wires coming down from the head to the servos below the waste. By adding the box for the FIT-PC there is only one wire that now extends above the waste rotation. This is split out to all of the servos via a hub on the back of the shoulders.

The cons...

There aren't many. Adding this box does raise the COG a bit. It raises the arms and arm brackets higher as well as the added weight of the box itself...which is partially offset by the loss of the head.

I'm not sure about how much of a pounding I want this robot to take now that it will have a FIT-PC in its chest...

The robot is also about 1.5 CM taller...which in the scheme of things is not bad at all. Currently her full height is just over 53 cm or (21 inches) She actually crouches a bit bringing her standing height to just under 46 cm (18 inches)

I haven't got a good way to weigh her at the moment.

I will have to adjust some of her poses. Surprisingly not as many as I originally thought though so I'm in better shape than I could be.

In any case, Here are the pictures and video.

358535843586


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

DB

darkback2
11-23-2011, 09:47 PM
So here is what I am trying to do. Not sure if it is even possible. I am trying to stitch together my commander code and my keyboard control code with little success. so in the void loop I am calling command.begin(38400) which ties up the serial port. I have commander button 1 set to change the mode. (remap the buttons) Mode 4 is loaded with keyboard commands. but the serial port is tied up by command.begin. I can't seam to get command.end and serial.begin(38400) to run as a part of changing modes. This would basically make it so that I could control the robot using either a keyboard or the commander using the same sketch. Any ideas? I know its hard to find, but I tried to notate in the code the different ideas that I have already tried.

DB



#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04a.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;

//----------------------Start Void Loop----------------------
void setup(){

//open the serial port

command.begin(38400);
//serial.begin(38400); tried this, it didn't do anything.

delay(2000); // recommended pause

//make defaults for variables
int mode = 0;
int panh = 512;
int panadd = 0;
int pany = 512;
int panyadd = 0;

//set up arbotix 2
// delay(1000);
// do a search for devices on the RX bus, default to AX if not found
// int i;
// for(i=0;i<AX12_MAX_SERVOS;i++){
// dynamixel_bus_config[i] = 1;
// if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
// dynamixel_bus_config[i] = 0;
// }
// crouch down slowly (currently seams the AX-12 side goes slow RX side goes really fast)
bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
// }
// set the compliance of all servos
for (int i = 1; i < 21; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);
}
//end new stuff from jess
}

void loop(){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-------change modes by pressing button R1---------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;

//tried it here, but I get a error about it not being in the void.setup. Can you have two void setups?
//if(mode == 4){
//command.end;
//serial.begin(38400)
}

//resets mode to 0
if(mode == 5){
mode = 0;

bioloid.playSeq(Defence01);}
bioloid.playSeq(slowjump);
}
//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//Push position
bioloid.playSeq(Push01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}

//--------get up mode-----------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L6) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L5) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L4) {
//Neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}

//---------cheesy moves mode-(soccer)-----------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Gunplay);
}if(command.buttons&BUT_L6) {
//Collaps
bioloid.playSeq(Collaps01);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L4) {
//Start getting up
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}

//------------mech warfare mode ---------------
if(mode == 3){
// int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(slowwalk01);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Qrt01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Qlt01);
}
//------------Head pan code ---------------------
//At current this does not work.
else if (command.lookH > 30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}
//------------Gun Tilt code ---------------------
//At current this does work.
else if (command.lookV > 30){
pany += panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
//trying to get servo 18 to move in the opposite direction
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
}else if (command.lookV < -30){
pany -= panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
//Here go the button commands
}if(command.buttons&BUT_R2) {
//shoot right gun
ax12SetRegister2(27, AX_GOAL_SPEED_L,312);
}if(command.buttons&BUT_R3) {
//turn off right gun
ax12SetRegister2(27,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L6) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,362);
}if(command.buttons&BUT_L5) {
//turn off left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Side step right
bioloid.playSeq(Sr01);
}if(command.buttons&BUT_LT) {
//Side step left
bioloid.playSeq(Sl01);
}
}
// keyboard mode Currently Untested
if(mode == 4){

char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == 'p') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == 'p')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0)
{
if(startByte == 'r') //if I press r relax the legs
{
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w') //if I press w walk forward
{
bioloid.playSeq(simplewalk01);
}
else if(startByte == 'W') //if I press W walk forward slowly
{
bioloid.playSeq(slowwalk01);
}
else if(startByte == 'X') //if I press X walk backword slowly
{
bioloid.playSeq(Rest01);
}
else if(startByte == 's') //if I press s jump
{
bioloid.playSeq(Jump01);
}
else if(startByte == 'S') //if I press S big jump
{
bioloid.playSeq(Jump02);
}
else if(startByte == 'x') //if I press x defensive pose
{
bioloid.playSeq(simpleback02);
}
else if(startByte == 'g') //if I press s get up facedown
{
bioloid.playSeq(Facedown01);
}
else if(startByte == 'G') //if I press s get up facedown
{
bioloid.playSeq(Facedown02);
}
else if(startByte == 'b') //if I press b roll over
{
bioloid.playSeq(Backup01);
}
else if(startByte == 'B') //if I press b roll over
{
bioloid.playSeq(Backup02);
}
else if(startByte == 'q') //if I press q turn left
{
bioloid.playSeq(Qrt01);
}
else if(startByte == 'a') //if I press a sidestep left
{
bioloid.playSeq(Sl01);
}
else if(startByte == 'e') //if I press e turn left
{
bioloid.playSeq(Qlt01);
}
else if(startByte == 'd')//if I press d sidestep right
{
bioloid.playSeq(Sr01);
}
else if(startByte == 'j') //if I press j attack left
{
bioloid.playSeq(LeftAttack01);
}
else if(startByte == 'l') //if I press l attack right
{
bioloid.playSeq(RightAttack01);
}
else if(startByte == 'k') //if I press k right punch attack front
{
bioloid.playSeq(Push01);
}
else if(startByte == 'K') //if I press k right punch attack front
{
bioloid.playSeq(Push02);
}
else if(startByte == 'i') //if I press i front push
{
bioloid.playSeq(AttackFront01);
}
else if(startByte == 'z') //if I press z rest
{
bioloid.playSeq(Defence01);
}
else if(startByte == 'n') //if I press n get into dropped push mode
{
bioloid.playSeq(DownPush01);
}
else if(startByte == 'N') //if I press n Downpush 02
{
bioloid.playSeq(DownPush02);
}
else if(startByte == 'm') //if I press n rest attack
{
bioloid.playSeq(RestAttack01);
}
else if(startByte == '`') //if I press ` set mode to 0
{
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}

}
}
}// end else

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
} //end loop

tician
11-24-2011, 01:05 AM
Going from the Commander.h/.cpp files on the arbotix google code site, there is no Commander::end() function. And Commander::begin(baud) ignores the user input baud rate to simply call "Serial.begin(38400)". The loop() function calls Commander::ReadMsgs() every time it loops, which then checks Serial.available() for available data and then reads the Serial buffer (received data is only able to be read once) for a valid commander packet. You have a bit of a problem with this, because your code checks for 'keyboard' mode only once and this is only after it has received a valid message/packet from the commander. I would move the entire code segment for 'if (mode ==4)' outside of the 'if (command.ReadMsgs()>0)' segment, so that it does not require a packet from the commander to receive from the keyboard.

Also, how many XBee's are you using and how are they matched up? Does the text editor/IDE that you use check for/highlight matched braces? Best advice I can give is pick an indentation/brace convention and stick with it religiously. Does not matter which you choose, but stop the mix'n'match stuff. After going through and fixing indentation/alignment, there appeared to be an extra close brace that ended loop() prematurely (matches fine with the addition of a 'mode' check before the 'command.ReadMsgs()>0' check).

Reformatted, modified, and untested code attached (the code tags don't always work quite as I would like).

EDIT: Failed to listen to my own advice when I forgot a conditional to prevent Commander::ReadMsgs from clearing the serial buffer when in keyboard mode.

darkback2
11-24-2011, 12:06 PM
Thanks for going over this tician,

As for the indentation problem, I apologize. That comes from switching back and forth between making changes in the arduino window, using PSP pad, and making quick fixes in the posting window here. Its also because about 5 different people have had input on this code at this point.

Currently the plan is to use 3 xbees. One on the arbotix one on the commander, and one on the FIT. the hope is that I can write a "personality" mostly a series of behaviors that use asci commands to control the body. My issue is that I also want to be able to control the robot through the commander.

Again, Thanks for the help I'll test the code out now.

DB

darkback2
11-24-2011, 12:13 PM
OK, so the code won't compile. All of the delay() come up as "delay not declared" Then the check serial comes up as an error also.

DB

darkback2
11-24-2011, 12:14 PM
Combinedtician.cpp: In function 'void setup()':
Combinedtician.cpp:23: error: 'delay' was not declared in this scope
Combinedtician.cpp: In function 'void loop()':
Combinedtician.cpp:70: error: 'Serial' was not declared in this scope
Combinedtician.cpp:74: error: 'HIGH' was not declared in this scope
Combinedtician.cpp:74: error: 'digitalRead' was not declared in this scope
Combinedtician.cpp:74: error: 'digitalWrite' was not declared in this scope
Combinedtician.cpp:78: error: 'Serial' was not declared in this scope
Combinedtician.cpp:80: error: 'Serial' was not declared in this scope
Combinedtician.cpp:93: error: 'Serial' was not declared in this scope
Combinedtician.cpp:179: error: 'HIGH' was not declared in this scope
Combinedtician.cpp:179: error: 'digitalRead' was not declared in this scope
Combinedtician.cpp:179: error: 'digitalWrite' was not declared in this scope


These are the specific errors

darkback2
11-24-2011, 01:29 PM
So this is the working commander code.



#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;

//----------------------Start Void Loop----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables
int mode = 0;
int panh = 512;
int panadd = 0;
int pany = 512;
int panyadd = 0;

//set up arbotix 2
// delay(1000);
//do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}
// crouch down slowly (currently seams the AX-12 side goes slow RX side goes really fast)
bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
// }
// set the compliance of all servos
for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);
}
//end new stuff from jess
}

void loop(){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-------change modes by pressing button R1---------

if(command.buttons&BUT_R1) {
// taunt as visual indicator
mode += 1;
//resets count bringing the mode back to 0
if(mode == 4){
mode = 0;
//this does not appear to be happening...When you get to mode 0 you are also supposed to crouch.
bioloid.playSeq(Defence01);}
bioloid.playSeq(slowjump);
}
//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//Push position
bioloid.playSeq(Push01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}

//--------get up mode-----------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L6) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L5) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L4) {
//Neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}

//---------cheesy moves mode-(soccer)-----------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Gunplay);
}if(command.buttons&BUT_L6) {
//Collaps
bioloid.playSeq(Collaps01);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L4) {
//Start getting up
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}

//------------mech warfare mode ---------------
if(mode == 3){
// int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(slowwalk01);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Qrt01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Qlt01);
}
//------------Head pan code ---------------------
//At current this does not work.
else if (command.lookH > 30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}
//------------Gun Tilt code ---------------------
//At current this does work.
else if (command.lookV > 30){
pany += panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
//trying to get servo 18 to move in the opposite direction
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
}else if (command.lookV < -30){
pany -= panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
//Here go the button commands
}if(command.buttons&BUT_R2) {
//shoot right gun
ax12SetRegister2(27, AX_GOAL_SPEED_L,312);
}if(command.buttons&BUT_R3) {
//turn off right gun
ax12SetRegister2(27,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L6) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,362);
}if(command.buttons&BUT_L5) {
//turn off left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Side step right
bioloid.playSeq(Sr01);
}if(command.buttons&BUT_LT) {
//Side step left
bioloid.playSeq(Sl01);
}
}
}
}// end else

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
} //end loop


and here is the keyboard code that I used.



/* ================================================== ======================== */
/* HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer to control servos using the arbotix.
/* Send data in the form of p (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it. */
/* If you use this you want to change the "iPoses01.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */

#include <ax12.h>
#include <BioloidController.h>
#include "Hikarikey04a.h" // exported from PyPose
BioloidController bioloid = BioloidController(1000000);

void setup()
{
// need to start the serial port
Serial.begin(38400);

// recommended pause
delay(100);
delay(1000);
//set up arbotix 2
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
// set compliance of all servos?
for (int i = 1; i < 29; i++) { // det the compliance of all servos
ax12SetRegister(i, 0x1A, 6);
delay(3);
ax12SetRegister(i, 0x1B, 6);
delay(3);
}
//end new stuff from jess
}
// stand up slowly
bioloid.loadPose(up01);
bioloid.readPose();
bioloid.interpolateSetup(500);

while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}

void loop() {
//create variables
char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == 'p') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == 'p') {
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0)
{
if(startByte == 'r') //if I press r relax the legs
{
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w') //if I press w walk forward
{
bioloid.playSeq(simplewalk01);
}
else if(startByte == 'W') //if I press W walk forward slowly
{
bioloid.playSeq(slowwalk01);
}
else if(startByte == 'X') //if I press X walk backword slowly
{
bioloid.playSeq(Rest01);
}
else if(startByte == 's') //if I press s jump
{
bioloid.playSeq(Jump01);
}
else if(startByte == 'S') //if I press S big jump
{
bioloid.playSeq(Jump02);
}
else if(startByte == 'x') //if I press x defensive pose
{
bioloid.playSeq(simpleback02);
}
else if(startByte == 'g') //if I press s get up facedown
{
bioloid.playSeq(Facedown01);
}
else if(startByte == 'G') //if I press s get up facedown
{
bioloid.playSeq(Facedown02);
}
else if(startByte == 'b') //if I press b roll over
{
bioloid.playSeq(Backup01);
}
else if(startByte == 'B') //if I press b roll over
{
bioloid.playSeq(Backup02);
}
else if(startByte == 'q') //if I press q turn left
{
bioloid.playSeq(Qrt01);
}
else if(startByte == 'a') //if I press a sidestep left
{
bioloid.playSeq(Sl01);
}
else if(startByte == 'e') //if I press e turn left
{
bioloid.playSeq(Qlt01);
}
else if(startByte == 'd')//if I press d sidestep right
{
bioloid.playSeq(Sr01);
}
else if(startByte == 'j') //if I press j attack left
{
bioloid.playSeq(LeftAttack01);
}
else if(startByte == 'l') //if I press l attack right
{
bioloid.playSeq(RightAttack01);
}
else if(startByte == 'k') //if I press k right punch attack front
{
bioloid.playSeq(Push01);
}
else if(startByte == 'K') //if I press k right punch attack front
{
bioloid.playSeq(Push02);
}
else if(startByte == 'i') //if I press i front push
{
bioloid.playSeq(AttackFront01);
}
else if(startByte == 'z') //if I press z rest
{
bioloid.playSeq(Defence01);
}
else if(startByte == 'n') //if I press n get into dropped push mode
{
bioloid.playSeq(DownPush01);
}
else if(startByte == 'N') //if I press n Downpush 02
{
bioloid.playSeq(DownPush02);
}
else if(startByte == 'm') //if I press n rest attack
{
bioloid.playSeq(RestAttack01);
}
//special commands from keyboard that sets specific servos to shooting.
else if(startByte == '.') //if I press "." fire the guns
{
ax12SetRegister2(27, AX_GOAL_SPEED_L,512);
ax12SetRegister2(28, AX_GOAL_SPEED_L,512);
}
else if(startByte == ',') //if I press "," stop shooting
{
ax12SetRegister2(27,AX_GOAL_SPEED_L,0);
ax12SetRegister2(28,AX_GOAL_SPEED_L,0);
}
else if(startByte == '"') //if I press """ fire the right rocket
{
ax12SetRegister2(29, AX_GOAL_SPEED_L,512);
}
else if(startByte == ':') //if I press ":" turn off right rocket
{
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}
else if(startByte == '}') //if I press "" fire the left rocket
{
ax12SetRegister2(30, AX_GOAL_SPEED_L,512);
}
else if(startByte == '{') //if I press "{" turn off the left rocket
{
ax12SetRegister2(30,AX_GOAL_SPEED_L,0);
}
//end of keyboard stuff.
}
//

if(bioloid.playing > 0)
{
// continue sequence
bioloid.play();
}

}// end else
} // end loop()


Now I am trying to combine the two...I seam to have messed up the code that I came up with to do this.

darkback2
11-24-2011, 02:45 PM
Ok, so I figured something out, tician's code was in the same folder as the old code, so the compiler was getting hung up on ticians code. I got things sort of sorted, course the code still doesn't work. I tried to go through with pspad to clean up the indenting...not sure if I made things more or less readable. Given that what I put together is literally just me copying and pasting the format for mode 4 switches. To be honest I think I like the second format better than the first. Here is what I have at this point.

DB



#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;

//----------------------Start Void Loop----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables
int mode = 0;
int panh = 512;
int panadd = 0;
int pany = 512;
int panyadd = 0;

//set up arbotix 2
// delay(1000);
//do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}
// crouch down slowly (fixed by getting arbotix2 code to work)

bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
// }

// set the compliance of all servos
for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);
}
//end new stuff from jess
}

void loop(){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-------change modes by pressing button R1---------

if(command.buttons&BUT_R1) {
// taunt as visual indicator
mode += 1;
//resets count bringing the mode back to 0
if(mode == 4){
mode = 0;
//this does not appear to be happening...When you get to mode 0 you are also supposed to crouch.
bioloid.playSeq(Defence01);}
bioloid.playSeq(slowjump);
}
//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);

//----------------Here go the button commands------------------------------
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//Push position
bioloid.playSeq(Push01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}

//--------get up mode-----------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);

//------------------Here go the button commands--------------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L6) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L5) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L4) {
//Neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}

//---------cheesy moves mode-(soccer)-----------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
//Here go the button commands
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Gunplay);
}if(command.buttons&BUT_L6) {
//Collaps
bioloid.playSeq(Collaps01);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L4) {
//Start getting up
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}

//------------mech warfare mode ---------------
if(mode == 3){
// int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(slowwalk01);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Qrt01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Qlt01);
}

//------------Head pan code ---------------------
else if (command.lookH > 30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}
//------------Gun Tilt code ---------------------
else if (command.lookV > 30){
pany += panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));

//-------trying to get servo 18 to move in the opposite direction---------------
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
}else if (command.lookV < -30){
pany -= panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//shoot right gun
ax12SetRegister2(27, AX_GOAL_SPEED_L,312);
}if(command.buttons&BUT_R3) {
//turn off right gun
ax12SetRegister2(27,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L6) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,362);
}if(command.buttons&BUT_L5) {
//turn off left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Side step right
bioloid.playSeq(Sr01);
}if(command.buttons&BUT_LT) {
//Side step left
bioloid.playSeq(Sl01);
}
}

//------------------keyboard mode -------------------------
if(mode==4){

//---------------allows control of specific servos -----
//create variables
char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == 'p') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == 'p') {

else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0)
{
if(startByte == 'r') //if I press r relax the legs
{
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w') //if I press w walk forward
{
bioloid.playSeq(simplewalk01);
}
else if(startByte == 'W') //if I press W walk forward slowly
{
bioloid.playSeq(slowwalk01);
}
else if(startByte == 'X') //if I press X walk backword slowly
{
bioloid.playSeq(Rest01);
}
else if(startByte == 's') //if I press s jump
{
bioloid.playSeq(Jump01);
}
else if(startByte == 'S') //if I press S big jump
{
bioloid.playSeq(Jump02);
}
else if(startByte == 'x') //if I press x defensive pose
{
bioloid.playSeq(simpleback02);
}
else if(startByte == 'g') //if I press s get up facedown
{
bioloid.playSeq(Facedown01);
}
else if(startByte == 'G') //if I press s get up facedown
{
bioloid.playSeq(Facedown02);
}
else if(startByte == 'b') //if I press b roll over
{
bioloid.playSeq(Backup01);
}
else if(startByte == 'B') //if I press b roll over
{
bioloid.playSeq(Backup02);
}
else if(startByte == 'q') //if I press q turn left
{
bioloid.playSeq(Qrt01);
}
else if(startByte == 'a') //if I press a sidestep left
{
bioloid.playSeq(Sl01);
}
else if(startByte == 'e') //if I press e turn left
{
bioloid.playSeq(Qlt01);
}
else if(startByte == 'd')//if I press d sidestep right
{
bioloid.playSeq(Sr01);
}
else if(startByte == 'j') //if I press j attack left
{
bioloid.playSeq(LeftAttack01);
}
else if(startByte == 'l') //if I press l attack right
{
bioloid.playSeq(RightAttack01);
}
else if(startByte == 'k') //if I press k right punch attack front
{
bioloid.playSeq(Push01);
}
else if(startByte == 'K') //if I press k right punch attack front
{
bioloid.playSeq(Push02);
}
else if(startByte == 'i') //if I press i front push
{
bioloid.playSeq(AttackFront01);
}
else if(startByte == 'z') //if I press z rest
{
bioloid.playSeq(Defence01);
}
else if(startByte == 'n') //if I press n get into dropped push mode
{
bioloid.playSeq(DownPush01);
}
else if(startByte == 'N') //if I press n Downpush 02
{
bioloid.playSeq(DownPush02);
}
else if(startByte == 'm') //if I press n rest attack
{
bioloid.playSeq(RestAttack01);
}

//end of keyboard stuff.
}
}//end else
}
}// end mode = 4

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
}
} //end loop

tician
11-24-2011, 06:57 PM
Sorry for sounding in any way angry in my previous post. It was not my intention, but it does come off that way a bit. I simply meant to emphasize that the easier it is to see where everything starts/stops, the easier it is to find simple errors (also I have mild OCD, so I like everything nice and neat or else it feels a bit like nails on a chalkboard).

Finally got around to installing a copy of the Arduino IDE (arduino-0023), and the only errors it has thrown for the code I uploaded earlier are related to the lack of the pose header file (and bad file naming - alphanumeric names only? wtfits?). Is the problem more that it does not compile, or that it does not behave as you intended? About your last code upload specifically, the "if (mode==4)" conditional is back under the "if (command.ReadMsgs()>0)" conditional. With it located where it is, hikari will check for a valid Commander packet every time loop() starts anew. Unless your keyboard code is sending a new Commander packet and then the keyboard input, it will never progress far enough into the code block to ever evaluate the "if (mode==4)" conditional.

I asked earlier about the XBee's because of bad experiences with the Robotis ZIG-100/110. Although I do have experience with the ZIG-100/110 in broadcast mode, I am not very familiar with using more than two XBee's. How are they matched/paired? I'm guessing the XBee on the commander and connected to the PC have the exact same self-ID and remote-ID, and to switch between the two you just power one down so they don't fight.

darkback2
11-24-2011, 07:53 PM
No Worries Tician,

Thanks again for even looking at it.

I went back to what I had been trying because I couldn't get the code you uploaded to compile. It was in part user error. Arduino wouldn't compile it because of the extension .pde is what it is looking for.

I opened up your code in pspad and changed the extension to .PDE and as promised it compiles just fine.

What it does now is:

When I run it the robot seams to just be switching mode repeatedly as soon as the commander gets turned on.

The idea is to run using the commander 90% of the time. I want to be able to switch over to an autonomous mode without loading new software. The autonomous mode would run on an on board PC and would control the arbotix through ascii commands. so I set the robot to mode 4, turn off the commander, turn on the fit-pc and off it goes. That is the idea at least.

Thanks again for the help.

DB

tician
11-24-2011, 08:50 PM
When I run it the robot seams to just be switching mode repeatedly as soon as the commander gets turned on.


That is likely due in no small part to my being an idiot.

One potential problem I created (by moving the keyboard related code out of the Commander code) that may or may not be affecting you is the transition from keyboard mode (mode==4) to any other mode. Line 166 checks for the '`' input from the keyboard and increments mode. If the first packet received from the Commander has "BUT_R1" as pressed, then the "mode" variable gets incremented again (line 185) and the following "if(mode==5)" check (line 196) to reset "mode" to zero will fail (because mode==6).

Another possible problem is on line 201. I think that is where I somehow lost track of an open/close brace as "bioloid.playSeq(slowjump);" is supposed to be a visual indicator that the mode has been changed, is it not? And the various Commander inputs are not to be acted upon unless "bioloid.playing==0", right?

Hopefully this version will be a bit better behaved.

darkback2
11-24-2011, 09:36 PM
Tician,

Thank you for your help today. The code is working. I have to rewrite the program that controlled specific servos using the mouse before I can test that part, that is what the whole cmd bit was about, but the keyboard commands work great.

I'll go through in pspad and comment and clean up as well as I can.

Thanks again!

DB

darkback2
11-24-2011, 10:45 PM
So here it is...I tried to clean it up a bit and notate it.



/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it. */
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* Tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */


#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;

//----------------------Start Void setup----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables
int mode = 0;
int panh = 512;
int panadd = 0;
int pany = 512;
int panyadd = 0;

//-----------------set up arbotix 2---------------------------------------------
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}

//--------------------crouch down slowly----------------------------------------

bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
//---------------set the compliance of all servos-------------------------------

for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);
}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------

//--------------------start void loop-------------------------------------------

void loop(){
// keyboard mode
if(mode == 4){

//--------------------takes commands from the keyboard--------------------------
//format # servonumber "p" four didget position---------------------------------

char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == '#')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(slowwalk01);
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
}else if(startByte == 's'){ //if I press s jump
bioloid.playSeq(Jump01);
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump02);
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(simpleback02);
}else if(startByte == 'g'){ //if I press s get up facedown
bioloid.playSeq(Facedown01);
}else if(startByte == 'G'){ //if I press s get up facedown
bioloid.playSeq(Facedown02);
}else if(startByte == 'b'){ //if I press b roll over
bioloid.playSeq(Backup01);
}else if(startByte == 'B'){ //if I press b roll over
bioloid.playSeq(Backup02);
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(Qrt01);
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(Sl01);
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(Qlt01);
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(Sr01);
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
}else if(startByte == 'k'){ //if I press k right punch attack front
bioloid.playSeq(Push01);
}else if(startByte == 'K'){ //if I press k right punch attack front
bioloid.playSeq(Push02);
}else if(startByte == 'i'){ //if I press i front push
bioloid.playSeq(AttackFront01);
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
}else if(startByte == '`'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}
}



if (mode != 4){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-----------------change modes by pressing button R1---------------------------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;

//------------------------resets mode to 0--------------------------------------
if(mode >= 5){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}

//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//Push position
bioloid.playSeq(Push01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
//-----------------------------end kungfu mode----------------------------------

//------------------------------get up mode-------------------------------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);

//----------------------Here go the button commands-----------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L6) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L5) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L4) {
//Neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
//----------------------------end get up mode-----------------------------------

//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Gunplay);
}if(command.buttons&BUT_L6) {
//Collapse
bioloid.playSeq(Collaps01);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L4) {
//Start getting up
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}
//--------------------------end cheesy moves mode ------------------------------

//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
//int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(slowwalk01);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Qrt01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Qlt01);
}

//----------------------------Head pan code ------------------------------------

else if (command.lookH > 30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}

//------------------------------Gun Tilt code ----------------------------------

else if (command.lookV > 30){
pany += panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
//trying to get servo 18 to move in the opposite direction
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
}else if (command.lookV < -30){
pany -= panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));

//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_R2) {
//shoot right gun
ax12SetRegister2(27, AX_GOAL_SPEED_L,312);
}if(command.buttons&BUT_R3) {
//turn off right gun
ax12SetRegister2(27,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L6) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,362);
}if(command.buttons&BUT_L5) {
//turn off left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Side step right
bioloid.playSeq(Sr01);
}if(command.buttons&BUT_LT) {
//Side step left
bioloid.playSeq(Sl01);
}
}
}
}
}

if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program

darkback2
11-25-2011, 12:57 AM
OK...tested and working! Thanks Tician. One thing is that it means I have to have a computer handy because once you switch to mode 4 you can't use the commander to switch to mode 0, but all in all it works! I'll post a vid of it working tomorrow...I also want to see if I can't get the vision tracking working too.

tician
11-25-2011, 12:02 PM
Having the keyboard program send a full Commander packet prior to each and every keyboard input would allow the "mode==4" code chunk to be re-inserted into the rest of the Commander code. Since that portion of code checks for the mode change button press before doing anything else, you would still be able to use the Commander to override the keyboard (and still allow the keyboard to exit via '`'). You would have to add a small delay between sending the Commander packet and sending the keyboard input because Commander::ReadMsgs() calls Serial.flush(), which clears the serial receive buffer on the arbotix. Added a slight delay in the "mode==4" area just before checking the serial buffer for incoming data. Hopefully it will prevent any missed keyboard data as long as the delay between sending the Commander packet and sending the keyboard data is shorter than this delay.

Don't think I made any new problems.



/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it. */
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */


#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;

//----------------------Start Void setup----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables
int mode = 0;
int panh = 512;
int panadd = 0;
int pany = 512;
int panyadd = 0;

//-----------------set up arbotix 2---------------------------------------------
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}

//--------------------crouch down slowly----------------------------------------

bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
//---------------set the compliance of all servos-------------------------------

for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);
}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------

//--------------------start void loop-------------------------------------------

void loop(){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-----------------change modes by pressing button R1---------------------------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;

//------------------------resets mode to 0--------------------------------------
if(mode >= 5){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}

//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//Front Attack
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//Push position
bioloid.playSeq(Push01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
//-----------------------------end kungfu mode----------------------------------

//------------------------------get up mode-------------------------------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);

//----------------------Here go the button commands-----------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L6) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L5) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L4) {
//Neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
bioloid.playSeq(LeftAttack01);
}
}
//----------------------------end get up mode-----------------------------------

//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Gunplay);
}if(command.buttons&BUT_L6) {
//Collapse
bioloid.playSeq(Collaps01);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L4) {
//Start getting up
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}
//--------------------------end cheesy moves mode ------------------------------

//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
//int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(slowwalk01);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Qrt01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Qlt01);
}

//----------------------------Head pan code ------------------------------------

else if (command.lookH > 30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -30){
panh += panadd;
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}

//------------------------------Gun Tilt code ----------------------------------

else if (command.lookV > 30){
pany += panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
//trying to get servo 18 to move in the opposite direction
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));
}else if (command.lookV < -30){
pany -= panyadd;
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 156));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 156)- 1024) * -1));
SetPosition(26, (((pany/1024.00) * 312.00)+ 456));

//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_R2) {
//shoot right gun
ax12SetRegister2(27, AX_GOAL_SPEED_L,312);
}if(command.buttons&BUT_R3) {
//turn off right gun
ax12SetRegister2(27,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L6) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,362);
}if(command.buttons&BUT_L5) {
//turn off left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//return to neutral position
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_RT) {
//Side step right
bioloid.playSeq(Sr01);
}if(command.buttons&BUT_LT) {
//Side step left
bioloid.playSeq(Sl01);
}
}

//--------------------keyboard mode---------------------------------------------
if(mode == 4){

//--------------------takes commands from the keyboard--------------------------
//format # servonumber "p" four didget position---------------------------------

char cmd[8];
int startByte = 0;

// Wait for keyboard input to be sent after Commander packet
delay(500); // likely can be shorter

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == '#')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(slowwalk01);
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
}else if(startByte == 's'){ //if I press s jump
bioloid.playSeq(Jump01);
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump02);
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(simpleback02);
}else if(startByte == 'g'){ //if I press s get up facedown
bioloid.playSeq(Facedown01);
}else if(startByte == 'G'){ //if I press s get up facedown
bioloid.playSeq(Facedown02);
}else if(startByte == 'b'){ //if I press b roll over
bioloid.playSeq(Backup01);
}else if(startByte == 'B'){ //if I press b roll over
bioloid.playSeq(Backup02);
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(Qrt01);
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(Sl01);
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(Qlt01);
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(Sr01);
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
}else if(startByte == 'k'){ //if I press k right punch attack front
bioloid.playSeq(Push01);
}else if(startByte == 'K'){ //if I press k right punch attack front
bioloid.playSeq(Push02);
}else if(startByte == 'i'){ //if I press i front push
bioloid.playSeq(AttackFront01);
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
}else if(startByte == '`'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}
}
}
}

if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program

darkback2
11-25-2011, 02:52 PM
Given that I'm not sure how to format a commander packet using MAX I'm going to leave things as they are. I'm planning to install a FIT-PC in Hikari's chest. This is a way for me to be able to control the robot without depending on the fit. I will probably just add a botton command that resets the mode before it gets to the keyboard mode so that I don't need to power up the fit, and don't have to wait 500 ms between keyboard commands. (not sure I have that figured out right.)

Thanks again,

DB

darkback2
12-11-2011, 12:54 AM
3667
Hi No Hikari is now sporting Dynamixel RX-64s for ankles.

I was running into a problem where after less than 10 minutes Hikari would start tipping over after just a few steps. Basically throughout her charge cycle the walking gaits would slowly degrade until it was a bit like she was drunk. As voltage dropped the Dynamixel RX-28s were unable to keep up with the load of her body swinging back and forth. I tried increasing the compliance, but that resulted in the drunk effect happening sooner rather than later in terms of her ankles.

After some consideration I decided to make two adjustments to try and stabilize things.

I upgraded the ankle servos from double RX-28s to double RX-64s. Now on the surface this is complete overkill, but in reality Hikari's legs are just over a foot long when she is at a 90 degree crouch. She weighs 9.4 pounds when fully loaded and most of her body weight is at or above her waist including her batteries (mounted on her butt) her controller/PC/ and her arms which hang down from nearly 18 inches above the ankles. When Hikari walks she has most of her body mass swinging from side to side. While I can imagine some sort of foot pressure based walking gate, where as the weight pendulums she takes a step when the pressure on the down foot reaches a certain value...That might make faster walking possible.

I also made the feet 1 cm wider and 1/2 cm longer. The length was more a consequence of the longer stock RX-64 bracket I'm using in the ankle.

I moved the RX-28s that I took out of the old ankles and moved them up to replace the RX-24fs in the hips. I noticed some slop in pulling the legs back together. The 28s seam to have taken care of most of that.

I still have to install the harddrive and software, design a decent battery cover for her 12 volt battery (for the AX servos, PC, and Arbotix) and armor or body panels.

3665
Hikari's new feet and ankles. The RX-64s are huge when compared with RX-28s, but they are also rock solid.

3666
I had this box cut out by big blue saw. There are mounts on the front and sides. The arbotix is on the bottom and the Fit-PC is on the top,


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

DresnerRobotics
12-16-2011, 12:26 PM
Are you all RX-28s in the rest of the legs now then?

darkback2
12-16-2011, 07:41 PM
@ tybes...the only RX-64s are in the ankles...RX-28s in the legs 18fs in the chest and AX-12s in the arms. She is sort of like a pyramid in that sense...

Gertlex
12-16-2011, 10:26 PM
@ tybes...the only RX-64s are in the ankles...RX-28s in the legs 18fs in the chest and AX-12s in the arms. She is sort of like a pyramid in that sense...
Except Cleopatra was attractive... unlike that doll thing :D

darkback2
12-18-2011, 08:05 AM
@ Gertie...She grows on you.

Here is a pic of her standing next to a stock bioloid my students put together.

3675

Given that she is almost twice as tall I figure the ankle problem should have been expected...

DB

darkback2
12-24-2011, 02:37 PM
So I get to go to Fonco next week. I'll hopefully get to contribute to MORAV in some way or another. While I'm there I'm also hoping to sneak in a bit of creative work on Hikari. In planning for the trip I got started with some spare foam from another project and started making a helmet.
3678

I've sort of sketched up two different ideas.

3677
The first idea was a militaryesq theme. Green and grey with a blue helmet, sort of to break things up. I wanted to have a faux air intake on the front of the upper chest, but the blocky body leaves the bottom of the chest sort of lacking. Here is where Mech Warfare sort of sucks. The front of the chest block is a perfect mounting spot for the front target panel, The problem being that the robot would look like butt if it had a big empty square on its front.

I thought about breaking up the front block somehow and added what looks like the nosecone from a jet to the belly area. This will make mounting the target panel a little bit harder, so I'm going to have to spend time figuring that part out.

Another problem is that I have to be careful because the head pans down, the vents on the chest can greatly limit the motion of the head.

3679
The second idea is the one I seriously think I'm going with. It is completely impractical from a mech warfare standpoint. Faded white/grey matte with giant air vents on either side of the head. The front chest plate has a molded feminine form and terminates in a downward facing jet nosecone. I figure there should be thigh plates which I'll model once I get there, and arm panels even though the arms won't be there for mech warfare.

In this case I really don't know where I'm going to mount the front panel. I might make the nosecone attach using velcro so that I can pull it off. I can then make a metal bracket for holding the front panel that won't detract too much from the overall look.

I have to make back battery cover anyway...

The one problem with this is that the xbee for the computer mounts on the shoulder. I might drop it inside of the upward facing vents, which also presents its own challenges...

Course...the challenges are the best part!

DB

gdubb2
12-24-2011, 08:32 PM
Cute DB.. I like the white one better than the green one..

Gary

darkback2
12-25-2011, 01:19 PM
Thanks Gdubb

Yeah...I'm sort of in love with the white one myself...I figured I could make the nosecone attach with velcro, and if I cut the plastic right the seam will be relatively hidden...That way I can make a target plate mount for the front. I'm planning to mount the side panels on the arms, though I am worried about the proximity to the guns... Meh...always something to worry about I guess.

elaughlin
12-25-2011, 06:53 PM
Make a harness for the side and back ones. So they dont have to attach to the arms, and can be easily removed, but it still rotates with the upper body and everything. Just an idea.

darkback2
12-25-2011, 08:27 PM
Ha Ha...I have this vision in my head of Hikari wearing a crossing guard vest with the sensors on it.

darkback2
12-29-2011, 03:31 PM
My trip to Fonco was a total success...I got the armor completed. I ended up going with a cross between the two designs, and I'm pretty happy with the way that it all turned out.

I used the original helmet mold, and added a small piece of foam to the top of the cowling to make give the helmet adequate clearance. I made the front and side molds using a combination of wood foam, and bits of plastic Fon had laying around. All in all the parts were much more simple than I thought they would be. I melted the various parts together using welding solvent.

Paint was pretty simple, I used grey primer then went over it with white. Then I painted lines by hand and dusted it with white primer. I might clear coat the whole thing when I get home, but the plastic needs to stay relatively flexible in order to go on and come back off easily. (I need access to some components.

I am concerned about the Fit PC. It is inside the body cavity and from what I have heard can put off quite a bit of heat. This may result in the plastic settling down, or it could result in the plastic warping badly. I may add a bit of Velcro to help the plastic panels stay put, so that if they do warp they will trend in my benefit instead of pulling away from the body. I may also drill tiny vent holes in the sides. No idea how I might ever install a fan. Where I didn’t plan ahead was in how I was going to mount the target panels. There are mounting holes under the chest panel that I can tie into. I have a mount that I am printing that is the opposite of the Bioloid F3 bracket. I will probably print that and mount it on the upper chest. That way I can make a target plate bracket for the front target plate at least. There are brackets low on the sides that I can use for side plates, and I may have a spot on the back over the batteries that I can use for the back plate. The sides and back won’t turn with the torso, only the front will, not really sure how that works out with the rule sets. The other option would be to build a sort of vest that the target panels could mount on, but I’m really not convinced about that one.

Here are the pics.

36853686368736883689

DB

gdubb2
12-29-2011, 03:48 PM
Looking good DB, love the beady eyes looking out from the helmet..

G

cire
12-30-2011, 12:49 PM
That is quite impressive, just make sure the helmet is always on so we don't have to see the horrors within.

darkback2
02-24-2012, 08:14 AM
3775

So almost two months is definately too long to go between posts on a project like this.

I've been really busy over the past few and have had little time to do much, I changed the arm position during the walking gates because the arms were getting damaged when she fell down. With the new ankles (Dual RX-64s) she doesn't fall down as much, but she is a big girl and falling strips the gears on her AX-18F shoulders faster than...well...you get the idea.

I sort of want to switch up to RX-24fs in the shoulders, but that would create a total wiring mess that I am not exactly ready to deal with, as well as a reworking of parts that I just got how I like them. (I think I'm saying I'll save that until after robogames, and next year.

For now I want to spend some serious time this Saturday working on making her walking gates cleaner and more stable.

I am having a few issues...For some reason I still can't get the ROS/Pypose deal working. I haven't been able to since I had to reformat my harddrive. I know I must have missed a step at some point, but it is sort of driving me crazy.

Also, I now have the camera installed and working with the FIT PC to send video...The problem is that the video and the xbee seam to have a lot of signal overlap. Though now that I think about it, I wonder if it isn't heat that is the issue. I'll have to play around with that later. Basically when both are running they both seam to slow down in responsiveness. But if I run one or the other things seam to work relatively well. I dropped the video down to 60/80 and 20 frames a second for testing purposes...Not high enough to use, but for a test seam fine. I'll probably end up boosting the resolution and dropping the frame rate tomorrow.

I am thinking that maybe I can switch to an external xbee antenna...

DB

darkback2
02-25-2012, 08:15 PM
Pypose/Ros issues:

So Fergs fixed me up. I somehow missed the step where I reset the baud rate in the ROS sketch. Once I set that to 38400 pypose worked just fine. I used that to fix a couple things with Hikari's walking patterns. Her side step was a bit messy and didn't always result in her moving in the right direction. With Pypose working I was able to fix her sidestep in about 10 seconds.

The xbee issue is still there. I am now starting to think I may have somehow managed to damage the xbee antenna's. the range has deteriorated severely. I'll have to run some tests with another xbee to see if I can figure things out.

DB

darkback2
04-01-2012, 01:03 AM
I've been distracted by FIRST Robotics for the past few months...Now that that is over I have been back at work on Hikari. I noticed while shooting some video that she was really tippy. I have adjusted her Mech Warfare gates so that she is a lot more stable. (Mostly shifting her weight forward and lowering her knees by just a tidge.) This also makes her much more stable while walking over BBs.
http://www.youtube.com/watch?v=BGKaiLNOsMg&feature=g-upl&context=G2b04b20AUAAAAAAABAA

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


I haven't had much of a chance to work with her via WiFi. I am currently using a USB webcam and broadcasting the video over an adhoc network. The video is round and shrouded because of the hole in the top of her head that the camera is peeping out through. I added a wide angle lens which has helped a lot though visibility is still a pretty big problem. I have the resolution set to 120 by 90 which seams to work relatively well. I can change the frame rate dynamically which is nice.
http://www.youtube.com/watch?v=qAHZ1j_2WI8&feature=g-upl&context=G2bd265fAUAAAAAAAAAA

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


I'm hoping to add some sighting software over the next few days. I am going to use tracking software and the computer I look through. I can set it to output the XY coordinates of the tracked object. I'll work to match the XY coordinates of the target with known aiming points. I'll have to have 4...one point for each gun both standing and crouching.


DB

elaughlin
04-01-2012, 08:31 AM
Darkback - Hikari looks really good. Not only her outside, but she looks battle ready. Your walking sequence looks good man, I know its a little slow, but your doing a biped, and it walks well. I didn't see any tip overs unless you edited the video a lot.

What are you using to put in tracking abilities?

And I think I am going to have the same view as yours, my camera is stuck within a little hole, and it is probably going to cut out some of my vision (ehh, it will seem more real life mech-ish).

Awesome DB. I am a fan of how far Hikari has come.

ramos96
04-01-2012, 03:59 PM
Very nice project, just finished reading all 17 pages, hoping to post up my build project soon when ever I get back Brasil one day ( i have been here all of 2012 so far due to work).

I am intending to use my bioloid kit I got for my b-day last year as the base and luckily I have a 3d printer (printrbot) on the way to make up any parts I may need.

Humbly,
ramos96

darkback2
05-01-2012, 11:03 PM
Hi No Hikari and I had a great time at robogames. We had a great fight with Yoshihiro from robot japan and a team from Mexico...I love rockets.

Since I got back I have been busy making Hikari New feet. One strange issue is that she was having difficulty turning in the arena. She used a jump turn technique in which she jumped, shifted her feet, and then slid them to even again...Funny thing is at home she was able to use this method to turn on carpet...At Robogames she could only turn in one direction quickly, and in the other direction she turned really slowly. This was a bit frustrating.

I added bearings to the outside of each foot. In this way she can tip up her ankles and slide her feet on the bearings. She then drops down onto the flat part of her feet to slide back to even which makes her turn. The cool part is that I think I can make her skate on the bearings if I play my cards right.
3975 3976

Next up Hikari has gotten a lot heavier. In order to help her with her stand sequences I decided to put those RX-24fs that I have kicking around to use in her shoulders. One thing about metal brackets in her shoulders is that they have a tendency to bend when she falls over. Plastic brackets have been great because they flex and don't break. Problem being that RX-24/28 brackets are metal by default. I am getting these shoulder brackets printed. I am orienting the print so that the strands run the full length of the piece...I tried printing them normaly but they were too brittle in the uprights.

I also thickened them up and beefed up some parts of the brackets.
3977

Here is what the chest will look like without the armor.
3978

I'm getting the chest parts cut on a waterjet by a friend. I may make metal shoulder brackets in addition to the plastic ones if the plastic ones don't work out.

DB

darkback2
05-02-2012, 10:08 PM
I got the shoulder brackets printed...The first set were printed with the grain running the wrong way and snapped pretty easily. The set I am currently using were printed on their sides so that the grain runs the length of the part instead of across it. The part seams a bit stronger printed this way. I am going to have a set cut out of metal also. The original set of shoulder brackets were made of metal. At the time Hikari was a whole lot les stable. The metal brackets got bent every time she fell over.

I'm having them cut out of thicker aluminum with bends running the length of the sides which should firm things up a lot.

3980

DB

darkback2
07-15-2012, 01:49 PM
4071 4072

I've always wanted to do more than one thing with my bots. In this case, Hikari is being modified for experiments in robot human interaction. Mostly I'm workng on a "robot opera" of sorts in which Hikari will be one of three robot performers. To that end each of the three robots will have a pico projector mounted somewhere in or on it. In Hikri's case I roughed this shoulder mount using a bioloid bracket. I'm hoping that falling won't crush it.

I'm going to be using the fit-PC in her chest to have her play short images and video's through the projector doing live video mixing. In any case, here is what it looks like with the lights on.

I am currently waiting for the HDMI cable that I'll use to connect it. I'll try to get some pics tonight showing what it looks like with the lights off.

DB

darkback2
07-26-2012, 06:11 PM
So Hikari is going to be one of three charaters in a robot opera based on the Oedipus greek tragidies. I'm starting with King Laius and ending with Antigone (played by Hi No Hikari.)

I will be building two more PC / Arbotics based robots for this and I'm running a kick start to help fund it.

Here is the link!

DB's Kickstarter (http://www.kickstarter.com/projects/1036258198/the-seed-a-robotic-multimedia-opera-based-on-oedip)

It is a techno Operaelectronic. Here is a link to the background music (http://soundcloud.com/che-edoga) I have put together so far. I will post software to this thread as it gets written.

DB

elaughlin
07-26-2012, 10:20 PM
Who are the other 2 robots in the opera.

I think this is an extremely wild idea but also neat at the same time. Plus I have liked some of your music that you have posted for it already as well. So I will support you DB. I really want to see what comes of this. Good luck my friend.

darkback2
07-28-2012, 01:50 AM
Thank you for the support E.

The other two robots are going to be Hikari esque from the waist up, and rolling chassis from the waste down. If all goes according to plan then I should be down at Fon's hopefully over spring break to make the plastics for the bodies. I'm hoping to make them taller, about Two feet so as to overcome the whole issue with robots being too small to really be entertaining. The onboard projectors should also help. Hikari's looks pretty cool, and I a working on openGL stuff for that.

I have to put writing music on hold though because my laptop is currently missing its screen, :-(. Meh, what can you do.

DB

darkback2
08-11-2012, 03:11 AM
http://www.youtube.com/watch?v=YZy6iejr4VM

Here is an update video. Most of the work that I have been doing has gone towards writing music and making small gadgets and programs for the Opera. I did manage to make new shoulders for Hikari. The plastic printed ones shattered the first time she fell on hard wood.

4112 4113

This is getting out of hand in a hurry.

darkback2
08-25-2012, 04:31 PM
4132
I came up with this as an added project for my robotics students. They are going to create anatomy posters featuring 5 robots throughout the semester. The anatomy project is basically a poster in which the students outline the major features of the robot and write a one or two paragraph description. Figured I woudl do this with Hikari as an example.

Over the past few days I upgraded her legs and ankles. I was having trouble in two areas. First off, her feet were dragging on the ground, and second off, even though the legs were laser cut and should have been really close to the same length, I found that the left leg was just a bit shorter than the right leg. Under closer inspection I found that the leg brackets on the left leg had bent along the rear linkages. This resulted in Hikari leaning backwards and really meant that her servos have not been benefiting from the whole PLM structure. I upgraded the bushings that she uses and am replacing the bolts that I used for the rear linkages to fix the problem. Her legs are much stiffer now and she stand completely upright.

Finally I added a gripper to her right arm. The gripper isn't really functional yet, and will result in an overload problem without some modification to the code. I also have to add software for controlling the arm directly during telepresence control.

Here are some videos of her walking and stuff.


http://www.youtube.com/watch?v=TmwRUWQqQaQ&amp;list=UUdzU2TukTwvXVYDSBkzzSUQ&amp; index=2&amp;feature=plcp


http://www.youtube.com/watch?v=QPqbdj7_q_0&amp;list=UUdzU2TukTwvXVYDSBkzzSUQ&amp; index=1&amp;feature=plcp

DB

darkback2
09-16-2012, 07:02 PM
A bunch of years back I took a short week long materials science course at a local community college. The course was geared towards teaching teachers how to teach materials science, and one of the things we got to do was play with plastics. One of the tools we got to use was a vacuum former. I think pretty much since then I've wanted a vacuum former of my own. We have one at work, it is in the metal shop, and the metal shop teacher keeps it locked away in a cabinet that I don't have keys to. Its not that I can't use it...its more that it isn't readily available to me. I can't "use it any time I want". A few summers ago I spent a few days hanging out at Fonco. There I did the first body work for Hikari. It ended up being this really bulky space man looking thing. At the time Hikari was using the trendnet...All in all it was pretty bad, and because I made it out of thin plastic the whole thing crumbled pretty quickly. Also in short order after I made her first body kit, I completely revamped Hikari so the kit didn't fit any more. I added a box containing an arbotix 2 and a fitPC. While at Fonco I made a new body kit. This one was less bulky, but was very boxy. I think I was just trying to wrap Hikari's metal bits in plastic. In anycase, the body kit didn't really turn any heads.

4155

A few weeks ago I stumbled onto a website that talked about making a vacuum former using an old toaster oven. I went out to the local thrift store and found a pile of them to choose from. I ended up making a vacuum former that has a 16 * 14 inch table to work with.

41564160

I pretty much immediately set out to make Hikari a new body. I started out with the slugs from her old body, and molded over them with clay. For the helmet I started out with a face plate from a bioloid premium. I turned it upside down and molded over that with clay and ended up with a helmet that looks a lot like something from HALO.

41574158

This time around I used 3/32nds thick plastic. This stuff is pretty thick, unfortunately it is also heavy. The clay definatly rounds out her body quite nicely. A little too nicely in parts. :-)

4159

Finally went off to the local craft store for paint in a variety of colors. I am still not good at designing color schemes, but all in all I am pretty happy with how it all came out.

4161

DB

darkback2
10-12-2012, 11:51 PM
So I've spent the past week or so modifying Hikari's arm. One of my students is working on a robotic arm, and as part of the project did some research into grippers. Hikari's original gripper was pretty ineffective outside of very specific situations. It had a flat surface for clamping things, and two "fingers" that hung over the front of the arm, but the angle involved meant that the fingers could only interact with things that were directly in front of the robot at arm level.
4215
The other thing is that as the fist moved along with the servo bracket making the gripper seam kind of like a game of hungry hungry hippos.
4216
This is the gripper we ended up going with on the arm my student is designing. It has three prongs that pinch together.
Again, the gripper isn't perfect, and the things she interacts with are going to have to be pretty specific to this gripper.
42174218

DB

darkback2
10-14-2012, 12:32 AM
So Its pretty amazing how much Hikari's code has increased from what it was when I first started. I've spent the past two days working on the code so that I can control the arm using the commander and the computer. The computer was easy because I could just control the servos directly using the serial port. I then translated my MAX/MSP code into arduino code.

I'm working on a way to have the gripper postition be based on the temperature of the servo. If the temperature rises above a certain threshold I want the servo to back off.

Here is what I have so far. I've tried greatly reducing the servo torque on the gripper servo and also tryied upping the compliance. The gripper can now hold a blue racket ball for about a minute before it goes into thermal shutdown. If the code worked then I don't think it would go into thermal shutdown at all, so I must be doing something wrong.

Any ideas?



/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it.
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* Tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */


#include <ax12.h>
#include <SharpIR.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// add sharp sensor
SharpIR myIR = SharpIR(GP2Y0A02YK,1);
//SharpIR face01 = SharpIR(GP2Y0A02YK,3);
//SharpIR face02 = SharpIR(GP2Y0A02YK,4);



// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;
int faceDown;
int faceUp;
int screwUp;
int armx;
int armxadd;
int army;
int armyadd;
int armz;
int armzadd;
int gripper;
int gripperadd;
int grippertemp;
//----------------------Start Void setup----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables and take initial readings
int mode = 0;
panh = 512;
panadd = 0;
pany = 512;
panyadd = 0;
//These are the arm movement vairables
int armx = 512;
int armxadd = 0;
int army = 512;
int armyadd = 0;
int armz = 512;
int armzadd = 0;
int gripper = 712;
int gripperadd = 0;
//These are the sensor inputs for the tilt sensors
int faceDown = 3;
int faceUp = 4;
pinMode(faceDown, INPUT);
pinMode(faceUp, INPUT);
faceDown = analogRead(3);
faceUp = analogRead(4);
//this takes the initial reading of the gripper servos
grippertemp = ax12GetRegister (26, 0x2B, 1);
//-----------------set up arbotix 2---------------------------------------------
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++)
//this would set the RX servos as 0 - 16
//for(i=0;<16;i++0
{
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}

//--------------------crouch down slowly----------------------------------------

bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
//---------------set the compliance of all leg servos-------------------------------

for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);

}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------

//--------------------start void loop-------------------------------------------

void loop(){
// keyboard mode
if(mode == 5){

//--------------------takes commands from the keyboard--------------------------
//format # servonumber "p" four didget position---------------------------------

char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == '#')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
// this should read the IR sensor
Serial.println(myIR.getData());
//this should read the tilt sensor
Serial.println(digitalRead(3));
//this should read the other tilt sensor
Serial.println(digitalRead(4));
//this should read the temp of the gripper servo
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(Msimplewalk01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
Serial.println(myIR.getData());
Serial.println(digitalRead(2));
Serial.println(digitalRead(3));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 's'){ //if I press s go to base fire pose
bioloid.playSeq(Mechfire);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(Msimpleback02);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'g'){ //if I press g get up facedown
bioloid.playSeq(Facedown01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'G'){ //if I press G get up facedown
bioloid.playSeq(Facedown02);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'b'){ //if I press b get up from back
bioloid.playSeq(Backup01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'B'){ //if I press B finish getting up
bioloid.playSeq(Backup02);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(Srt02);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(MSl01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(Slt02);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(MSr01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'k'){ //if I press k right push front
bioloid.playSeq(Push01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'K'){ //if I press k right finish push front
bioloid.playSeq(Push02);
Serial.println(myIR.getData());
//Serial.println(face01.getData());
//Serial.println(face02.getData());
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(startByte == 'i'){ //if I press i front punch
bioloid.playSeq(AttackFront01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (26, 0x2b,1));
}else if(startByte == '>'){ //if I press > shoot right gun
ax12SetRegister2(29, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}else if(startByte == '<'){ //if I press < shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}else if(startByte == '"'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}
}



if (mode != 5){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-----------------change modes by pressing button R1---------------------------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;

//------------------------resets mode to 0--------------------------------------
if(mode >= 6){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}

//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Msimpleback02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Slt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Srt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//paused side attack
bioloid.playSeq(FrontAttack01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
SetPosition(26, 712);
}if(command.buttons&BUT_L6) {
//attackfront
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(26, 524);
delay(200);
bioloid.playSeq(LeftAttack01);
delay(400);
SetPosition(26, 712);
}
}
//-----------------------------end kungfu mode----------------------------------

//------------------------------get up mode-------------------------------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Slt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Srt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//----------------------Here go the button commands-----------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L5) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L4) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L6) {
//Neutral position
mode -= 1;
}if(command.buttons&BUT_RT) {
//Right attack

bioloid.playSeq(RightAttack01);

}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(26, 524);
bioloid.playSeq(LeftAttack01);
delay (400);
SetPosition(26, 712);
}
}
//----------------------------end get up mode-----------------------------------

//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01armup);
SetPosition(26, 712);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(Msimpleback02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(RestSkate01);
}if(command.buttons&BUT_L6) {
//Collapse
SetPosition(26, 512);
delay (200);
bioloid.playSeq(Cruel01);
delay (800);
SetPosition(26, 512);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
SetPosition(26, 524);
SetPosition(27, 612);
delay (200);
bioloid.playSeq(GoodRobot01);
delay (800);
SetPosition(26, 712);
SetPosition(27, 512);
}if(command.buttons&BUT_L4) {
//Start getting up
SetPosition(27, 412);
delay (200);
bioloid.playSeq(BadRobot01);
delay (700);
SetPosition(27, 512);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}
//--------------------------end cheesy moves mode ------------------------------

//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
//int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Slt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Srt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}

//----------------------------Head psn code ------------------------------------

else if (command.lookH > 25){
panh += (2 * panadd);
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -25){
panh += (2 * panadd);
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}

//------------------------------Gun/head Tilt code ----------------------------------

else if (command.lookV > 25){
pany += (2 * panyadd);
SetPosition(18, (((((pany/1024.00) * 312.00)+ 412)- 1024) * -1));
//trying to get servo 18 to move in the opposite direction
SetPosition(17, (((pany/1024.00) * 312.00)+ 62));
SetPosition(27, (((pany/1024.00) * 312.00)+ 312));
}else if (command.lookV < -25){
pany -= (2 * panyadd);
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 62));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 412)- 1024) * -1));
SetPosition(27, (((pany/1024.00) * 312.00)+ 312));

//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//shoot right gun
ax12SetRegister2(29, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_LT) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R3) {
//shoot right cannon
ax12SetRegister2(40, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(40, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//shoot left cannon
ax12SetRegister2(41, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(41, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R2) {
//rest01
SetPosition(26, 524);
delay (400);
bioloid.playSeq(Cruel01);
delay (1000);
SetPosition(26, 712);
}if(command.buttons&BUT_L5) {
//Side step left
bioloid.playSeq(Mechfire);
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 3;
}
}//-------------------------armcontrolmode -----------------------------------
if(mode == 4){
//this sets the initial value of the variables
int panadd = (-float(command.lookH)/20);
int armxadd = (-float(command.lookH)/20);
int panyadd = (5);
int armyadd = (5);
int armzadd = (5);
int gripperadd = (5);
SetPosition(17, 262);
SetPosition(19, 262);
SetPosition(21, 762);
SetPosition(23, 562);
//this drastically reduces the torque for the gripper servo(not sure this helps)
ax12SetRegister(26, 0x0E, 2);
//sets the compliance really high for the gripper servo (also not sure this helps)
ax12SetRegister(26, 0x1B, 100);
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Slt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Srt02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//----------------------------ArmXCode ------------------------------------

}else if (command.lookH > 25){
armx += (2 * armxadd);
SetPosition(25, (((armx/1024.00) * 400.00)+312));
}else if (command.lookH < -25){
armx += (2 * armxadd);
SetPosition(25, (((armx/1024.00) * 400.00)+312));
}

//------------------------------armYcode ----------------------------------

else if (command.lookV < -25){
//move the gripper forward
pany -= (1.75 *panyadd);
army += (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
}else if (command.lookV > 25){
//move the gripper backwards
pany += (1.75 * panyadd);
army -= (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//close the gripper
//this needs a bit of work.
//the current goal is to monitor the temperature upon closing.
//if the temperature rises the gripper should relax a bit.
//setting the compliance relatively high and limiting the torque
//of the gripper seams to have helped.
gripper += (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (26,0x2B,1);
}if(command.buttons&BUT_LT) {
//open the gripper
gripper -= (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (26,0x2B,1);
}if(command.buttons&BUT_R3) {
//lower the arm
armz -= (2 * armzadd);
pany += (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));

}if(command.buttons&BUT_L4) {
//raise the arm
armz += (2 * armzadd);
pany -= (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
}if(command.buttons&BUT_R2) {
bioloid.playSeq(Rest01);

}if(command.buttons&BUT_L5) {
//stand up
bioloid.playSeq(UpSlow01);
}if(ax12GetRegister (26,0x2B1,1) >= 50) {
gripper -= (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 4;
}
}
}
}
}


if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program


DB

darkback2
12-28-2012, 08:47 PM
I've finally had some time over this most recent holiday to do some additional work on Hikari. I feel like at one point I had the sensors working, but over time as I have jumped from one version of the code to another the sensor inputs stopped working. I finally went through and added two tilt sensors, and the IR distance sensor on her shoulder back in.

The current placement of the IR sensor will be pretty useless with the exception of detecting buildings. I may add more to her later, but given that she is a biped there isn't much room for stuff like that. The tilt sensors are placed behind her head and do a pretty good job of detecting falls.

I've also repainted her new helmet. I sort of stole Gdubb2's skull theme. :D

Finally I made some new plastic covers for her thighs, and over the next few weeks I'll be adding more plastic covers and moding the ones that are already there.

Here is a video demonstrating the fall detection. Hope you understand me not slamming her down given that she has a fitPC in her chest. :-)

https://www.youtube.com/watch?v=BZuYH8PVRgs&amp;list=UUdzU2TukTwvXVYDSBkzzSUQ&amp; index=1
I also need to find a way to reduce the torque to her arms, at least in kungfu mode, when she isn't actively moving them so that she doesn't strip her gears when she falls.

Here is the latest code so far.

/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it.
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* Tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */


#include <ax12.h>
#include <SharpIR.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// add sharp sensor
SharpIR myIR = SharpIR(GP2Y0A02YK,A0);




// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;
int sensorpin3 = 3; // analog pin used to connect the sharp sensor
int faceDown = 0;
int sensorpin4 = 4; // analog pin used to connect the sharp sensor
int faceUp = 0;
int screwUp;
int armx;
int armxadd;
int army;
int armyadd;
int armz;
int armzadd;
int gripper;
int gripperadd;
int grippertemp;
int IRpin;
//----------------------Start Void setup----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables and take initial readings
int mode = 0;
panh = 512;
panadd = 0;
pany = 512;
panyadd = 0;
//These are the arm movement vairables
int armx = 512;
int armxadd = 0;
int army = 512;
int armyadd = 0;
int armz = 512;
int armzadd = 0;
int gripper = 712;
int gripperadd = 0;
//These are the sensor inputs for the tilt sensors
int faceDown = 0;
int faceUp = 0;
//this takes the initial reading of the gripper servos
grippertemp = ax12GetRegister (26, 0x2B, 1);
int IRpin = 0;
//-----------------set up arbotix 2---------------------------------------------
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++)
//this would set the RX servos as 0 - 16
//for(i=0;<16;i++0
{
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}

//--------------------crouch down slowly----------------------------------------

bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
//---------------set the compliance of all leg servos-------------------------------

for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);

}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------

//--------------------start void loop-------------------------------------------

void loop(){
// keyboard mode
if(mode == 5){

//--------------------takes commands from the keyboard--------------------------
//format # servonumber "p" four didget position---------------------------------

char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == '#')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 's'){ //if I press s go to base fire pose
bioloid.playSeq(Mechfire);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(Msimpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'g'){ //if I press g get up faceDown
bioloid.playSeq(Facedown01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'G'){ //if I press G get up faceDown
bioloid.playSeq(Facedown02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'b'){ //if I press b get up from back
bioloid.playSeq(Backup01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'B'){ //if I press B finish getting up
bioloid.playSeq(Backup02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(MSl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(MSr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'k'){ //if I press k right push front
bioloid.playSeq(Push01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'K'){ //if I press k right finish push front
bioloid.playSeq(Push02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'i'){ //if I press i front punch
bioloid.playSeq(AttackFront01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == '>'){ //if I press > shoot right gun
ax12SetRegister2(29, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}else if(startByte == '<'){ //if I press < shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}else if(startByte == '"'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}
}



if (mode != 5){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-----------------change modes by pressing button R1---------------------------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;

//------------------------resets mode to 0--------------------------------------
if(mode >= 6){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}

//------------Initial mode-(kung fu)--------------------
if(mode==0){
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Msimpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//paused side attack
bioloid.playSeq(FrontAttack01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
SetPosition(26, 712);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}
else if(faceUp < 1000){
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
if (faceUp < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);

//switch to getup mode
mode = 1;}
else{}
}else if(faceDown < 1000){
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
if (faceDown < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);

//switch to getup mode
mode = 1;}
else {}
}
if(command.buttons&BUT_L6) {
//attackfront
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(26, 524);
delay(200);
bioloid.playSeq(LeftAttack01);
delay(400);
SetPosition(26, 712);
}
}
//-----------------------------end kungfu mode----------------------------------

//------------------------------get up mode-------------------------------------
if(mode == 1){
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//----------------------Here go the button commands-----------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L5) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L4) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L6) {
//Neutral position
mode -= 1;
}if(command.buttons&BUT_RT) {
//Right attack

bioloid.playSeq(RightAttack01);

}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(26, 524);
bioloid.playSeq(LeftAttack01);
delay (400);
SetPosition(26, 712);
}
}
//----------------------------end get up mode-----------------------------------

//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
SetPosition(26, 712);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(Msimpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(RestSkate01);
}if(command.buttons&BUT_L6) {
//Collapse
SetPosition(26, 512);
delay (200);
bioloid.playSeq(Cruel01);
delay (800);
SetPosition(26, 512);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
SetPosition(26, 524);
SetPosition(27, 612);
delay (200);
bioloid.playSeq(GoodRobot01);
delay (800);
SetPosition(26, 712);
SetPosition(27, 512);
}if(command.buttons&BUT_L4) {
//Start getting up
SetPosition(27, 412);
delay (200);
bioloid.playSeq(BadRobot01);
delay (700);
SetPosition(27, 512);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}
//--------------------------end cheesy moves mode ------------------------------

//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
//int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}

//----------------------------Head psn code ------------------------------------

else if (command.lookH > 25){
panh += (2 * panadd);
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -25){
panh += (2 * panadd);
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}

//------------------------------Gun/head Tilt code ----------------------------------

else if (command.lookV > 25){
pany += (2 * panyadd);
SetPosition(18, (((((pany/1024.00) * 312.00)+ 412)- 1024) * -1));
//trying to get servo 18 to move in the opposite direction
SetPosition(17, (((pany/1024.00) * 312.00)+ 62));
SetPosition(27, (((pany/1024.00) * 312.00)+ 312));
}else if (command.lookV < -25){
pany -= (2 * panyadd);
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 62));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 412)- 1024) * -1));
SetPosition(27, (((pany/1024.00) * 312.00)+ 312));

//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//shoot right gun
ax12SetRegister2(29, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_LT) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R3) {
//shoot right cannon
ax12SetRegister2(40, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(40, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//shoot left cannon
ax12SetRegister2(41, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(41, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R2) {
//rest01
SetPosition(26, 524);
delay (400);
bioloid.playSeq(Cruel01);
delay (1000);
SetPosition(26, 712);
}if(command.buttons&BUT_L5) {
//Side step left
bioloid.playSeq(Mechfire);
}else if(faceUp < 1000){
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
if (faceUp < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);

//switch to getup mode
mode = 1;}
else{}
}else if(faceDown < 1000){
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
if (faceDown < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);
//switch to getup mode
mode = 1;}
else {}
}
if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 3;
}
}//-------------------------armcontrolmode -----------------------------------
if(mode == 4){
//this sets the initial value of the variables
int panadd = (-float(command.lookH)/20);
int armxadd = (-float(command.lookH)/20);
int panyadd = (5);
int armyadd = (5);
int armzadd = (5);
int gripperadd = (5);
SetPosition(17, 262);
SetPosition(19, 262);
SetPosition(21, 762);
SetPosition(23, 562);
//this drastically reduces the torque for the gripper servo(not sure this helps)
ax12SetRegister(26, 0x0E, 2);
//sets the compliance really high for the gripper servo (also not sure this helps)
ax12SetRegister(26, 0x1B, 100);
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//----------------------------ArmXCode ------------------------------------

}else if (command.lookH > 25){
armx += (2 * armxadd);
SetPosition(25, (((armx/1024.00) * 400.00)+312));
}else if (command.lookH < -25){
armx += (2 * armxadd);
SetPosition(25, (((armx/1024.00) * 400.00)+312));
}

//------------------------------armYcode ----------------------------------

else if (command.lookV < -25){
//move the gripper forward
pany -= (1.75 *panyadd);
army += (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
//SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
}else if (command.lookV > 25){
//move the gripper backwards
pany += (1.75 * panyadd);
army -= (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
//SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//close the gripper
//this needs a bit of work.
//the current goal is to monitor the temperature upon closing.
//if the temperature rises the gripper should relax a bit.
//setting the compliance relatively high and limiting the torque
//of the gripper seams to have helped.
gripper += (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (26,0x2B,1);
}if(command.buttons&BUT_LT) {
//open the gripper
gripper -= (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (26,0x2B,1);
}if(command.buttons&BUT_R3) {
//lower the arm
armz -= (2 * armzadd);
pany += (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));

}if(command.buttons&BUT_L4) {
//raise the arm
armz += (2 * armzadd);
pany -= (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
}if(command.buttons&BUT_R2) {
bioloid.playSeq(Rest01);

}if(command.buttons&BUT_L5) {
//stand up
bioloid.playSeq(UpSlow01);
}if(ax12GetRegister (26,0x2B1,1) >= 50) {
gripper -= (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 4;
}
}
}
}
}


if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program

Yeah...progress.

DB

gdubb2
12-29-2012, 10:46 AM
Looking good DB.. Love the helmet:cool:

Gary

jwatte
12-29-2012, 11:16 AM
I also need to find a way to reduce the torque to her arms

Btw: Did you get that working? Looks like the register 34/35 lets you limit the torque per servo.

tician
12-29-2012, 03:55 PM
The NimbRo-OP has a pretty nice balance and fall detection algorithm that releases torque on most of the joints when it detects an inevitable fall (tilting too far or too fast). Of course, that is based on the DARwIn-OP code and requires a CM-730, but it should be possible to adapt the basics of it if you want to try digging through their modifications (https://github.com/NimbRo/nimbro-op).

darkback2
12-29-2012, 10:49 PM
The NimbRo-OP has a pretty nice balance and fall detection algorithm

The problem is that I am not using a nice fancy IMU. I'm using two ball switches. They are little balls in metal tubes...When the tube turns over the ball closes the switch. The problem is that the balls bounce when she walks, so...if I have everything set too sensitively the arms loose torque and she switches modes all the time. So I have some delay's and rechecking before she looses torque.

In the latest version of the code, assembled after I posted the other bit last night, I have the torque set super low except for when she is actually doing anything. Then when I do something, the torque kicks up, she makes the move, then it goes back down to the minimum again. The relax bit is to keep the arms from getting too horribly caught up if she falls down. But for the most part the torque in her shoulders and elbows is set super low.


/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it.
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* Tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */


#include <ax12.h>
#include <SharpIR.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// add sharp sensor
SharpIR myIR = SharpIR(GP2Y0A02YK,A0);




// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;
int sensorpin3 = 3; // analog pin used to connect the sharp sensor
int faceDown = 0;
int sensorpin4 = 4; // analog pin used to connect the sharp sensor
int faceUp = 0;
int screwUp;
int armx;
int armxadd;
int army;
int armyadd;
int armz;
int armzadd;
int gripper;
int gripperadd;
int grippertemp;
int IRpin;
//----------------------Start Void setup----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables and take initial readings
int mode = 0;
panh = 512;
panadd = 0;
pany = 512;
panyadd = 0;
//These are the arm movement vairables
int armx = 512;
int armxadd = 0;
int army = 512;
int armyadd = 0;
int armz = 512;
int armzadd = 0;
int gripper = 712;
int gripperadd = 0;
//These are the sensor inputs for the tilt sensors
int faceDown = 0;
int faceUp = 0;
//this takes the initial reading of the gripper servos
grippertemp = ax12GetRegister (26, 0x2B, 1);
int IRpin = 0;
//-----------------set up arbotix 2---------------------------------------------
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++)
//this would set the RX servos as 0 - 16
//for(i=0;<16;i++0
{
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}

//--------------------crouch down slowly----------------------------------------

bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
//---------------set the compliance of all leg servos-------------------------------

for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);

}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------

//--------------------start void loop-------------------------------------------

void loop(){
// keyboard mode
if(mode == 5){

//--------------------takes commands from the keyboard--------------------------
//format # servonumber "p" four didget position---------------------------------

char cmd[8];
int startByte = 0;

// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;

servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);

}// end if(startByte == '#')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 's'){ //if I press s go to base fire pose
bioloid.playSeq(Mechfire);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(Msimpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'g'){ //if I press g get up faceDown
bioloid.playSeq(Facedown01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'G'){ //if I press G get up faceDown
bioloid.playSeq(Facedown02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'b'){ //if I press b get up from back
bioloid.playSeq(Backup01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'B'){ //if I press B finish getting up
bioloid.playSeq(Backup02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(MSl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(MSr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'k'){ //if I press k right push front
bioloid.playSeq(Push01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'K'){ //if I press k right finish push front
bioloid.playSeq(Push02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'i'){ //if I press i front punch
bioloid.playSeq(AttackFront01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(startByte == '>'){ //if I press > shoot right gun
ax12SetRegister2(29, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}else if(startByte == '<'){ //if I press < shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}else if(startByte == '"'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}
}



if (mode != 5){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence

//-----------------change modes by pressing button R1---------------------------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;

//------------------------resets mode to 0--------------------------------------
if(mode >= 6){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}

//------------Initial mode-(kung fu)--------------------
if(mode==0){

//use Button1 to change mode.
if(command.walkV > 80){
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
// walk forward
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}else if(command.walkV < -50){
// walk defence pose
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(Msimpleback02);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}else if(command.walkH > 50){
// side step right
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(MSr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}else if(command.walkH < -50){
// side step left
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(MSl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}else if(command.lookH > 50){
// turn left
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}else if(command.lookH < -50){
// turn right
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}else if(command.lookV < -50){
// crouch
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//paused side attack
ax12SetRegister2(19, 0x22, 712); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 712);
ax12SetRegister2(21, 0x22, 712);
ax12SetRegister2(22, 0x22, 712);
bioloid.playSeq(FrontAttack01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
SetPosition(26, 712);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
delay (100);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}
else if(faceUp < 1000){
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
if (faceUp < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);

//switch to getup mode
mode = 1;}
else{}
}else if(faceDown < 1000){
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
if (faceDown < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);

//switch to getup mode
mode = 1;}
else {}
}
if(command.buttons&BUT_L6) {
//attackfront
ax12SetRegister2(19, 0x22, 750); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 750);
ax12SetRegister2(21, 0x22, 750);
ax12SetRegister2(22, 0x22, 750);
bioloid.playSeq(AttackFront01);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}if(command.buttons&BUT_L5) {
//drop down and push
ax12SetRegister2(19, 0x22, 750); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 750);
ax12SetRegister2(21, 0x22, 750);
ax12SetRegister2(22, 0x22, 750);
bioloid.playSeq(DownPush02);
ax12SetRegister2(19, 0x22, 250); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 250);
ax12SetRegister2(21, 0x22, 250);
ax12SetRegister2(22, 0x22, 250);
}if(command.buttons&BUT_L4) {
//reach back...back push?
ax12SetRegister2(19, 0x22, 750); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 750);
ax12SetRegister2(21, 0x22, 750);
ax12SetRegister2(22, 0x22, 750);
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
ax12SetRegister2(19, 0x22, 750); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 750);
ax12SetRegister2(21, 0x22, 750);
ax12SetRegister2(22, 0x22, 750);
delay (200);
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
ax12SetRegister2(19, 0x22, 750); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 750);
ax12SetRegister2(21, 0x22, 750);
ax12SetRegister2(22, 0x22, 750);
delay (200);
SetPosition(26, 524);
delay(200);
bioloid.playSeq(LeftAttack01);
delay(400);
SetPosition(26, 712);
}
}
//-----------------------------end kungfu mode----------------------------------

//------------------------------get up mode-------------------------------------
if(mode == 1){

//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//----------------------Here go the button commands-----------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
ax12SetRegister2(19, 0x22, 1012); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 1012);
ax12SetRegister2(21, 0x22, 1012);
ax12SetRegister2(22, 0x22, 1012);
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
ax12SetRegister2(19, 0x22, 1012); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 1012);
ax12SetRegister2(21, 0x22, 1012);
ax12SetRegister2(22, 0x22, 1012);
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L5) {
//Start getting up from back down
ax12SetRegister2(19, 0x22, 1012); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 1012);
ax12SetRegister2(21, 0x22, 1012);
ax12SetRegister2(22, 0x22, 1012);
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L4) {
//Finish getting up from back down
ax12SetRegister2(19, 0x22, 1012); //reducing torque to the arms.
ax12SetRegister2(20, 0x22, 1012);
ax12SetRegister2(21, 0x22, 1012);
ax12SetRegister2(22, 0x22, 1012);
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L6) {
//Neutral position
mode -= 1;
}if(command.buttons&BUT_RT) {
//Right attack

bioloid.playSeq(RightAttack01);

}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(26, 524);
bioloid.playSeq(LeftAttack01);
delay (400);
SetPosition(26, 712);
}
}
//----------------------------end get up mode-----------------------------------

//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
SetPosition(26, 712);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(Msimpleback02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(Qlt01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(Qrt01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(RestSkate01);
}if(command.buttons&BUT_L6) {
//Collapse
SetPosition(26, 512);
delay (200);
bioloid.playSeq(Cruel01);
delay (800);
SetPosition(26, 512);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
SetPosition(26, 524);
SetPosition(27, 612);
delay (200);
bioloid.playSeq(GoodRobot01);
delay (800);
SetPosition(26, 712);
SetPosition(27, 512);
}if(command.buttons&BUT_L4) {
//Start getting up
SetPosition(27, 412);
delay (200);
bioloid.playSeq(BadRobot01);
delay (700);
SetPosition(27, 512);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}
//--------------------------end cheesy moves mode ------------------------------

//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
//int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}

//----------------------------Head psn code ------------------------------------

else if (command.lookH > 25){
panh += (2 * panadd);
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -25){
panh += (2 * panadd);
SetPosition(25, (((panh/1024.00) * 512.00)+256));
}

//------------------------------Gun/head Tilt code ----------------------------------

else if (command.lookV > 25){
pany += (2 * panyadd);
SetPosition(18, (((((pany/1024.00) * 312.00)+ 412)- 1024) * -1));
//trying to get servo 18 to move in the opposite direction
SetPosition(17, (((pany/1024.00) * 312.00)+ 62));
SetPosition(27, (((pany/1024.00) * 312.00)+ 312));
}else if (command.lookV < -25){
pany -= (2 * panyadd);
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 62));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 412)- 1024) * -1));
SetPosition(27, (((pany/1024.00) * 312.00)+ 312));

//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//shoot right gun
ax12SetRegister2(29, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(29,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_LT) {
//shoot left gun
ax12SetRegister2(28, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(28, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R3) {
//shoot right cannon
ax12SetRegister2(40, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(40, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//shoot left cannon
ax12SetRegister2(41, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(41, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R2) {
//rest01
SetPosition(26, 524);
delay (400);
bioloid.playSeq(Cruel01);
delay (1000);
SetPosition(26, 712);
}if(command.buttons&BUT_L5) {
//Side step left
bioloid.playSeq(Mechfire);
}else if(faceUp < 1000){
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
delay(300);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
if (faceUp < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);

//switch to getup mode
mode = 1;}
else{}
}else if(faceDown < 1000){
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
delay(300);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
if (faceDown < 1000){ // relax arms
Relax(17);
Relax(18);
Relax(19);
Relax(20);
Relax(21);
Relax(22);
Relax(23);
Relax(24);
delay (3000);
//switch to getup mode
mode = 1;}
else {}
}
if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 3;
}
}//-------------------------armcontrolmode -----------------------------------
if(mode == 4){
//this sets the initial value of the variables
int panadd = (-float(command.lookH)/20);
int armxadd = (-float(command.lookH)/20);
int panyadd = (5);
int armyadd = (5);
int armzadd = (5);
int gripperadd = (5);
SetPosition(17, 262);
SetPosition(19, 262);
SetPosition(21, 762);
SetPosition(23, 562);
//this drastically reduces the torque for the gripper servo(not sure this helps)
ax12SetRegister(26, 0x0E, 2);
//sets the compliance really high for the gripper servo (also not sure this helps)
ax12SetRegister(26, 0x1B, 100);
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(Slt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(Srt02);
faceDown = analogRead(sensorpin3); // reads the value of the sharp sensor
Serial.print("faced ");
Serial.println(faceDown);
faceUp = analogRead(sensorpin4); // reads the value of the sharp sensor
Serial.print("faceu ");
Serial.println(faceUp); // prints the value of the sensor to the serial monitor
float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
float distance = 65*pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
Serial.print("dista ");
Serial.println(distance);

//----------------------------ArmXCode ------------------------------------

}else if (command.lookH > 25){
armx += (2 * armxadd);
SetPosition(25, (((armx/1024.00) * 400.00)+312));
}else if (command.lookH < -25){
armx += (2 * armxadd);
SetPosition(25, (((armx/1024.00) * 400.00)+312));
}

//------------------------------armYcode ----------------------------------

else if (command.lookV < -25){
//move the gripper forward
pany -= (1.75 *panyadd);
army += (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
//SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
}else if (command.lookV > 25){
//move the gripper backwards
pany += (1.75 * panyadd);
army -= (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
//SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//close the gripper
//this needs a bit of work.
//the current goal is to monitor the temperature upon closing.
//if the temperature rises the gripper should relax a bit.
//setting the compliance relatively high and limiting the torque
//of the gripper seams to have helped.
gripper += (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (26,0x2B,1);
}if(command.buttons&BUT_LT) {
//open the gripper
gripper -= (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (26,0x2B,1);
}if(command.buttons&BUT_R3) {
//lower the arm
armz -= (2 * armzadd);
pany += (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));

}if(command.buttons&BUT_L4) {
//raise the arm
armz += (2 * armzadd);
pany -= (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(27, (((pany/1024.00) * 262.00)+ 412));
}if(command.buttons&BUT_R2) {
bioloid.playSeq(Rest01);

}if(command.buttons&BUT_L5) {
//stand up
bioloid.playSeq(UpSlow01);
}if(ax12GetRegister (26,0x2B1,1) >= 50) {
gripper -= (2 * gripperadd);
SetPosition(26, ((((gripper/1024.00) * 262.00)+ 512)));
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 4;
}
}
}
}
}


if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program

DB

darkback2
06-02-2014, 01:44 PM
5610
One of Hikari's main issues is that she doesn't have hip rotation. I've done all sorts of seemingly fruitless things in order to get her to walk properly. Using skid steering She can walk and turn OK on smooth surfaces, and using a trick I picked up from Rob Farrell She can even turn while walking forward. (you make one leg shorter than the other when she is supposed to be walking straight and she turns towards the shorter leg.) But she is also hugely limited.

Hikari is also very heavy and tall. Using the PLM leg brackets Andrew designed makes adding hip rotation really hard without ending up with a super tall robot. While I have been looking for ways to hide or nest the hip rotation servos, this has proven to be exceedingly difficult or expensive.

In the shower the other morning I came up with an idea that I probably should have thought of a long time ago. I could nest the rotation in the feet instead of in the hips. This poses its own problems, but by using Airsoft pellets as ball bearings and milling out a trough I can get a reasonable level of stability without sacrificing much in the way of height. She'll gain a bit over 1cm of height with the change.

I started milling the feet out today, and should have something to post in the next few.

DB

jwatte
06-02-2014, 05:20 PM
using Airsoft pellets as ball bearings

That's... out-of-the-box :-)

Wouldn't the plastic be soft enough to compress under the weight?

If not, then at least maintenance and ball replacement will be cheap, although likely frequent :-)

darkback2
06-03-2014, 12:07 AM
5613
Here is the bottom layer with the bbs all in the groove. I would have liked to use a slightly bigger bit so that more of the BB actually sits inside of the track, Unfortunatly I only have a 1/8th inch end mill and a 1/4 inch end mill on hand...so I used the 1/4 on the top plate, and the 1/8th on the bottom. This keeps the bbs from sliding out provided the gap is kept pretty tight, while also ensuring there isn't too much slide from side to side.
5614
As for the plastic squishing, I stood on top of the plate and spun around. I weigh just under 200lbs. There are 42 bbs...oddly I should have know that would be the case given that 42 is the answer to the universe. The Bbs have some scaring, but my weight wasn't enough to break them. Hikari weighs a little over 10 pounds or so give or take a bit with the guns mounted. I know she will be a bit more dynamic than I am, but I'm thinking things will be OK. That and the cost/weight of metal ball bearings in really high. I've been using airsoft pellets as ball bearings in an arm that one of my students built a few years ago. Haven't had to replace any of them yet...

I'm thinking of 3d printing a plate to help hold the bbs in place, This would also help to hide / nest the bolt heads so that they don't hit each other. I still have to attach a skid plate to the bottom of the foot, though it will be less necessary.

Now one thing that I am not looking forward to is basically redoing everything in terms of all of her poses. I will first try editing her pypose exports by hand adding the two new servos, then I'm going to have to redo her turning sequences.

Down the line I would love to learn how to implement IK. she really needs it.

DB

jwatte
06-03-2014, 11:06 AM
I've been using airsoft pellets as ball bearings in an arm that one of my students built a few years ago. Haven't had to replace any of them yet...

Wow! That's amazing!

darkback2
06-11-2014, 10:20 PM
https://www.youtube.com/watch?v=Ea0xYxhT4WI

Hikari can finally walk and turn on carpet. The airsoft pellet ball bearings have held up so far, and While they show some signs of scaring after I myself climbed onto the foot and gave it a spin, none have fallen out which means the foot spacing is actually pretty good. I'll see how long they hold up.

In either case, it feels good to finally make this kind of progress with Hikari...

Also, not featured in the video, Hikari can now make noise. The speakers (an old pair of headphones) are really quiet, but she is making some sound. She talks randomly selecting pre composed statements, but in the future i hope to have her dynamically generate sentences.

I also added a feature where she says which mode she is in.

So stoked.

DB

kamondelious
06-11-2014, 10:46 PM
Great job DB! That looks fantastic.

gdubb2
06-12-2014, 10:37 AM
Dang DB.. That is soooo great.. You have hit on a great solution to the problem. :wink:

jwatte
06-12-2014, 11:14 AM
That looks sweet!
Do the "soles" turn entirely based on gravity/pressure, or do you have actuators working on them?

darkback2
06-12-2014, 11:26 AM
First off...thanks for the complements!

There is an RX24f that turns the soles. I have a 12 volt regulator between the last RX-64 and the RX-24f. I was originally planning to go with RX-28s but I've already dumped a ton of money into this thing. Final total for everything is just over $6500 for everything that is actually on there... Course I've spent way more than that on dead ends that went nowhere...But at this point I am super happy...

DB

Zenta
06-12-2014, 02:21 PM
Looks great! Very creative DB!

darkback2
06-12-2014, 04:09 PM
Having Zenta call your work creative...:wink:

darkback2
06-19-2014, 10:05 AM
Somehow managed to kill both RX-24fs yesterday in one go...Probably a bad wire in her legs or something...Not really sure. I was running the RX-24fs at the end of a chain of RX-28s and 64s on a 4 cell lipo. I was using a 12v regulator in the power lines between the last RX-64s and the RX-24fs. Figured it was enough protection. :-(

darkback2
06-20-2014, 07:19 PM
Ended up swapping out the busted RX-24fs for RX-28s. Also found a couple mashed wires in the legs that may have contributed to the problem. All is well that ends well I guess.

ArduTank
06-20-2014, 11:51 PM
That sucks. busted up wiring is never good, especially when it kills servos :(

darkback2
06-21-2014, 12:34 AM
Thanks ArduTank...

Thing is, these legs have a lot of sharper edges and pinch points. I'm sort of lucky things have held up as well as they have. Even with careful wire routing things still end up getting pinched a lot.

Having RX-28s which can handle the full 4cell lipo is probably as good as I'll end up getting.

DB

ArduTank
06-21-2014, 10:04 AM
There's a sheathing that I use on my computer builds when I have odd colored cables that might help you with the pinch points. It looks like this:

5625

The stuff takes some sawing action to cut with a razor blade, and is very flexible. You'd only have to worry about the spiral slots, in which case you could use some of this:

5626
The one in the picture uses carbon fiber for the mesh material :D

darkback2
06-21-2014, 10:18 AM
Yeah...I've got that on some of the joints...It actually doesn't fit in other places...Tolerances are a bit tight on this one...maybe even too tight to be honest.

DB

tician
06-21-2014, 11:14 AM
Mesh sleeving is good for preventing abrasion and sharp edge damage, much less so for pinch/crush damage. When pinched, it can actually make things worse since you can no longer easily inspect the individual wires for damage (especially if you have secured the ends with zip-tie or heat shrink). Best way to prevent pinch damage is to properly route the wires in occasionally ugly ways with twine/clips/tape/zip-ties attached to frames and maybe add plastic/rubber guides to keep it out of joint spaces. I still think properly routed and laced cable harnesses are a thing of beauty, even if cable lacing can be a bit more complex/time-consuming to install than zip-ties or velcro.

darkback2
06-21-2014, 01:43 PM
Tician, I whole heartedly agree. I am always complaining to my students about how their robots look like radio shack threw up in them. Hikari has been a strange challenge in that "A", she has has so many variations, and "B", she did not have a clear goal from the outset. She started out as a biped mech also capable of competing in Mech Warfare (never quite made it at either) and now she is a PC based humanoid robot performer? As things have changed, a number of the changes have been overlaid on the original design, which is a crappy way to do things.

Also, Her legs are a bit of a problem no matter how you slice it. She has metal brackets holding servos that move in relation to each other. The distance varies enough to cause problems with wires being tied down too tightly, and routing them to the outside where they would be less likely to get pinched would require having so much slack that the arms get hung up on them. Currently the wires travel through an internal void in the legs, but periodically the twist in the wires isn't tight enough to keep the wires from bulging out through a pinch point.

Really what I am saying is that this is a problem that will require quite a bit of time to think over thoroughly...Some day when I'm in the shower an idea will pop into my head and I'll wonder why I didn't think of it years ago.

DB

tician
06-21-2014, 02:30 PM
One possibility for routing variable length wires on the exterior of the frames would be a bit of heatshrink or other soft tubing rigidly attached to the frame, but permitting the wires to slide freely within. A more expensive and complex possibility would be two plates of plastic separated by washers/nuts/standoffs for the wire to slide through with a very low force torsion spring mechanism in the center to pull in the slack to keep it from forming large loops near the joints.

Simpler tensioner mechanism for the two plate design would be a tiny compression/extension spring (or just a rubber band) at the center of two plastic standoffs pushing/pulling against a U of wire between the standoffs. Need to have the standoffs/bushings large enough diameter to not damage wire, but could have the sliding roller move in little slot routed in each plate. In the crappy drawing: blue is wire, black is fixed position screw with bushing/standoff, green is sliding rod/roller, and red is compression spring.

darkback2
06-21-2014, 11:05 PM
Yeah...see...about 6 months from now I'm going to be in the shower and bam...I'll think of how to actually implement that idea. :-) Thanks.

DB

tician
06-22-2014, 12:08 AM
A little after that last post, I realized that a rubber band, and maybe some tape, is all you would need and you can leave the rubber band as a loop or cut it into a strand. I was going to make some photos of my attempt, but, this being Georgia, the several year old bag of natural-rubber rubber bands in the pantry has turned into a glob of hard, crusty gunk. Natural rubber does not last in Georgia, ever. After a couple years it starts sticking to anything and everything, and some time after that it becomes a dried-out, crusty mess without releasing whatever it already stuck to.

Since I cannot draw well, steps 1 and 2 (and maybe 3 depending on how well the rubber band gripped the cables) of Figure 9-59 (http://www.hnsa.org/doc/cabling/part5.htm) (Telephone Hitch) illustrate what I was going to do to attach three sections of the rubber band loop to the loosest configuration of the cable with the two segments between the knots attached to the frame (basically three equally spaced knots with the midpoints of the free segments tied/zip-tied/taped to the frame).

Xevel
06-22-2014, 01:04 AM
Sometimes, when I have wires that have to go over a complicated joint, i route them helicoidally around the joint, and force its passage at certain key points by having them go through a bit of tube secured to a part... Not sure if it makes sense. The result is that whatever the motion of the 2 or 3 dof joint, the cables will just coil a little bit more or less, instead of sometimes slacking, sometimes being stretched.
Imagine a slinky around the joint(s) if it gives you a more graphical idea of what I mean...

ArduTank
06-22-2014, 08:41 AM
I do that one too. Makes for tidy turrets, but culd also work on Hikari's legs if long enough wires were found/made. Maybe you could make a splitter for them that allows you to connect each pair of servos with a short wire, then have the splitter tree come spiraling down the leg, dropping a branch at each pair of servos, then repeat for the other leg.

darkback2
06-28-2014, 03:25 PM
So a couple of problems...Not the end of the world, but still...

Problem 1, LEDs and Buttons

Hikari has several different "modes" because I wanted to expand the capabilities of the arbotix commander. So for example, she starts out in "Kung-fu" mode, and pressing one of the buttons on the commander switching which mode she is in. She currently has 6 modes, the last of which is mode 5 in which she can be controlled by computer.

I wanted to add some sort of visual indicator of which mode she is in. So I hooked up 4 LEDs. The LEDs are wired connecting the ground to pins 0 - 3 through the LED. The center pin is not connected. (not sure if that is the problem.)

I feel like the LEDs did work at one point, but using previous versions of the code has not resulted in them working again.

Now I also want to connect a button (robotgeek) (http://www.trossenrobotics.com/robotGeek-pushbutton) and mount it on the top of Hikari's shoulder. Eventually I want to set it up so that pressing that button takes her directly to mode 5 so that she starts listening to the FIT-PC in her chest. I currently have the code setup so that releasing the button makes her do a move. This means that if I plug the button into the right pin she should immediately start moving. (button is LOW when pressed.)

The thing is neither of these two things work. :-( (I think I took out the button code because I didn't want to run into a problem later.)


Problem 2, ROS

I am using the arbotix2...Rarely seen in the wild. :-p

I for the life of me can't get the ROS sketch to compile. I feel like I must simply be looking in the wrong place, but it means that I can't run Pypose...Which means I can't create new poses...which means it is really hard to make Hikari do anything new. I feel like this is something simple...


/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digets) , (goal position 4 digets) CR
/* You can also send it letters from the keyboard and have it play */
/* sequences based on which letters you send it.
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if thens to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* Tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */

#include <ax12.h>
#include <SharpIR.h>
#include <BioloidController.h>
#include <Commander.h>
#include <WProgram.h>
#include "Hikarikey04b.h" // exported from PyPose
BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();
// add sharp sensor
SharpIR myIR = SharpIR(GP2Y0A02YK,0);
//SharpIR face01 = SharpIR(GP2Y0A02YK,3);
//SharpIR face02 = SharpIR(GP2Y0A02YK,4);

// make variables
int mode;
int panh;
int panadd;
int pany;
int panyadd;
int faceDown;
int faceUp;
int screwUp;
int armx;
int armxadd;
int army;
int armyadd;
int armz;
int armzadd;
int gripper;
int gripperadd;
int grippertemp;
//----------------------Start Void setup----------------------
void setup(){

//open the serial port
command.begin(38400);

delay(2000); // recommended pause

//make defaults for variables and take initial readings
int mode = 0;
pinMode (0,OUTPUT);
pinMode (1,OUTPUT);
pinMode (2,OUTPUT);
panh = 512;
panadd = 0;
pany = 512;
panyadd = 0;
//These are the arm movement vairables
int armx = 512;
int armxadd = 0;
int army = 512;
int armyadd = 0;
int armz = 512;
int armzadd = 0;
int gripper = 712;
int gripperadd = 0;
//These are the sensor inputs for the tilt sensors
int faceDown = 3;
int faceUp = 4;
pinMode(3, INPUT);
pinMode(4, INPUT);
faceDown = analogRead(3);
faceUp = analogRead(4);
//this takes the initial reading of the gripper servos
grippertemp = ax12GetRegister (28, 0x2B, 1);
//-----------------set up arbotix 2---------------------------------------------
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++)
//this would set the RX servos as 0 - 16
//for(i=0;<16;i++0
{
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}
//--------------------crouch down slowly----------------------------------------
bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
//---------------set the compliance of all leg servos-------------------------------

for (int i = 1; i < 16; i++) {
ax12SetRegister(i, 0x1A, 3);
delay(3);
ax12SetRegister(i, 0x1B, 3);
delay(3);

}
for (int i = 17; i < 24; i++) {
ax12SetRegister(i, 0x1A, 30);
delay(3);
ax12SetRegister(i, 0x1B, 30);
delay(3);

}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------
//--------------------start void loop-------------------------------------------

void loop(){
// keyboard mode
if(mode == 5){
digitalWrite(0,HIGH);
digitalWrite(1,LOW);
digitalWrite(2,HIGH);
//--------------------takes commands from the keyboard--------------------------
//format # servonumber "p" four didget position---------------------------------
char cmd[8];
int startByte = 0;
// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));
} // end Serial.available()
if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;
servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 diget servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 diget goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);
}// end if(startByte == '#')
else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
// this should read the IR sensor
Serial.println(myIR.getData());
//this should read the tilt sensor
Serial.println(analogRead(3));
//this should read the other tilt sensor
Serial.println(analogRead(4));
//this should read the temp of the gripper servo
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(Msimplewalk01tr);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 's'){ //if I press s go to base fire pose
bioloid.playSeq(Mechfire);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(Msimpleback02);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'g'){ //if I press g get up facedown
bioloid.playSeq(Facedown01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'G'){ //if I press G get up facedown
bioloid.playSeq(Facedown02);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'b'){ //if I press b get up from back
bioloid.playSeq(Backup01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'B'){ //if I press B finish getting up
bioloid.playSeq(Backup02);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(SWmTurnLeft01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(MSl01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(SWmTurnRight01);
Serial.println(myIR.getData());
Serial.println(digitalRead(3));
Serial.println(digitalRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(MSr01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'k'){ //if I press k right push front
bioloid.playSeq(AttackFront01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'K'){ //if I press k Taunt
bioloid.playSeq(Taunt01);
Serial.println(myIR.getData());
//Serial.println(face01.getData());
//Serial.println(face02.getData());
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(startByte == 'i'){ //if I press i gunplay
bioloid.playSeq(Msimplewalk01au);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'u'){ //if I press u Bad Robot
bioloid.playSeq(BadRobot01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'o'){ //if I press o good robot
bioloid.playSeq(GoodRobot01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == '<'){ //if I press Left Kick
bioloid.playSeq(LeftKick01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == '>'){ //if I press Right Kick
bioloid.playSeq(RightKick01);
Serial.println(myIR.getData());
Serial.println(analogRead(3));
Serial.println(analogRead(4));
Serial.println(ax12GetRegister (28, 0x2b,1));
}else if(startByte == '"'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

//end of keyboard stuff.
}
}
}

if (mode != 5){
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence
//-----------------change modes by pressing button R1---------------------------

if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;
//------------------------resets mode to 0--------------------------------------
if(mode >= 6){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}
//------------Initial mode-(kung fu)--------------------
if(mode==0){
digitalWrite(0,HIGH);
digitalWrite(1,LOW);
digitalWrite(2,HIGH);
//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01tr);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Msimpleback02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// new turn left using twisty foot thing
bioloid.playSeq(SWmTurnLeft01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//paused side attack
bioloid.playSeq(FrontAttack01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
SetPosition(28, 712);
}if(command.buttons&BUT_L6) {
//attackfront
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(28, 524);
delay(200);
bioloid.playSeq(LeftAttack01);
delay(400);
SetPosition(28, 712);
}
}
//-----------------------------end kungfu mode----------------------------------

//------------------------------get up mode-------------------------------------
if(mode == 1){
digitalWrite(0,HIGH);
digitalWrite(1,LOW);
digitalWrite(2,LOW);
//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(SWmTurnLeft01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//----------------------Here go the button commands-----------------------------
}if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L5) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L4) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L6) {
//Neutral position
mode -= 1;
}if(command.buttons&BUT_RT) {
//Right attack

bioloid.playSeq(RightAttack01);

}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(28, 524);
bioloid.playSeq(LeftAttack01);
delay (400);
SetPosition(28, 712);
}
}
//----------------------------end get up mode-----------------------------------

//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
digitalWrite(0,LOW);
digitalWrite(1,HIGH);
digitalWrite(2,LOW);
//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
SetPosition(28, 712);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(Msimpleback02);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(SWmTurnLeft01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//--------------------Here go the button commands-------------------------------
}if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Taunt01);
}if(command.buttons&BUT_L6) {
//Collapse
SetPosition(28, 512);
delay (200);
bioloid.playSeq(Cruel01);
delay (800);
SetPosition(28, 512);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
SetPosition(28, 524);
SetPosition(29, 612);
delay (200);
bioloid.playSeq(GoodRobot01);
delay (800);
SetPosition(28, 712);
SetPosition(29, 512);
}if(command.buttons&BUT_L4) {
//Start getting up
SetPosition(29, 412);
delay (200);
bioloid.playSeq(BadRobot01);
delay (700);
SetPosition(29, 512);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}
}
//--------------------------end cheesy moves mode ------------------------------

//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
digitalWrite(0,HIGH);
digitalWrite(1,HIGH);
digitalWrite(2,LOW);
//int panh = 512;
int panadd = (-float(command.lookH)/20);
int panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(SWmTurnLeft01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}

//----------------------------Head psn code ------------------------------------

else if (command.lookH > 25){
panh += (2 * panadd);
SetPosition(27, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -25){
panh += (2 * panadd);
SetPosition(27, (((panh/1024.00) * 512.00)+256));
}
//------------------------------Gun/head Tilt code ----------------------------------

else if (command.lookV > 25){
pany += (2 * panyadd);
SetPosition(18, (((((pany/1024.00) * 312.00)+ 312)- 1024) * -1));
SetPosition(22, 512);
//trying to get servo 18 to move in the opposite direction
SetPosition(17, (((pany/1024.00) * 312.00)+ 312));
SetPosition(21, 512);
SetPosition(29, (((pany/1024.00) * 312.00)+ 312));
}else if (command.lookV < -25){
pany -= (2 * panyadd);
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 312));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 312)- 1024) * -1));
SetPosition(29, (((pany/1024.00) * 312.00)+ 312));

//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//shoot right gun
ax12SetRegister2(31, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(31,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_LT) {
//shoot left gun
ax12SetRegister2(30, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(30, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R3) {
//shoot right cannon
ax12SetRegister2(40, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(40, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//shoot left cannon
ax12SetRegister2(41, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(41, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R2) {
//rest01
SetPosition(28, 524);
delay (400);
bioloid.playSeq(Cruel01);
delay (1000);
SetPosition(28, 712);
}if(command.buttons&BUT_L5) {
//Side step left
bioloid.playSeq(Mechfire);
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 3;
}
}//-------------------------armcontrolmode -----------------------------------
if(mode == 4){
digitalWrite(0,LOW);
digitalWrite(1,LOW);
digitalWrite(2,HIGH);
//this sets the initial value of the variables
int panadd = (-float(command.lookH)/20);
int armxadd = (-float(command.lookH)/20);
int panyadd = (5);
int armyadd = (5);
int armzadd = (5);
int gripperadd = (5);
SetPosition(17, 262);
SetPosition(19, 262);
SetPosition(21, 762);
SetPosition(23, 562);
//this drastically reduces the torque for the gripper servo(not sure this helps)
ax12SetRegister(28, 0x0E, 2);
//sets the compliance really high for the gripper servo (also not sure this helps)
ax12SetRegister(28, 0x1B, 100);
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(SWmTurnLeft01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
Serial.println(faceDown, DEC);
Serial.println(faceUp, DEC);

//----------------------------ArmXCode ------------------------------------

}else if (command.lookH > 25){
armx += (2 * armxadd);
SetPosition(27, (((armx/1024.00) * 400.00)+312));
}else if (command.lookH < -25){
armx += (2 * armxadd);
SetPosition(27, (((armx/1024.00) * 400.00)+312));
}
//------------------------------armYcode ----------------------------------

else if (command.lookV < -25){
//move the gripper forward
pany -= (1.75 *panyadd);
army += (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
}else if (command.lookV > 25){
//move the gripper backwards
pany += (1.75 * panyadd);
army -= (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
//------------------------Here go the button commands---------------------------
}if(command.buttons&BUT_RT) {
//close the gripper
//this needs a bit of work.
//the current goal is to monitor the temperature upon closing.
//if the temperature rises the gripper should relax a bit.
//setting the compliance relatively high and limiting the torque
//of the gripper seams to have helped.
gripper += (2 * gripperadd);
SetPosition(28, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (28,0x2B,1);
}if(command.buttons&BUT_LT) {
//open the gripper
gripper -= (2 * gripperadd);
SetPosition(28, ((((gripper/1024.00) * 262.00)+ 512)));
ax12GetRegister (28,0x2B,1);
}if(command.buttons&BUT_R3) {
//lower the arm
armz -= (2 * armzadd);
pany += (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));

}if(command.buttons&BUT_L4) {
//raise the arm
armz += (2 * armzadd);
pany -= (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
}if(command.buttons&BUT_R2) {
bioloid.playSeq(Rest01);

}if(command.buttons&BUT_L5) {
//stand up
bioloid.playSeq(UpSlow01);
}if(ax12GetRegister (28,0x2B1,1) >= 50) {
gripper -= (2 * gripperadd);
SetPosition(28, ((((gripper/1024.00) * 262.00)+ 512)));
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 4;
}
}
}
}
}

if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program




Error output from arduino






ros:37: error: no matching function for call to 'BioloidController::BioloidController()'
C:\Users\darkb_000\Documents\Arduino\libraries\Bio loid/BioloidController.h:48: note: candidates are: BioloidController::BioloidController(long int)
C:\Users\darkb_000\Documents\Arduino\libraries\Bio loid/BioloidController.h:45: note: BioloidController::BioloidController(const BioloidController&)
In file included from ros.ino:72:
/user_hooks.h: In function 'unsigned char userWrite(int, unsigned char)':
user_hooks.h:15: error: 'ERR_INSTRUCTION' was not declared in this scope
ros.ino: In function 'unsigned char handleWrite()':
ros:119: error: 'ERR_INSTRUCTION' was not declared in this scope
ros:123: error: 'ERR_INSTRUCTION' was not declared in this scope
ros:131: error: 'ERR_INSTRUCTION' was not declared in this scope
ros:140: error: 'ERR_INSTRUCTION' was not declared in this scope
ros:158: error: 'ERR_INSTRUCTION' was not declared in this scope
ros:178: error: 'ERR_NONE' was not declared in this scope
ros:182: error: 'ERR_NONE' was not declared in this scope
ros.ino: In function 'void loop()':
ros:322: error: 'ERR_CHECKSUM' was not declared in this scope
ros:345: error: 'class BioloidController' has no member named 'setup'
ros:379: error: 'class BioloidController' has no member named 'setup'
ros:381: error: 'class BioloidController' has no member named 'setId'
ros:397: error: 'class BioloidController' has no member named 'getId'
In file included from ros.pde:39:
/ros.h: At global scope:
ros.h:67: error: redefinition of 'int mode'
ros.h:67: error: 'int mode' previously defined here
ros.h:69: error: redefinition of 'unsigned char id'
ros.h:69: error: 'unsigned char id' previously defined here
ros.h:70: error: redefinition of 'unsigned char length'
ros.h:70: error: 'unsigned char length' previously defined here
ros.h:71: error: redefinition of 'unsigned char ins'
ros.h:71: error: 'unsigned char ins' previously defined here
ros.h:73: error: redefinition of 'unsigned char params [143]'
ros.h:73: error: 'unsigned char params [143]' previously declared here
ros.h:74: error: redefinition of 'unsigned char index'
ros.h:74: error: 'unsigned char index' previously defined here
ros.h:76: error: redefinition of 'int checksum'
ros.h:76: error: 'int checksum' previously declared here
ros:43: error: 'PML' does not name a type
ros:48: error: conflicting declaration 'HServo servos [2]'
ros:47: error: 'servos' has a previous declaration as 'Servo servos [10]'
ros:60: error: redefinition of 'Motors2 drive'
ros:53: error: 'Motors2 drive' previously declared here
In file included from ros.pde:63:
pid.h:32: error: redefinition of 'int left_pwm'
diff_controller.h:34: error: 'int left_pwm' previously declared here
pid.h:33: error: redefinition of 'int right_pwm'
diff_controller.h:35: error: 'int right_pwm' previously declared here
pid.h:34: error: redefinition of 'int left_speed'
diff_controller.h:36: error: 'int left_speed' previously declared here
pid.h:35: error: redefinition of 'int right_speed'
diff_controller.h:37: error: 'int right_speed' previously declared here
pid.h:38: error: redefinition of 'int Kp'
diff_controller.h:40: error: 'int Kp' previously declared here
pid.h:39: error: redefinition of 'int Kd'
diff_controller.h:41: error: 'int Kd' previously declared here
pid.h:40: error: redefinition of 'int Ki'
diff_controller.h:42: error: 'int Ki' previously declared here
pid.h:41: error: redefinition of 'int Ko'
diff_controller.h:43: error: 'int Ko' previously declared here
pid.h:42: error: redefinition of 'int maxAccel'
diff_controller.h:44: error: 'int maxAccel' previously declared here
pid.h:45: error: redefinition of 'unsigned int PIDmode'
diff_controller.h:47: error: 'unsigned int PIDmode' previously declared here
pid.h:51: error: redefinition of 'long unsigned int f_time'
diff_controller.h:53: error: 'long unsigned int f_time' previously declared here
pid.h:53: error: redefinition of 'unsigned char moving'
diff_controller.h:55: error: 'unsigned char moving' previously defined here
pid.h:54: error: redefinition of 'unsigned char paused'
diff_controller.h:56: error: 'unsigned char paused' previously defined here
pid.h:65: error: conflicting declaration 'typedef struct SetPointInfo SetPointInfo'
diff_controller.h:67: error: 'SetPointInfo' has a previous declaration as 'typedef struct SetPointInfo SetPointInfo'
pid.h:67: error: redefinition of 'SetPointInfo left'
diff_controller.h:69: error: 'SetPointInfo left' previously declared here
pid.h:67: error: redefinition of 'SetPointInfo right'
diff_controller.h:69: error: 'SetPointInfo right' previously declared here
/pid.h: In function 'void setupPID()':
pid.h:70: error: redefinition of 'void setupPID()'
diff_controller.h:72: error: 'void setupPID()' previously defined here
/pid.h: In function 'void DoPid(SetPointInfo*)':
pid.h:81: error: redefinition of 'void DoPid(SetPointInfo*)'
diff_controller.h:83: error: 'void DoPid(SetPointInfo*)' previously defined here
/pid.h: In function 'void ClearPID()':
pid.h:106: error: redefinition of 'void ClearPID()'
diff_controller.h:108: error: 'void ClearPID()' previously defined here
/pid.h: In function 'void updatePID()':
pid.h:117: error: redefinition of 'void updatePID()'
diff_controller.h:119: error: 'void updatePID()' previously defined here
/pid.h: In function 'void clearAll()':
pid.h:137: error: redefinition of 'void clearAll()'
diff_controller.h:139: error: 'void clearAll()' previously defined here
ros.pde: At global scope:
ros:67: error: redefinition of 'unsigned char baud'
ros:59: error: 'unsigned char baud' previously defined here
ros:68: error: redefinition of 'unsigned char ret_level'
ros:60: error: 'unsigned char ret_level' previously defined here
ros:69: error: redefinition of 'unsigned char alarm_led'
ros:61: error: 'unsigned char alarm_led' previously defined here
ros:70: error: redefinition of 'int servo_vals [10]'
ros:48: error: 'int servo_vals [10]' previously declared here
ros:76: error: conflicting declaration 'typedef struct sp_trans_t sp_trans_t'
ros:67: error: 'sp_trans_t' has a previous declaration as 'typedef struct sp_trans_t sp_trans_t'
ros:77: error: conflicting declaration 'int poses [30][30]'
ros:68: error: 'poses' has a previous declaration as 'int poses [30][32]'
ros:78: error: redefinition of 'sp_trans_t sequence [50]'
ros:69: error: 'sp_trans_t sequence [50]' previously declared here
ros:79: error: redefinition of 'int seqPos'
ros:70: error: 'int seqPos' previously declared here
ros.pde: In function 'void setup()':
ros:96: error: redefinition of 'void setup()'
ros:90: error: 'void setup()' previously defined here
ros:105: error: 'pml' was not declared in this scope
ros.pde: In function 'unsigned char handleWrite()':
ros:120: error: redefinition of 'unsigned char handleWrite()'
ros:112: error: 'unsigned char handleWrite()' previously defined here
ros:126: error: 'INSTRUCTION_ERROR' was not declared in this scope
ros:131: error: 'REG_DIGITAL' was not declared in this scope
ros:152: error: 'INSTRUCTION_ERROR' was not declared in this scope
ros:161: error: 'INSTRUCTION_ERROR' was not declared in this scope
ros:179: error: 'INSTRUCTION_ERROR' was not declared in this scope
ros:183: error: 'REG_LM_SIGN' was not declared in this scope
ros:185: error: 'REG_LM_PWM' was not declared in this scope
ros:191: error: 'REG_RM_SIGN' was not declared in this scope
ros:193: error: 'REG_RM_PWM' was not declared in this scope
ros:201: error: 'REG_LM_SPEED_L' was not declared in this scope
ros:203: error: 'REG_LM_SPEED_H' was not declared in this scope
ros:205: error: 'REG_RM_SPEED_L' was not declared in this scope
ros:207: error: 'REG_RM_SPEED_H' was not declared in this scope
ros:223: error: 'REG_KP' was not declared in this scope
ros:225: error: 'INSTRUCTION_ERROR' was not declared in this scope
ros:228: error: 'REG_KD' was not declared in this scope
ros:230: error: 'REG_KI' was not declared in this scope
ros:232: error: 'REG_KO' was not declared in this scope
ros:237: error: 'REG_PML_SERVO' was not declared in this scope
ros:239: error: 'pml' was not declared in this scope
ros:240: error: 'REG_PML_SENSOR' was not declared in this scope
ros:241: error: 'pml' was not declared in this scope
ros:242: error: 'REG_STEP_START_L' was not declared in this scope
ros:243: error: 'step_start' was not declared in this scope
ros:244: error: 'REG_STEP_START_H' was not declared in this scope
ros:245: error: 'step_start' was not declared in this scope
ros:246: error: 'REG_STEP_VALUE' was not declared in this scope
ros:247: error: 'step_value' was not declared in this scope
ros:248: error: 'REG_STEP_COUNT' was not declared in this scope
ros:249: error: 'pml' was not declared in this scope
ros:249: error: 'step_start' was not declared in this scope
ros:249: error: 'step_value' was not declared in this scope
ros:250: error: 'REG_PML_ENABLE' was not declared in this scope
ros:251: error: 'pml' was not declared in this scope
ros:252: error: 'pml' was not declared in this scope
ros:256: error: 'INSTRUCTION_ERROR' was not declared in this scope
ros.pde: In function 'int handleRead()':
ros:263: error: redefinition of 'int handleRead()'
ros:189: error: 'int handleRead()' previously defined here
ros:281: error: 'REG_DIGITAL' was not declared in this scope
ros:314: error: 'REG_ENC_LEFT_L' was not declared in this scope
ros:316: error: 'REG_ENC_RIGHT_L' was not declared in this scope
ros:320: error: 'REG_KP' was not declared in this scope
ros:326: error: 'REG_KD' was not declared in this scope
ros:328: error: 'REG_KI' was not declared in this scope
ros:330: error: 'REG_KO' was not declared in this scope
ros:335: error: 'REG_PML_DIR' was not declared in this scope
ros:338: error: 'scan_dir' was not declared in this scope
ros:338: error: 'pml' was not declared in this scope
ros:340: error: 'REG_PML_TIME_L' was not declared in this scope
ros:341: error: 'pml' was not declared in this scope
ros:342: error: 'REG_PML_TIME_H' was not declared in this scope
ros:343: error: 'pml' was not declared in this scope
ros:344: error: 'REG_PML_BASE' was not declared in this scope
ros:345: error: 'scan_dir' was not declared in this scope
ros:345: error: 'UP_SCAN' was not declared in this scope
ros:346: error: 'pml' was not declared in this scope
ros:348: error: 'pml' was not declared in this scope
ros:355: error: 'BYTE' was not declared in this scope
As of Arduino 1.0, the 'BYTE' keyword is no longer supported.
Please use Serial.write() instead.
ros.pde: In function 'int doPlaySeq()':
ros:393: error: redefinition of 'int doPlaySeq()'
ros:251: error: 'int doPlaySeq()' previously defined here
ros.pde: In function 'void statusPacket(int, int)':
ros:414: error: redefinition of 'void statusPacket(int, int)'
ros:272: error: 'void statusPacket(int, int)' previously defined here
ros:415: error: 'BYTE' was not declared in this scope
As of Arduino 1.0, the 'BYTE' keyword is no longer supported.
Please use Serial.write() instead.
ros.pde: In function 'void loop()':
ros:427: error: redefinition of 'void loop()'
ros:285: error: 'void loop()' previously defined here
ros:464: error: 'CHECKSUM_ERROR' was not declared in this scope
ros:474: error: 'BYTE' was not declared in this scope
As of Arduino 1.0, the 'BYTE' keyword is no longer supported.
Please use Serial.write() instead.
ros:515: error: 'ARB_TEST' was not declared in this scope
ros:528: error: 'BYTE' was not declared in this scope
As of Arduino 1.0, the 'BYTE' keyword is no longer supported.
Please use Serial.write() instead.
ros:563: error: 'BYTE' was not declared in this scope
As of Arduino 1.0, the 'BYTE' keyword is no longer supported.
Please use Serial.write() instead.
ros:592: error: 'pml' was not declared in this scope



Hopefully I'm just doing something stupid?

DB

tician
06-28-2014, 04:13 PM
1) Where did you get the 5 pin LED? Any spec sheets or wiring diagrams?

2a) The arbotix_firmware (ros.ino, ros.h, etc.) within the arbotix_ros repo (https://github.com/vanadiumlabs/arbotix_ros/) is completely independent of the pypose sketch (pypose.ino) contained within the arbotix repo (https://github.com/vanadiumlabs/arbotix/). If you want to use PyPose (the actual Python program that runs on the PC and contained within the PyPose repo (https://github.com/vanadiumlabs/pypose/)), you do not use ROS anywhere.
2b) Did you update the libraries acquired from the arbotix repo? There was a change to BioloidController a while back that added a new object constructor, and the ros.ino sketch appears to require the new constructor and object configuration functions.

darkback2
06-28-2014, 05:02 PM
First off, thanks for the quick reply.

Sorry for the confusion, it isn't a 5 pin LED. It is 4 individual LEDs

I am using an arbotix2. Not sure if things have changed, but you used to have to use upload the ROS sketch to the arbotix2 in order to use pypose to control your robot. With the regular arbotix you would upload the pypose sketch in order to use pypose to control your robot...

I will update my libraries and see if that doesn't do it...Figured it had to be something simple...I've fallen behind because everyone else switched over to MX servos.

DB

tician
06-28-2014, 05:31 PM
I spoke a bit soon as ros.ino appears to be based largely on pypose.ino comms with the PC, but with lots of ROS extras added on. The pypose code is fairly device independent, but using RX servos with the ax12 library requires the 'scan()' function from the ros.ino sketch. Either modifying pypose.ino with the scan() function, or just using ros.ino with the updated library, should work about as well.

So you have four LEDs wired with the anode (+) of the LED connected to an arbotix pin and the cathode (-) connected to ground? Did these ever have current limiting resistors added? What is this 'center pin' that is not connected? If you are talking about the RobotGeek LED driver, you need to connect G to ground, V to +5V, and S to an arbotix pin.

darkback2
06-28-2014, 05:53 PM
He he...so updating the bioloid controller files gets me past the first error, but all of the error return stuff hangs it up. I can comment that out, but it seams like a bad idea to do stuff that way. The problem is that then the combined hi no hikari sketch returns a bunch of errors. i'm starting to think I need to find old versions of everything...Arduino, and all of the libraries and run things that way. At least then they worked. Hikari might just be a dinosaur. :-(

The LEDs are ones that I just got from radioshack. There is a resister in there.

If I connect the center pin and ground the LED lights up so there is voltage on the center +5v pin. I will have to pick up a robot geek LED and see if the code works with that. I am not getting any voltage from the signal pin which I thought should spit out 5v when set to HIGH.

DB

tician
06-28-2014, 06:38 PM
Did you update all of the library files? The error definitions used in ros.ino are defined in ax12.h and there are likely other changes in ax12.cpp and the other libraries that might be included by ros.ino and other sketches.


So it is a single color LED and a single resistor in a single molded with three pins? A quick search through the RS site does not have anything like that, so a part/catalog number or clearer description of how you are actually wiring the LED up would be useful. They list 3-pin bi-color LEDs without integrated resistors (web only) and a 3-pin 'high-brightness' 7-color blinking LED with a control pin to change color (RS#276-016). With the blinking LED: pin 1 is GND, pin 2 is 3.2Vmax, pin 3 is mode (pull low to change color).

tician
06-29-2014, 01:41 AM
So... I had a bit of time on my hands and there were so many things that irked me...
The creation of new, local variables when wanting to use the global variables. The massive amount of copy-pasta for printing sensor states. The repeated attempts to use variables that were never actually updated anywhere.

Added quite a few comments; commented out and/or replaced lots of println's with printSensor() calls to eradicate copy-pasta; added setLEDs(int), flashLEDs(int), and flashLEDs(void) to make setting/flashing LEDs much easier/cleaner; added soft/hard functions for actuators. Not certain the flashLEDs(void) function will actually work as I expect it to, but otherwise Arduino-1.5.4 is only giving me errors due to the missing pose file and poses. Had to un-comment the arbotix2 options from the hardware/arbotix/avr/boards.txt file in the most recent version of the arbotix repo.



/* ================================================== ======================== */
/* Commander/HikariKey/ServoControl
/* This code is a good starting place for using the
/* keyboard of a computer and the commander to control servos using the arbotix.
/* Send data in the form of # (servo ID 2 digits) , (goal position 4 digits) CR
/* You can also send it letters from the keyboard and have it play
/* sequences based on which letters you send it.
/* If you use this you want to change the "hikarikey04b.h to whatever
/* your poses are called. You also want to change the if-else's to
/* things that make sense to you.
/*
/* This code was written by Darkback2 with more help than not by
/* Chris Novak
/* lnxfergy (Michael Ferguson, inventor of the arbotix)
/* Upgrayd
/* Jess
/* tician
/* and the rest of the Trossen Robotics Forum.
/* ================================================== ======================== */

#include <ax12.h>
#define GetTemperature(id) (ax12GetRegister(id, AX_PRESENT_TEMPERATURE, 1))

#include <SharpIR.h>
#include <BioloidController.h>
#include <Commander.h>
#include "Hikarikey04b.h" // exported from PyPose
BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();
// add sharp sensor
SharpIR myIR = SharpIR(GP2Y0A02YK,0); // Really pin 0? Is that not an LED output?
//SharpIR face01 = SharpIR(GP2Y0A02YK,3);
//SharpIR face02 = SharpIR(GP2Y0A02YK,4);

// make variables
int mode;

int panh;
int panadd;
int pany;
int panyadd;

int armx;
int armxadd;
int army;
int armyadd;
int armz;
int armzadd;
int gripper;
int gripperadd;

int irValue;
int faceDown;
int faceUp;
int screwUp;
int gripperTemp;


//---------------------- Set AX/RX state for AX12 library ----------------------
void AXRXscan()
{
#if defined(AX_RX_SWITCHED)
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++)
{
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1))
{
dynamixel_bus_config[i] = 0;
}
}
#endif
}


#define checkFaceDown() analogRead(3)
#define checkFaceUp() analogRead(4)
void updateSensors()
{
// read the IR sensor
irValue = myIR.getData();
// read the tilt sensor (analog?)
faceDown = checkFaceDown();
// read the other tilt sensor (analog?)
faceUp = checkFaceUp();
// read the temp of the gripper servo
gripperTemp = GetTemperature(28);
}
void printSensors()
{
updateSensors();
// print the IR sensor
Serial.println(irValue);
// print the tilt sensor
Serial.println(faceDown);
// print the other tilt sensor
Serial.println(faceUp);
// print the temp of the gripper servo
Serial.println(gripperTemp);
}

void setLEDs(int num) // Sets LED states to 'num'
{
// Set LEDs as if they were bits in a 3-bit number
digitalWrite(0, (num&(1<<0)));
digitalWrite(1, (num&(1<<1)));
digitalWrite(2, (num&(1<<2)));
}
void flashLEDs(int num) // Flashes and finally sets LED states to 'num'
{
setLEDs(0);
delay(50);
setLEDs(num);
delay(50);
setLEDs(0);
delay(50);
setLEDs(num);
}
void flashLEDs() // Flashes LEDs using current state (does not modify)
{
int currentState = 0;
currentState += (digitalRead(0)<<0);
currentState += (digitalRead(1)<<1);
currentState += (digitalRead(2)<<2);

flashLEDs(currentState);
}

void makeActuatorSoft(int servoNum) // Drop torque and increase compliance
{
// decrease the torque limit
ax12SetRegister2(servoNum, AX_TORQUE_LIMIT_L, 200);

// increase the compliance slopes and margins
ax12SetRegister(servoNum, AX_CW_COMPLIANCE_MARGIN, 30);
ax12SetRegister(servoNum, AX_CCW_COMPLIANCE_MARGIN, 30);
ax12SetRegister(servoNum, AX_CW_COMPLIANCE_SLOPE, 128);
ax12SetRegister(servoNum, AX_CCW_COMPLIANCE_SLOPE, 128);
}
void makeActuatorHard(int servoNum) // Restore torque and compliance to default
{
// increase the torque limit
ax12SetRegister2(servoNum, AX_TORQUE_LIMIT_L, 1023);
// assumes AX_MAX_TORQUE_L/H is 1023

// restore the compliance slopes and margins to defaults
ax12SetRegister(servoNum, AX_CW_COMPLIANCE_MARGIN, 1);
ax12SetRegister(servoNum, AX_CCW_COMPLIANCE_MARGIN, 1);
ax12SetRegister(servoNum, AX_CW_COMPLIANCE_SLOPE, 32);
ax12SetRegister(servoNum, AX_CCW_COMPLIANCE_SLOPE, 32);
}

//----------------------Start Void setup----------------------
void setup(){
//open the serial port
command.begin(38400);

delay(2000); // recommended pause

// Set 0, 1, and 2 as outputs for LEDs
pinMode (0,OUTPUT);
pinMode (1,OUTPUT);
pinMode (2,OUTPUT);
// Set 3 and 4 as inputs for tilt sensors
pinMode(3, INPUT);
pinMode(4, INPUT);


// set defaults of global variables and take initial readings
mode = 0;

panh = 512;
panadd = 0;
pany = 512;
panyadd = 0;

//These are the arm movement variables
armx = 512;
armxadd = 0;
army = 512;
armyadd = 0;
armz = 512;
armzadd = 0;
gripper = 712;
gripperadd = 0;

updateSensors();

//--------- arbotix only: scan for RX servos to operate with RX-bridge ---------
delay(1000);
AXRXscan();

//--------------------crouch down slowly----------------------------------------
bioloid.loadPose(rest01);
bioloid.readPose();
bioloid.interpolateSetup(2000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}

//---------------set the compliance of all leg servos---------------------------
for (int i = 1; i < 16; i++) {
ax12SetRegister(i, AX_CW_COMPLIANCE_MARGIN, 3);
delay(3);
ax12SetRegister(i, AX_CCW_COMPLIANCE_MARGIN, 3);
delay(3);
}
for (int i = 17; i < 24; i++) {
ax12SetRegister(i, AX_CW_COMPLIANCE_MARGIN, 30);
delay(3);
ax12SetRegister(i, AX_CCW_COMPLIANCE_MARGIN, 30);
delay(3);
}
//-------------------end new stuff from jess------------------------------------
}
//-------------------end setup loop --------------------------------------------

//--------------------start void loop-------------------------------------------
void loop(){

updateSensors();
// you keep calling/printing faceDown, faceUp, and others variables
// without updating them anywhere in the loop!

//************************************************** ****************************
//************************************************** ****************************
// keyboard mode
if(mode == 5){
// Set LEDs to display binary '5'
flashLEDs(5);

//--------------------takes commands from the keyboard--------------------------
// Servo command format: #nnpppp
// (hash/pound delimiter)(two digit servo number)(four digit goal position)
// input must conform exactly to this format as parsing code is very fragile
// Other commands: c
// (one ASCII character)

char cmd[8];
int startByte = 0;
// if there are bytes waiting on the serial port
if(Serial.available() > 0) {
// read the incoming byte
startByte = Serial.read();
// toggle LED so we know johnny5 is alive
// digitalWrite(0,HIGH-digitalRead(0));
flashLEDs();
} // end Serial.available()

if(startByte == '#') {
// get the command from the serial port
while(Serial.available() < 8); // wait for data to arrive, do nothing
for(int i=0;i<8;i++){
cmd[i] = Serial.read();
} //this creates an array that takes in the data from the serial port
// convert command to two numbers
int servoNum = 0;
int servoPos = 0;
servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
//builds the 2 digit servo Id number
servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
//builds the 4 digit goal position
SetPosition(servoNum,servoPos);
//sets servo position using 2 numbers one for servo, other for position.
// debug
Serial.print("cmd array:");
for(int i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.print("servoNum:");
Serial.println(servoNum);
Serial.print("servoPos:");
Serial.println(servoPos);
}// end if(startByte == '#')

else {
//process commands other than servo positions
// only process if we aren't currently doing a sequence
if(bioloid.playing == 0){
if(startByte == 'r'){ //if I press r relax the legs
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
//commands from keyboard
else if(startByte == 'w'){ //if I press w walk forward
bioloid.playSeq(simplewalk01);
}else if(startByte == 'W'){ //if I press W walk forward slowly
bioloid.playSeq(Msimplewalk01tr);
}else if(startByte == 'X'){ //if I press X walk backword slowly
bioloid.playSeq(Rest01);
}else if(startByte == 's'){ //if I press s go to base fire pose
bioloid.playSeq(Mechfire);
}else if(startByte == 'S'){ //if I press S big jump
bioloid.playSeq(Jump01);
}else if(startByte == 'x'){ //if I press x defensive pose
bioloid.playSeq(Msimpleback02);
}else if(startByte == 'g'){ //if I press g get up facedown
bioloid.playSeq(Facedown01);
}else if(startByte == 'G'){ //if I press G get up facedown
bioloid.playSeq(Facedown02);
}else if(startByte == 'b'){ //if I press b get up from back
bioloid.playSeq(Backup01);
}else if(startByte == 'B'){ //if I press B finish getting up
bioloid.playSeq(Backup02);
}else if(startByte == 'q'){ //if I press q turn left
bioloid.playSeq(SWmTurnLeft01);
}else if(startByte == 'a'){ //if I press a sidestep left
bioloid.playSeq(MSl01);
}else if(startByte == 'e'){ //if I press e turn left
bioloid.playSeq(SWmTurnRight01);
}else if(startByte == 'd'){ //if I press d sidestep right
bioloid.playSeq(MSr01);
}else if(startByte == 'j'){ //if I press j attack left
bioloid.playSeq(LeftAttack01);
}else if(startByte == 'l'){ //if I press l attack right
bioloid.playSeq(RightAttack01);
}else if(startByte == 'k'){ //if I press k right push front
bioloid.playSeq(AttackFront01);
}else if(startByte == 'K'){ //if I press k Taunt
bioloid.playSeq(Taunt01);
}else if(startByte == 'i'){ //if I press i gunplay
bioloid.playSeq(Msimplewalk01au);
}else if(startByte == 'u'){ //if I press u Bad Robot
bioloid.playSeq(BadRobot01);
}else if(startByte == 'o'){ //if I press o good robot
bioloid.playSeq(GoodRobot01);
}else if(startByte == 'z'){ //if I press z rest
bioloid.playSeq(Defence01);
}else if(startByte == 'n'){ //if I press n get into dropped push mode
bioloid.playSeq(DownPush01);
}else if(startByte == 'N'){ //if I press n Downpush 02
bioloid.playSeq(DownPush02);
}else if(startByte == 'm'){ //if I press n rest attack
bioloid.playSeq(RestAttack01);
}else if(startByte == '<'){ //if I press Left Kick
bioloid.playSeq(LeftKick01);
}else if(startByte == '>'){ //if I press Right Kick
bioloid.playSeq(RightKick01);
}else if(startByte == '"'){ //if I press ` set mode to 0
mode += 1;
}
//special commands from keyboard that sets specific servos to shooting.

// Cut down on code repetition. copy-pasta breeds errors.
printSensors(); // Calls updateSensors() again, internally

//end of keyboard stuff.
}
}
}

//************************************************** ****************************
//************************************************** ****************************
if (mode != 5){
// check for new messages from Commander
if(command.ReadMsgs() > 0){
// toggle LED so we know johnny5 is alive
// digitalWrite(0,HIGH-digitalRead(0));
// exact mode unknown, set all 3 LEDs to identify it as commander control
flashLEDs(7);

// only process commands if we aren't currently doing a sequence
if(bioloid.playing == 0){
//-----------------change modes by pressing button R1---------------------------
if(command.buttons&BUT_R1) {
// slowjump as visual indicator
mode += 1;
//------------------------resets mode to 0--------------------------------------
if(mode >= 6){
mode = 0;
bioloid.playSeq(Defence01);
}
bioloid.playSeq(slowjump);
}

//************************************************** ****************************
//************************************************** ****************************
//---------------------------Initial mode-(kung fu)-----------------------------
if(mode==0){
// Set LEDs to display binary '6' since '0' is not helpful
flashLEDs(6);

//use Button1 to change mode.
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01tr);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk defence pose
bioloid.playSeq(Msimpleback02);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// new turn left using twisty foot thing
bioloid.playSeq(SWmTurnLeft01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}
//--------------------Here go the button commands-------------------------------
/// Kinda guessing these should be mostly '}else if() {' ?
/// Do we really want all of these possibly playing in this sequence from a
/// single Commander packet?
if(command.buttons&BUT_R2) {
//paused side attack
bioloid.playSeq(FrontAttack01);
}if(command.buttons&BUT_R3) {
//return to neutral position
bioloid.playSeq(UpSlow01);
SetPosition(28, 712);
}if(command.buttons&BUT_L6) {
//attackfront
bioloid.playSeq(AttackFront01);
}if(command.buttons&BUT_L5) {
//drop down and push
bioloid.playSeq(DownPush02);
}if(command.buttons&BUT_L4) {
//reach back...back push?
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(28, 524);
delay(200);
bioloid.playSeq(LeftAttack01);
delay(400);
SetPosition(28, 712);
}

// Is anybody actually listening here? A physical Commander
// with default firmware does not listen to anything sent
// to it over Xbee. Maybe a fit-pc2i listening in or emulating?
printSensors();
}
//-----------------------------end kungfu mode----------------------------------


//************************************************** ****************************
//************************************************** ****************************
//------------------------------get up mode-------------------------------------
if(mode == 1){
// Set LEDs to display binary '1'
flashLEDs(1);

//get up mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(simpleback02);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(Sr01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(Sl01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(SWmTurnLeft01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}
//----------------------Here go the button commands-----------------------------
/// Kinda guessing these should be mostly '}else if() {' ?
/// Do we really want all of these possibly playing in this sequence from a
/// single Commander packet?
if(command.buttons&BUT_R2) {
//Start getting up from face down
bioloid.playSeq(Facedown01);
}if(command.buttons&BUT_R3) {
//Finish getting up from face down
bioloid.playSeq(Facedown02);
}if(command.buttons&BUT_L5) {
//Start getting up from back down
bioloid.playSeq(Backup01);
}if(command.buttons&BUT_L4) {
//Finish getting up from back down
bioloid.playSeq(Backup02);
}if(command.buttons&BUT_L6) {
//Neutral position
mode -= 1;
}if(command.buttons&BUT_RT) {
//Right attack
bioloid.playSeq(RightAttack01);
}if(command.buttons&BUT_LT) {
//Left Attack
SetPosition(28, 524);
bioloid.playSeq(LeftAttack01);
delay (400);
SetPosition(28, 712);
}

// Is anybody actually listening here? A physical Commander
// with default firmware does not listen to anything sent
// to it over Xbee. Maybe a fit-pc2i listening in or emulating?
printSensors();
}
//----------------------------end get up mode-----------------------------------


//************************************************** ****************************
//************************************************** ****************************
//------------------------cheesy moves mode-(soccer)----------------------------
if(mode == 2){
// Set LEDs to display binary '2'
flashLEDs(2);

//special moves
if(command.walkV > 80){
// walk forward
bioloid.playSeq(simplewalk01);
SetPosition(28, 712);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// walk backwards
bioloid.playSeq(Msimpleback02);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// side step right
bioloid.playSeq(MSr01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// side step left
bioloid.playSeq(MSl01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookH > 50){
// turn left
bioloid.playSeq(SWmTurnLeft01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.lookV < -50){
// crouch
bioloid.playSeq(Defence01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}
//--------------------Here go the button commands-------------------------------
/// Kinda guessing these should be mostly '}else if() {' ?
/// Do we really want all of these possibly playing in this sequence from a
/// single Commander packet?
if(command.buttons&BUT_R2) {
//jump
bioloid.playSeq(Jump01);
}if(command.buttons&BUT_R3) {
//gunplay
bioloid.playSeq(Taunt01);
}if(command.buttons&BUT_L6) {
//Collapse
SetPosition(28, 512);
delay (200);
bioloid.playSeq(Cruel01);
delay (800);
SetPosition(28, 512);
}if(command.buttons&BUT_L5) {
//Finish getting up from face down
SetPosition(28, 524);
SetPosition(29, 612);
delay (200);
bioloid.playSeq(GoodRobot01);
delay (800);
SetPosition(28, 712);
SetPosition(29, 512);
}if(command.buttons&BUT_L4) {
//Start getting up
SetPosition(29, 412);
delay (200);
bioloid.playSeq(BadRobot01);
delay (700);
SetPosition(29, 512);
}if(command.buttons&BUT_RT) {
//right kick
bioloid.playSeq(RightKick01);
}if(command.buttons&BUT_LT) {
//Left kick
bioloid.playSeq(LeftKick01);
}

// Is anybody actually listening here? A physical Commander
// with default firmware does not listen to anything sent
// to it over Xbee. Maybe a fit-pc2i listening in or emulating?
printSensors();
}
//--------------------------end cheesy moves mode ------------------------------


//************************************************** ****************************
//************************************************** ****************************
//-------------------------mech warfare mode -----------------------------------
if(mode == 3){
// Set LEDs to display binary '3'
flashLEDs(3);

//int panh = 512;
panadd = (-float(command.lookH)/20);
panyadd = (5);
//mech warfare mode
if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Rest01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(SWmTurnLeft01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}

//----------------------------Head psn code ------------------------------------
else if (command.lookH > 25){
panh += (2 * panadd);
SetPosition(27, (((panh/1024.00) * 512.00)+256));
}else if (command.lookH < -25){
panh += (2 * panadd);
SetPosition(27, (((panh/1024.00) * 512.00)+256));
}
//------------------------------Gun/head Tilt code ----------------------------------
else if (command.lookV > 25){
pany += (2 * panyadd);
SetPosition(18, (((((pany/1024.00) * 312.00)+ 312)- 1024) * -1));
SetPosition(22, 512);
//trying to get servo 18 to move in the opposite direction
SetPosition(17, (((pany/1024.00) * 312.00)+ 312));
SetPosition(21, 512);
SetPosition(29, (((pany/1024.00) * 312.00)+ 312));
}else if (command.lookV < -25){
pany -= (2 * panyadd);
SetPosition(17, (((pany/ 1024.00) * 312.00)+ 312));
SetPosition(18, (((((pany/1024.00) * 312.00)+ 312)- 1024) * -1));
SetPosition(29, (((pany/1024.00) * 312.00)+ 312));
}
//------------------------Here go the button commands---------------------------
/// Kinda guessing these should be mostly '}else if() {' ?
/// Do we really want all of these possibly playing in this sequence from a
/// single Commander packet?
if(command.buttons&BUT_RT) {
//shoot right gun
ax12SetRegister2(31, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(31,AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_LT) {
//shoot left gun
ax12SetRegister2(30, AX_GOAL_SPEED_L,312);
delay (600);
ax12SetRegister2(30, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R3) {
//shoot right cannon
ax12SetRegister2(40, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(40, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_L4) {
//shoot left cannon
ax12SetRegister2(41, AX_GOAL_SPEED_L,362);
delay (1000);
ax12SetRegister2(41, AX_GOAL_SPEED_L,0);
}if(command.buttons&BUT_R2) {
//rest01
SetPosition(28, 524);
delay (400);
bioloid.playSeq(Cruel01);
delay (1000);
SetPosition(28, 712);
}if(command.buttons&BUT_L5) {
//Side step left
bioloid.playSeq(Mechfire);
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 3;
}

// Is anybody actually listening here? A physical Commander
// with default firmware does not listen to anything sent
// to it over Xbee. Maybe a fit-pc2i listening in or emulating?
printSensors();
}
//--------------------------end mech warfare mode ------------------------------


//************************************************** ****************************
//************************************************** ****************************
//------------------------- arm control mode -----------------------------------
if(mode == 4){
// Set LEDs to display binary '4'
flashLEDs(4);

//this sets the initial value of the variables
panadd = (-float(command.lookH)/20);
armxadd = (-float(command.lookH)/20);
panyadd = (5);
armyadd = (5);
armzadd = (5);
gripperadd = (5);
SetPosition(17, 262);
SetPosition(19, 262);
SetPosition(21, 762);
SetPosition(23, 562);
// This should drastically reduce the torque for the gripper servo
// It should work now that it is correct command and going
// to the correct register table address
// ax12SetRegister2(28, AX_TORQUE_LIMIT_L, 200);
// This sets the compliance really high for the gripper servo
// Only sets one direction
// ax12SetRegister(28, 0x1B, 100);
makeActuatorSoft(28);

if(command.walkV > 80){
// walk forward
bioloid.playSeq(Msimplewalk01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkV < -50){
// rest pose
bioloid.playSeq(Defence01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH > 50){
// turn right
bioloid.playSeq(SWmTurnLeft01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(SWmTurnRight01);
// Serial.println(faceDown, DEC);
// Serial.println(faceUp, DEC);

//----------------------------ArmXCode ------------------------------------
}else if (command.lookH > 25){
armx += (2 * armxadd);
SetPosition(27, (((armx/1024.00) * 400.00)+312));
}else if (command.lookH < -25){
armx += (2 * armxadd);
SetPosition(27, (((armx/1024.00) * 400.00)+312));
}
//------------------------------armYcode ----------------------------------
else if (command.lookV < -25){
//move the gripper forward
pany -= (1.75 *panyadd);
army += (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
}else if (command.lookV > 25){
//move the gripper backwards
pany += (1.75 * panyadd);
army -= (2 * armyadd);
SetPosition(18, ((((army/1024.00) * 612.00)+ 212)));
SetPosition(22, (((((army/1024.00) * 612.00)+ 212.00) - 1024.00) * -1));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
}
//------------------------Here go the button commands---------------------------
/// Kinda guessing these should be mostly '}else if() {' ?
/// Do we really want all of these possibly playing in this sequence from a
/// single Commander packet?
if(command.buttons&BUT_RT) {
//close the gripper
//this needs a bit of work.
//the current goal is to monitor the temperature upon closing.
//if the temperature rises the gripper should relax a bit.
//setting the compliance relatively high and limiting the torque
//of the gripper seams to have helped.
gripper += (2 * gripperadd);
SetPosition(28, ((((gripper/1024.00) * 262.00)+ 512)));
// ax12GetRegister (28,0x2B,1);
// What we doing getting, but not storing/printing/using?
}if(command.buttons&BUT_LT) {
//open the gripper
gripper -= (2 * gripperadd);
SetPosition(28, ((((gripper/1024.00) * 262.00)+ 512)));
// ax12GetRegister (28,0x2B,1);
// What we doing getting, but not storing/printing/using?
}if(command.buttons&BUT_R3) {
//lower the arm
armz -= (2 * armzadd);
pany += (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
}if(command.buttons&BUT_L4) {
//raise the arm
armz += (2 * armzadd);
pany -= (2 *panyadd);
SetPosition(18, ((((armz/1024.00) * 612.00)+ 212)));
SetPosition(29, (((pany/1024.00) * 262.00)+ 412));
}if(command.buttons&BUT_R2) {
bioloid.playSeq(Rest01);
}if(command.buttons&BUT_L5) {
//stand up
bioloid.playSeq(UpSlow01);
}if(command.buttons&BUT_L6) {
//reset to mode 0
mode += 4;
}

// Is anybody actually listening here? A physical Commander
// with default firmware does not listen to anything sent
// to it over Xbee. Maybe a fit-pc2i listening in or emulating?
printSensors(); // calls updateSensors() again, internally

// Temperature Safety Checks/Corrections on gripper servo
// updateSensors();
// already called in printSensors() and at start of loop()
if (gripperTemp >= 50)
{
gripper -= (2 * gripperadd);
SetPosition(28, ((((gripper/1024.00) * 262.00)+ 512)));
}
}
}
}
}

if(bioloid.playing > 0){
bioloid.play(); // continue sequence
}
} //end program

darkback2
06-29-2014, 03:35 PM
Thank you for all of your help Tician.

I've decided to start over from scratch installing Arduino again. I am not a programmer, and have very little experience programming with the exception of this project which might explain how confusing and copy pasting the code has gotten.

I am also a bit paranoid in that when something works I don't like to change it, and because so few people have or have even heard of arbotix2, I have always kind of felt like I am on my own with this one...So...I would never have thought that arbotix2 support would be a part of any new code.

I'll be doing this today, and I'll let you know how it goes...hopefully this evening.

DB

darkback2
06-29-2014, 05:02 PM
Question...When, or how do I tell arduino that it is an arbotix2 and not just an arbotix? In the ax-12.h file it says


/** Configuration **/
#if defined(ARBOTIX)
// no config needed
#elif defined(SERVO_STIK) || defined(ARBOTIX2)
#define AX_RX_SWITCHED
#define SET_RX_WR (PORTC |= 0x40)
#define SET_AX_WR (PORTC |= 0x80)
#define SET_RX_RD (PORTC = (PORTC & 0xBF) | 0x80)
#define SET_AX_RD (PORTC = (PORTC & 0x7F) | 0x40)
#define INIT_AX_RX DDRC |= 0xC0; PORTC |= 0xC0


and in the boards
#arbotix2.name=ArbotiX2

is in there, but I don't understand when I tell arduino that the program is going onto an arbotix2

I hope this makes sense.

DB

darkback2
06-29-2014, 05:26 PM
Just got arduino all set up and uploaded tician's code. The good news is the LEDs flash like crazy. The bad news:

When you turn her on she twitches into an awkward pose, then doesn't respond to the commander. In order to upload the code I copied it from the above post and then uploaded it to the arbotix2. Her PC was not turned on.

DB

KurtEck
06-29-2014, 05:27 PM
I am pretty sure it does not mater if it is an Arbotix or an Arbotix2. Note: My PhantomX now (sometimes) has an Arbotix2 on it. Currently it is running with a Teensy3.1, but can switch to Arbotix2 with a couple of cable changes.

The Pin definitions for the Arbotix and Arbotix2 are the same. The main differences is that some of the features of the Arbotix was removed (like stuff for motors) and some more pins IO pins were populated on the board.

Note: I should mention, that I am using my own fork of the Arbotix project. (https://github.com/kurte/arbotix). The differences with my project is that I updated the Hardware core code base to the Arduino 1.0.5 code base where the current official code base is something like Arduino 1.0.0 or maybe 1.0.1. I wanted this as there were several fixes and features put into the Arduino code base that I wanted to use. Things like Serial read time outs, Serial.flush() works properly. Before it only waited until the queue was empty, current code base waits until the last byte was actually sent...

A month or two ago they were looking at doing a pull from my fork into the main fork, but for some reason that has not happened yet. They may have run into issues with installing it on other platforms. I mainly use a windows 7 setup, but I have also tried it out on my Intel NUC running Ubuntu 14.04. I have not tried on a MAC as I don't have one... Actually my wife does, but...

KurtEck
06-29-2014, 05:44 PM
... then doesn't respond to the commander. In order to upload the code I copied it from the above post and then uploaded it to the arbotix2. Her PC was not turned on.
DB
Couple things I would check:
a) Are the XBee paired with each other?

b) You are using a current version of the commander class. It used to have an issue, with the call: command.begin(38400);
As the parameter to begin used to be int and 38400 would be converted to a negative value... Which is why for a long time the programs used to call Serial.begin() insteand of command.begin().


Kurt

darkback2
06-29-2014, 05:58 PM
Kurt,

This may be my problem? I did a fresh install of arduino this morning. I created a new sketchbook folder and downloaded the arbotix folder following the directions on the arbotix robocontroller page.


To use the ArbotiX with the Arduino IDE we need to add some files to our Arduino sketchbook folder. Most users will want to download the latest ArbotiX/PyPose (http://github.com/vanadiumlabs/arbotix/zipball/master), extract the zip file, and copy the contents of the arbotix folder into the sketchbook. This includes the hardware folder, libraries folder and a number of example sketches.
Alternatively you can grab a zipball of the absolute latest (http://github.com/vanadiumlabs/arbotix/zipball/master), bleeding edge source, or checkout from the github repository (http://github.com/vanadiumlabs/arbotix), and then locate the files correctly as described above.

I've never used github before and couldn't figure out how to just download files from it... :-( (My neighbor who does coding will be home later to walk me through that one.)

perhaps these aren't the files I am looking for?

The LED's worked though which is good...

DB

KurtEck
06-29-2014, 06:29 PM
DB,

Not sure which OS you are using. But simple getting stuff from github is usually not very hard. You can go to the main project page:
example: https://github.com/vanadiumlabs/arbotix

From there, couple of options (windows and Ubuntu and..): On the right hand side you will see a button: Download Zip, which rolls everything into a ZIP file and downloads it. Depending on your OS and options, it can either save the file away, or on my windows machine it typically runs winzip, which I use to extract the files and on Ubuntu I use Archive Manager to do the same thing.

If you wish to actually be able to use other commands to sync up to changes and the like, you alternatively can actually create a clone of the project on your machine, which allows you to use all of git. Note: I am no expert on using all of git!!!

Ways to do this:
Command line: Linux (and maybe other places):
Above the Download zip file, is a box with the clone name, you can copy the text to clipboard. Then in command prompt, on a linux box, I will have a directory like: ~/git which I will set as my current directory and then type in a command, something like:
git clone https://github.com/vandadiumlabs/arbotix.git
(Note the path came from the clipboard.)

On Windows, I use the app github for windows, which hides a lot of the gunk for you. As I have this application installed and have an account on github, when I am at a top project page, there is an additional button: Clone in Desktop, which will automatically download the project to my machine at the location that I have configured into github for windows...

On Ubuntu: I have experimented a little using smartgithg app, which is free for hobbyist, again I am no expert on using it...

Good luck

tician
06-29-2014, 07:17 PM
Oy. Lots of stuff happens when I finally look away.

You uncomment the arbotix2 settings in the boards.txt file by removing the '#' sign from the start of each arbotix2.* line. Restarting the IDE after updating the boards.txt file should then make the Arbotix2 show up in the boards list. There are some differences in settings and definitions between the Arbotix/Arbotix-M with RX-bridge and the Arbotix2, so probably best not to try using anything else.

How far is the pose from what she should be doing? Is it horrible contortions or is it just not-quite-right?

Just realized how messed up my flashing LED code usage was since it was called every run of loop() instead of only when the mode changed. I suspect my rearrangement of the sensor printing is interfering a bit with the commander's serial port. I finally forked the arbotix repo, and reorganized it a bit to make installing on Arduino-1.5 (https://github.com/tician/arbotix/archive/arduino-1.5.zip) easier (just copy-merge the hardware folder into your sketchbook's hardware folder then uncomment the arbotix2 options in boards.txt). Added another updated version of 'hikarikey (https://github.com/tician/arbotix/blob/arduino-1.5/hardware/arbotix/avr/examples/hikarikey/hikarikey.ino)' to the repo since it is easier/cleaner than the code tags and I will probably be messing things up worse before any of it gets better (also a couple small changes to ax12 and bioloidcontroller libraries).

darkback2
06-29-2014, 08:55 PM
Tician,

Progress! Thank you !

Ok,

So I merged the hardware folders and switched to using the arbotix2 board in the tools section of arduino. I also copied and pasted the hikarikey code you posted in post #221 and ran into a few problems. one I fixed by copying #define GetTemperature (id) over from the earlier code you posted earlier in this thread.

The second problem was "commsTypeScan was not declared in this scope." That I don't know how to fix.

The twitch/contortion is something that I have seen before. It comes from not telling the arbotix2 that it has both AX and RX servos...(not sure if I am wording that correctly.)

So I did an experiment...I ran the code you posted earlier today or yesterday and now instead of twitching and freezing with the LEDs going like crazy, she jittered and moved really slowly, no twitch.

The new code posted in post #221 I got to compile by commenting out commsTypeScan. The twitch came back, but then after the twitch everything seems to work.

Thank you so much...

DB

tician
06-29-2014, 09:28 PM
I had moved the GetTemperature() macro to ax12.h, and renamed/moved the AXRXScan() to commsTypeScan() in ax12.h/cpp (to determine which are TTL or RS-485 servos as part of the library instead of requiring the user do it in the sketch). If it is not finding commsTypeScan() in scope, then the ax12.h/cpp files may not have been updated when you merged the hardware folder. If you do not have AXRXScan() in the hikarikey.ino, then you should have 'Arduino-sketchbook-folder/hardware/arbotix/avr/libraries/Bioloid/ax12.h' with line#135 as "void commsTypeScan(); // checks which servos are TTL or RS-485 bus" and ax12.cpp with the renamed AXRXScan() starting at line #39.

tician
06-29-2014, 10:11 PM
Just realized that the more likely cause is that you did not remove the 'Arduino-sketchbook-folder/libraries/Bioloid', etc. versions of the libraries, and those 'sketchbook' level versions are overriding the 'platform' level versions that were updated in my fork. My fault for not remembering that is frequent installation location of the arbotix libraries.

darkback2
06-29-2014, 11:56 PM
That was the problem.

Everything is working now...Thank you for the help!

DB

darkback2
07-04-2014, 04:12 PM
https://www.youtube.com/watch?v=Lex6Oa9WtVc

kamondelious
07-04-2014, 07:45 PM
hahaha Awesome DB!

darkback2
09-16-2015, 10:00 AM
This morning in the shower I had a dream about Hikari and IK. I haven't been able to figure out some aspects of IK forever, which may sound silly, but I think I am starting to get some insights that may lend to a breakthrough of sorts. So...I am going to start with the following concepts and see if perhaps I have something...

So...To start with, I am going to set two variables. StrideLength() and BodyHight() The idea will be to have BodyHight be a constant set at the beginning, and everything else is based off of that. StrideLength is based on the Forward/Backwards position of the joystick.

So...A stride counter would count from 0 to 9. 0 = one foot is all the way forward, the other all the way back 4 would be the other end of the stride, and 5 and 9 would be the opposites...The count would start at 2 so that the feet start centered...

Each count would move the foot along the stride path determined by the stride length...

The body height would be how long the leg has to be in order for Hikari to maintain her bodyheight throughout the stride...

Now to make it happen...