Robot arm - feasible to use Teensy (USB-RAW-HID) so host can do PID?

[howiemnet]

I'm trying to revive an old Puma 200 robot arm:

So far, I've hooked up 6 off-the-shelf motor controllers that each read one of the joints' encoders, and do some simple PID to drive the motors.

To talk to the 6 controllers I've had to use 6 USB-serial adapters, which has been a pain in the arse to wrangle with as they show up on my Mac with different /dev/cu.usb-serialxxxxx names every time the machine's rebooted, and if I have to abort my program without the ports being closed properly, it takes a reboot before the Mac can talk to them again... but that's another story.

I want to change the way all this works. Ideally I'd like to handle the PID stuff myself, so I can fiddle with it - maybe implement the control loop as PV instead of PD, etc.

Wondering about the feasibility of moving all the PID stuff over to my host computer (the Mac), using a Teensy to handle the communications. So the Teensy's jobs would be:
- read and keep track of the 6 encoders
- send that data to the Mac as frequently as possible
- receive torque commands from the Mac
- use torque commands to generate 6 PWM signals to feed into the motor drivers (which would now be acting like pure amplifiers).

I've had a play with a Teensy and a tiny servo motor just to see if I could get a single channel working. It tracks the encoder, updates the Mac using USB-HID-RAW, and receives new PWM commands:

Benefits:
- I can work out which way gravity is, so I can build that into my control loop
- I can measure and deal with static friction relatively easily
- Mostly, it means I can keep all my difficult coding in one place (the Mac) rather than having to track down bugs in two separate architectures

My single servo test seems to work OK, but it raises some questions:
1. First, does this seem like a crazy idea? I know typically you'd have PID implemented on the microcontroller, rather than on the host computer (which ain't really designed for realtime stuff) - but the Mac is so damn fast I'm hoping it'll succeed just through brute force
2. Can I expect a reasonably regular (ie regular enough for PID) update rate? I was overjoyed to see around 1500 updates/second appearing on the Mac, but then noticed a lot of jitter; 2 packets coming in at once, that sort of thing. I can mitigate it by timestamping the packets (well, timestamping the most recent encoder change)
3. The longest period between updates over USB has been (in my tests) 2ms - so does a 500kHz PID loop frequency sound good enough for what I'm trying to do?

and lastly:
4. My robot arm's encoders are pretty high-res (2500ppr = 10,000 counts/rev); at top speed I'd expect to see something like 400,000 counts/sec coming in. Do you think the Teensy could handle 6 channels of this?

Rather than using interrupts, I usually check quad encoders in the main program loop using this funky bit of code I found:

long count=0;

void loop() {
...
count += readEncoder();
...
if (timeSinceLastUSBupdateInMicros > 1000) {  sendOutPositionDataByUSB(); }
...
}

int8_t readEncoder() {  
   static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0  };
   static uint8_t old_AB = 0;
   old_AB <<= 2;                   //remember previous state
   byte CURR_STATE = (digitalRead(0) << 1) | (digitalRead(1));
   old_AB |= CURR_STATE;  //add current state
   return ( enc_states[( old_AB & 0x0f )]);
}

