Controlling an animatronics bust

[system]

I am currently developing a custom board to control a wowwee alive Animatronic Elvis bust. I will likely have many questions along the way, so I figured that I would define the scope of it here for reference (and suggestions.)

The input to the controller is Mini-SSC II compatible commands. This is a popular Serial Servo Controller. The command structure is very simple, for each command, there are three bytes <Sync = 0xFF> <Chan (0 to 254)> <Pos (0 to 254)>.

The output of the controller is 10 DC motors which control:

1. Eyes UD
2. Eyes LR
3. Eyebrows UD
4. Right Eyelid UD
5. Left Eyelid UD
6. Lip UD (signature Elvis lip curl)
7. Jaw UD
8. Head LR
   9.Neck swing LR (tilt)
9. Neck swing UD (nod)

Controlling these all is not directly straightforward.

7 of the motors inside the head postion (face motors) are tracked by potentiometers. Within the head is an ADC chip (SNAD01).
1 motor (Head LR.. rotate like "no") has a potentiometer, but it is not connected to the ADC within the head.
The final 2 motors for Tilt and Nod of the neck are linear actuators which incorporate optical encoder wheels and limit switches. The encoders are not quadrature, so there is no absolute position or direction information. They are like a tachometer.

The encoder wheels are connected at the point of the motor shaft. 4 slots in it (so, 4 pulses per revolution of the motor). The gear box is a 10:1 ratio, meaning it takes 10 rotations of the motor to move the final output shaft 1 revolution. The output shaft is connected to a lead-screw which converts the rotation into linear movement. For 1 revolution of the output shaft, there is 5mm of linear travel. If you do the math, you learn that 8 pulses from the encoder wheel equals 1mm of linear travel.

One linear actuator has a full travel of 64mm (tilt), and the other 32mm (nod). There are 4 limit switches located at the extremes of movements.

Controlling the other 8 motors is pretty straightfoward since they have potentiometers and we have absolute position from them. The only complexity there is communicating with the built-in ADC for 7 of the motors. That ADC is a Sonix SNAD01. I have already written a library for this for use with the Arduino. The 8th motor will just use an internal ADC channel of the AVR.

So, back to the linear actuators. I have devised some code to accurately track the position of the linear actuators using interrupts from the encoder wheel.

Overall the project will find it's final destination in an ATMEGA32 which has exactly enough I/O while coming in a DIP package. However, for ease of prototyping, I am using an Arduino (Uno and 2560.)

So far I have the Mini-SSC II serial parsing coded, the linear actuator code tracking position, and a driver library for the SNAD01 ADC. Now I need to piece it all together.

Here's how I think it will work:

The main loop will watch for new serial commands, and upon receiving one, it will send the chan and pos to a Move() function which will determine direction, magnatude of change, and control the motors.
An updatepos position will manage the various methods of gaining the current position of each servo.
A timer will run at 50Hz, which will refresh the motors with the last position given to ensure that they stay in position

Now what I am trying to wrap my head around is the overall code, the big picture, if you will. I think that it makes sense to simply create an array of each motors position, and when a new command comes in, it will put that new position in the array which will be fixed at the next update() call. All movements must be completed in 20ms. Does this make sense, or does it make more sense to start moving the motor immediately upon a new command and just leave the update() function to maintain?

Ultimately, I think this code will have much use beyond just this application and because of that, it is my intent to make it as modular as I possibly can. It could be used for any number of applications in which we need to gain servo control over a number of motors using the popular serial servo control interface.

I am not really 'new' to this, but I am not a great programmer. Hardware has always been my specialty. Typically my code is never 'clean' and often is buggy. What I would really like is some support and guidance (not hand-holding) in this project. I fully intend to put this code into the public domain for anyone to use, abuse, or otherwise.

Finally, my next project after this is a Wowwe Alive! animatronic chimp head. For that, I will be creating my own H-bridge and ADC board. I will also make that hardware (schematics, etc..) available for anyone to use for their projects. Since that will just use pots for every motor, there is actually a nice motor control shield and code that will be useful in just about any project.

Expansion of the code (if it becomes universally modular) would include closed loop control, open loop control, encoders (quadrature and tacho), potentiometers, gray-code encoders, etc... It would be the ultimate motor library.

So is anyone interested in helping?

[zoomkat]

What have you done so far, and what is the use of using an Mini-SSC II and an arduino?

[system]

I am not using a mini-ssc with the arduino, I am turning the arduino into a mini-ssc. But instead of controlling RC servos, I will be controlling the motors in the bust. Then the bust can be controlled from computer software as if it were a bunch of RC servos. Does that make sense?

What I have done so far is completely revere-engineered the bust, written a library to talk to the Sonix ADC and get the analog readings for it, written the Mini-SSC serial handling, written a nice, stable routine for tracking the position of the linear actuators, and begun on the initial shell of the code.

What I am having trouble with right now is:

1. Reliably controlling the motors to a position without using PWM (I may end up using it, but the original control circuitry simply couldn't have)
   [UPDATE: The motors are currently using a simple form of PWM in the original application as verified on a logic analyzer]

2. Focusing on the bigger picture. I keep wandering a bit between trying to control an entire frame at a time or just acting like a Mini-SSC II which will command one motor at a time

3. Keeping my code clean (it's a hack up at the moment) so that I can make it modular for use with other projects

Mostly just overall concept as I am one of those people that tends to get lost in the details and has trouble tying all the pieces together, and trying to figure out how to reliably control the position of the motors. I can write code, so I am not coming in needing my hand held. I am not great at it, and my code usually doesn't look very clean or is optimized as well as it could be. I guess what I need is to colaborate with someone that is good at the "big picture" and understands motor control methods better than I do.

As I said, the original application very unlikely used PWM. The motors are connected to a series of 3 latches connected on a 16 bit databus shared by flash, ram, etc... The best I could imagine they did was to send the motors used in each frame moving, then come back at some point and check on them. Keep them moving if they haven't reached their destination or stop them when they have.

I am beginning to suspect that they were not controlled in a closed-loop at all, and instead just given an amount of time to run relative to a position wanted. So, the software would determine how much time was needed based on the magnitude of change and the direction to move based on the current position. If we say they were refreshed at 50Hz (which they were actually) you get 20ms for all motors to complete their movements in a frame. If you checked on them every 1ms to see if they have reached position, that would probably work. But it is a hack way to do it.

In this application, it is not necessary to 'hold' the motors since it requires a lot of force to move them out of position manually. The gearing and mechanisms hold the position very well. Also, the eyebrows and mouth spring back to a nuetral position when unpowered and moving them just moves up or down from the centered position.

So, again, I can "hack it" to make things work for this particular application, but my overall goal is to make nice clean code that can be used in many applications where people want servo control over a number of DC motors.

You can think of it like I am trying to make a new servo library that controls DC motors instead of RC servos, and provides the popular Mini-SSC II interface for which tons of software has been written.

And the library allows for different forms of feedback (optical tach and limit switches, quadrature encoders, gray encoders, hall effect encoders, potentiometers, internal ADCs, and external ADCs, etc...) or just open-loop control using time.

I can pretty much already write all the code to control just this application using the timing/checking method mentioned above, but it will not be ultimately useful for other applications.

I think really all I need is someone to bounce ideas off and someone willing to help me clean up and package my code.

[zoomkat]

1. Reliably controlling the motors to a position without using PWM (I may end up using it, but the original control circuitry simply couldn't have)

Don't see how you would control a motor "to a position" just using PWM. Unless you use stepper motors, you will probably need some motor position feedback.

2. Focusing on the bigger picture. I keep wandering a bit between trying to control an entire frame at a time or just acting like a Mini-SSC II which will command one motor at a time

I've had a mini-ssc II for 10+ years and don't really see its function in your project (arduinos can control servos like a mini-ssc). If you just want to use the mini-ssc three byte protocol, I suggest you change that. I suggest you use a control protocol that uses normal keyboard ascii characters to make code debugging easer. There are arduino servo librarys that have better options for servo control.

I think really all I need is someone to bounce ideas off and someone willing to help me clean up and package my code.

What code? Seems to currently be your secret.

[system]

All motors already have feedback. As mentioned in the original post, 8 of them are using potentiometers, and 2 are using optical encoders. So, that is how I will track position.

Also, as stated in my original post, my project is to turn 10 "non-RC servos" into RC-Servos as far as Mini-SSC is concerned. My controller IS the mini-SSC, but it will be controlling 10 raw DC motors with feedback instead of RC Servos. On the computer side, it just acts like a normal Mini-SSC controller. On the other side are 10 raw DC motors with their forms of feedback.

I am not dealing with controlling actual RC Servos, I am MAKING RC servos out of motors and pots, and motors and optical enocoders.

Position will require some form of closed-loop control, PWM is required to control the speed of the motors while moving into position to avoid overshoot.

I am here looking to discuss logistics. The code is not completely done yet (and I described what parts I am currently working on and what parts I have already done.) That is why I am here. I apologize if you were looking for completed code. There is only snippets and several sketches testing out various parts of the entire project. I am currently using an ascii interface for testing of the various parts. That will be removed in the final code and replaced with the Mini-SSC control interface.

In the end, the computer will handle the animation, and my controller will handle the actual translation of that animation into motor movements. The finished code will be useful for controlling all sorts of animated toys (elmos, furbies, you name it.) Anything currently run on gearbox motors with some sort of feedback.

Essentially, this will turn any motors/feedback into RC servos as well as providing the Mini-SSC controller all wrapped into one board.

[system]

I am mystified by how every time I present this project, the responses are as if I am speaking a foreign language that nobody speaks but me. So, here are pictures:

The first is the typical way to control something using a Mini-SSC servo controller using standard RC servos.

The second is a mini-SSC, the servo driver board, and a raw DC motor with feedback.

The last one is my concept, combining the Mini-SSC and servo driver into one controller.

[system]

The point is to control stuff like these as if they were standard RC servos using popular animation control software that works with the Mini-SSC controllers. All I am doing is combining the Mini-SSC controller with servo drivers in order to turn ordinary DC motors with feedback into servos:

[system]

I apologize if I come off as a bit snide in the above posts, but I just don't understand what is so complicated about the concept. It is so simple. Implementation is the difficult part and that is what I need help with. But every time I talk about this, nobody understands the concept and it is very discouraging to even post about it. I realize the posts are a bit long, but its as if nobody reads it and just asks me AGAIN what I am doing after I just spelled it all out. Coming back to see a response and finding that all it is is asking is for you to describe your project once AGAIN is a bit frustrating. Everything is in my initial post; everything. Maybe too much information, but that is because I have had to do this over and over again answering the same questions. But it nevers goes any further than that.

I'm not picking on you, Zoomcat. Everytime I post about this project, it happens. I hope the pictures help make it more clear and discussion can move on to actually implementing this. I am looking for help, this is not a post with completed code all packaged up. When I am done, I will post all that for anyone to use to their hearts content.

[terryking228]

Maybe the problem is that these smart tekkie guys are too young to remember the anxiety associated with asking a girl to dance to Heartbreak Hotel the week it came out.

I don't have answers here, but I think what you are trying to do is to make each actuator with it's feedback act as if it was a traditional position servo. But you are just going to pass it position data, not actual pulses. Is that right?

Seems you could start with just one motor-pot section and get that working. Then make that code work for any actuator.

Later you have the challenge of writing high level coordinated moves.

Like "Smirk" ?

[system]

terryking228:
Maybe the problem is that these smart tekkie guys are too young to remember the anxiety associated with asking a girl to dance to Heartbreak Hotel the week it came out.

I don't have answers here, but I think what you are trying to do is to make each actuator with it's feedback act as if it was a traditional position servo. But you are just going to pass it position data, not actual pulses. Is that right?

Seems you could start with just one motor-pot section and get that working. Then make that code work for any actuator.

Later you have the challenge of writing high level coordinated moves.

Like "Smirk" ?

Yep. You've got the general idea. Obviously my code will provide the pulses to the motor. But that will all be handled within the microcontroller and hidden from the end-user. As far as the end user is concerned, they are just using a Mini-SSC controller as if they are controlling standard RC servos. over the serial connection, you just command a channel to a desired position and my controller handles the ugly bits of actually doing that.

[zoomkat]

In your third schematic, the "my controller" could probably be replaced with an arduino/mega controller and a number of h-bridges smilar to below to drive your motors.

http://www.ebay.com/itm/L298N-DC-Stepper-Motor-Dual-H-Bridge-Drive-Controller-Board-Module-for-Arduino-/380520063503?pt=LH_DefaultDomain_0&hash=item5898c3620f

[system]

The "my controller" IS my arduino and associated hardware. I already have the h-bridges, the pullups, proper caps and filtering, the power supply, etc... The hardware is all done (the easy part for me.)

I am working on the software and that is where I need help.

[system]

ok, I think I have found a better way to approach this... since I am currently trying to tie all the pieces together and am really asking for birds-eye view help, it makes sense to start with a flow chart. That is really what I need help with, generically. Individually I will need some help with some of the pieces, but for now it is the "big picture" part of it.

I have attached the beginning of my flow chart. If someone would be willing to look it over and help me get it right, I would appreciate that. I have attached both a picture of it and the actual flowchart file drawn in the freeware program "Diagram Designer" Diagram Designer

elvis_flow.ddd (1.12 KB)

[system]

Just refreshing my code as I have added a bunch more and am starting to link things up better. I may have bungled up the PID routine as it was a bit confusing which variables I needed to track externally between calls. If anyone sees glaring errors, please let me know. It's not completed code, so I am aware that it won't just "run" as it is.

Elvis.c (8.83 KB)

[system]

Just of the top of my head I would initialize the motors in a loop. I'm pretty sure there is a nicer way to do it, but a loop would definitely simplify it a lot.

Edit: you might also want to look at the PID library
http://playground.arduino.cc/Code/PIDLibrary

[system]

Thanks. I have actually looked at the PID library and it seemed too complex for what I am trying to do, especially with trying to tie in a bunch of motors. It could be that I am just not understanding it fully. if someone is willing to help me figure out how to incorporate it, I would be extremely grateful.

As for the loop suggestion. That is the idea. New, incoming serial commands get loaded into the array. A loop pulls the commands out of the array and updates the motors. If no new commands were loaded, it just refreshes the motors to the last position they were in the array (in case they had moved somehow.) That generically holds the position. Upon first initialization, I load all the default positions into the array for neutral. And the head will move into a nuetral state on the next update.

So, I am technically only commanding a single motor at a time but instead of directly moving the motor I load the command into an array, and a timer triggers the update routine and refreshes the motors.

[system]

The way I would approach this is to create an object for each servo that you want to control. Each servo object would use an input position sensing object to determine the position of the output, and contain a PID instance (or other feedback algorithm if you prefer) to control the power/direction output requirements.

If you need to support different types of position sensing (I think you implied that in a different post) then use a class hierarchy with a base class providing the interface and common functionality, and derived classes providing the alternative implementations.

Design your servo class with a method that can be called periodically to read the current position, compare that to the commanded position, perform the feedback calculation and generate and send the output control.

I'd represent the error input to the PID and the power and direction outputs using signed integer values.

In setup you would create and configure the required servo instances and store then in an array, and then in loop() you would call each servo's update method at regular intervals. If you want to be able to receive servo positioning commands too then you'd configure the interface for that and in loop() you'd also check for any incoming commands, parse them, and send them to the relevant servo. It doesn't sound particularly complicated, and the most critical part would be designing the class hierarchies for your servo and input position sensor so that you avoid code duplication and are able to treat the different implementation types the same once you have instantiated them.

[system]

PeterH,

That pretty much sounds like what I am aiming for in my attached code. The beginnings of it anyway. I still have to determine how to handle feedback structure. However, I am using structures instead of classes. I am really unfamiliar with the concept of classes just yet.

I like the way you are suggesting, just trying to figure out how to implement it. You say error input to my PID calculations, but isn't it the other way around? Doesn't the PID calculate the error as a result? Then I use the error to determine the direction to move and the speed (in another routine, or just do that in the PID routine itself?)

[system]

No, you calculate the error and feed it into a PID routine. Libraries may do the calculation for you, but the versatility of the PID is that it is irrespective of the inputs/outputs. It is really just a zero finding scheme on the error function.

[system]

laadams85:
No, you calculate the error and feed it into a PID routine. Libraries may do the calculation for you, but the versatility of the PID is that it is irrespective of the inputs/outputs. It is really just a zero finding scheme on the error function.

hmm... according to my notes from reseearch, this is the PID formula:

error = P * Kp + I * Ki + D * Kd);

P, I, and D

are calculated like this:

P = Current position - Set position
D = P - Last_P

So, perhaps we are defining error differently? As my notes have error as the entire PID formula.

Is error in your terms equal to currpos - setpos?

Thanks