I'd be needing to run this code 6 times in each cycle, with at least 400,000 cycles per second... but I don't know whether the USB comms will screw this up. (I don't think they're blocking, but I'll need to try and see)

Does all this sound crazy? Comments? Thoughts?

 

[iwanders]

That's a very cool project you're working on, my background is in robotics, I'm eager to see the results! I've not done motor control with the Teensy yet, but thought I'd chime in anyway.

In particular you have the question about doing the PID control loop on the Mac instead of on the Teensy. I would argue against doing so. I agree with you that the Mac is really fast, especially in comparison to the Teensy. However, the points 2. and 3. you make are very important. You are completely at the mercy of how OS X handles the communication over USB and whether or not your program is actually getting CPU cycles to calculate the control loop. The USB protocol probably has some requirements on timing, but it's not guaranteed to be processed in time and performance will probably get lower if your Mac is doing something or using an USB harddisk. Addtionally, the USB communcation at the Teensy is serviced with an interrupt, which with your current encoder code will result in missed ticks.

I'm not entirely sure how you got to the value of a 500kHz PID loop when the longest period between USB packets was 2ms. But 500 kHz is probably not achievable, which is not that much of a problem as the dynamics of your robot are probably fine with something like 100 Hz control loop. I think 500 Hz should be doable. This thread by Onehorse claims a 1000Hz update loop for Madgwicks AHRS or Mahony's sensor fusion algorithms. So I think the PID controllers should be fine.

I would implement the entire control loop in the Teensy. I think this will save you a lot of trouble with glitches and strange behaviour of which you cannot find the cause.
- I would check if IntervalTimer is capable of running the control loop. I would implement the control loop as a state space, which allows you to use matrix multiplication. Perhaps look at 28181-Eigen-library-for-linear-algebra-Teensy-3-1-Help-needed, I haven't used it, but it sounds good. Addtionally, if you run into issues about computational speed, it might be worth looking at how the Audio library does its calculations. Also, make it possible to set the control parameters from the Mac (just push the matrices over the serial port), such that you can easily change parameters without reprogramming the Teensy.
- Check if you can use td_libs_Encoder or the T3 library linked there for the encoders. You need to be absolutely sure that you do not miss any encoder ticks, if you do you risk that your measurement values drift away from the real values and end up slamming the robot through the table or something else. Your current code to read the encoders is guaranteed to miss ticks, especially when you have to do it for 6 encoders.

So basically; try to do as much in hardware/interrupts as possible (especially sensors and motor control). Run the control loop with intervalTimer or in your main loop. Dump the state back to the Mac over the serial port in your main (non-interrupt) loop. One more thing, be sure to include limits from the earliest tests on, just halt all motors when sensors readings are out of certain bounds and be sure to guard against integral windup when you are testing...
I hope this at least provides some pointers, as said - I haven't done anything like this - but this is how I'd go about it

 

[howiemnet]

Oops - yep, a naughty 'k' or two slipped in there - I meant 500Hz update rate (given that 2ms between packets of USB data seems pretty reliable).

Thanks for the heads-up on the Encoder library - these encoders are so high res (and seriously geared down) so I think I might be hitting the Teensy's limits trying to read 6 of them /and/ do useful calcs + 6 channels of PID + comms with Mac.

Difficult to know what to do: I'm trying to reduce complexity, as I've spent a good while now trying to wrangle the Motion Mind motor controllers to do my bidding (that fight's not over yet). I've pretty much started the code from scratch three times over now, and now I'm getting the hang of C/C++ and Xcode as an IDE, it'd be lovely to just do all my clever coding there, on the Mac, rather than ending up with lots of different bits of code everywhere that all have to sing in time.

I think if do end up trying to handle the PID myself, I'll probably have to buy another 5 Teensys, stick with one per channel. It'd give me the flexibility of implementing PIV, or cascaded loops, or try and get my head round Jacobians so I can feed forward some well constructed torque suggestions so things rely less on the PID stuff.

Thanks for the IntervalTimer suggestion - it may be best for me to stick with elapsedMicros so I don't have anything other than the encoder using interrupts. Timing jitter is the potential gotcha - need to have a reliable control loop timing - some testing will be needed methinks

Cheers

 

[howiemnet]

Thought I'd better have a go at doing my own PID with a Teensy, just to see what's involved, so I dug out an old servo motor, stuck it in a vice, and fed it with a sine wave: (No this is not an ad for Canon (!)):

https://youtu.be/YnLd4EneKX0

This is with a 500uS ( = 2kHz) PID loop, and I seem to get better results by updating my PID setpoint at the same rate. Haven't quite got my head round why you'd have cascaded PID loops with differing frequencies. Yet.

Everything (well, the encoder stuff and the PID stuff) is running off interrupts - my loop() function is empty. Seems to work OK. Ultra slow moves are a tiny bit juddery but that's a well-known challenge...

[man, 10 years ago it would have been impossible to go from idea to finished film in 4 hours... what a world we're living in, huh]

 

[iwanders]

I've pretty much started the code from scratch three times over now, and now I'm getting the hang of C/C++ and Xcode as an IDE, it'd be lovely to just do all my clever coding there, on the Mac, rather than ending up with lots of different bits of code everywhere that all have to sing in time.

I know the feeling... especially testing code that's got to run on another platform is cumbersome. But I believe it will be necessary to do the control loop on the Teensy, you will need some specific code on that anyway to be able to read the encoders and control the motor. You should be able to express your control loop as a state space representation later on, then you can just push new system matrices to your Teensies. I think that's the route I would take. Additionally you can relatively easily test your state-space code in a standalone program specifically to test it on your mac. I did this once, where you just use exactly the same cpp/h files as you will run later on the Teensy, only instead of getting the sensor values you read values from STDIN and output on STDOUT for example. I'm not sure how good C/C++ skills have gotten, so this might take some time to build, but using this approach might be worthwhile to be able to verify the workings of your algorithms. Especially since you can use another tool to generate the input and plot the results and such (Python for example).

I think if do end up trying to handle the PID myself, I'll probably have to buy another 5 Teensys, stick with one per channel. It'd give me the flexibility of implementing PIV, or cascaded loops, or try and get my head round Jacobians so I can feed forward some well constructed torque suggestions so things rely less on the PID stuff.

Perhaps a Teensy for each degree of freedom is the easiest approach yes, you'll have to do some test to figure that out. In regard to calculating the trajectories; be careful to correctly take into account any singularities, especially if you use inverse kinematics, such that your solution does not blow up
In regard of the Jacobians and such, take a look at 'A mathematical introduction to Robotic Manipulation' by Murray, Li and Sastry; the theory described seems well applicable to your robot. The math is quite involved, but it presents a very neat way to represent such robots composed of several rigid bodies and clearly defined joints in between.

(No this is not an ad for Canon (!)): https://youtu.be/YnLd4EneKX0

Looks good! You can always start making advertisements for Canon

Haven't quite got my head round why you'd have cascaded PID loops with differing frequencies. Yet.

Hmm, I'm not sure whether having a different sample period in your loops is ever a good idea. I wouldn't have the faintest idea how to show that your system is stable or that your controller is adequate. On the other hand, it probably just works as long as your control loop is 'fast enough'... regardless of the change of loop frequency or any irregularities in the feedback loop frequency.

10 years ago it would have been impossible to go from idea to finished film in 4 hours... what a world we're living in, huh

Ten years ago I wasn't busy with robotics or microcontrollers, but yeah, I totally agree that it's pretty awesome. I wonder if it will become even easier in the future, or that it will pretty much stabilize at this level.