## December 21, 2016

### Some Motor Math and Simulation

While waiting for boards and parts for the motor dyno to show up, I got distracted by doing some motor modeling and simulation. Having a decent motor-modeling platform is an incredibly useful tool, because it lets you quickly test out motor control stuff in simulation, before actually implementing it on hardware and testing it for real.

I wanted a model which could encompass controller effects like sample time,  switching, PWM resolution, A/D resolution, sensor noise, etc, and generally be, well, as general as possible, so that I can use it for arbitrary motors and motor controllers.  I started out doing most of the model in the D-Q frame, but realized to get what I want, it really makes more sense to do all the motor modeling at the phases, and then just use the transformations as necessary for control purposes.  You know, like how motors work in real life.

In working through this problem, I finally sat down and did a bunch of motor modeling math by hand, which I should probably have done a while ago.  Fortunately Bayley  was semi-coincidentally working through similar problems at the same time, to do some optimization for controlling hybrid car motors for a go kart, so we did some back-and forth to sanity check results.

Rather than putting it all here, I spent the time to write up a nice Latex-ed document with my notes, which can be found here.  This is just the analytical portion of the modeling - i.e. coming up with the equations that actually get solved by a computer.

Here you go.

With that part of the problem out of the way, I could actually implement the equations in MATLAB, and do interesting things with them.

Here are a couple useful sanity checks for making sure the motor equations were behaving like I expected them to once implemented, and some other experimentation.

Open Circuit Back-EMF
i.e., spin the motor with the phases floating, and measure the voltage between terminals.  This is the classic test I use all the time to quickly estimate motor parameters of random motors.  The next few plots are for the hybrid car motor being used in the go-kart.

Short-Circuit Torque Vs Speed
i.e. Short the motor terminals together and spin the motor.  For low speeds, the motor looks behaves like a resistor, so expected result is that there's a damping torque proportional to angular velocity.  But as speed increases, the motor behaves like an inductor, not a resistor, so damping should drop off to a constant value.  The crossover speed should be right around the motor's R/L break frequency.  And so it is:

The plot above, and those following are per pole pair.  This is a 3 pole pair motor, so actual torque and speed get multiplied and divided by 3 respectively.

PM and Reluctance Torque vs Current Angle
Looking at the permanent magnet and reluctance components of torque vs the relative angle of the stator current to the rotor, the expected plot looks like this.  Here's the result for the same hybrid motor (this plot's actually a little out-of-date, but it gets the picture across)

Auto-Generate "Optimal" Current Trajectories
Unlike for the classic surface-permanent-magnet "brushless" motor, where you typically want to just put all the current on the Q-axis (neglecting field-weakening), internal permanent magnet motors (like those in most hybrid cars) needed to be treated differently.  I wrote a script which sweeps operating speeds, and for every speed and current setpoint finds the current phase angle which produces the most torque.  A pretty cool thing about this method is that it automatically produces the correct field-weakening trajectory for operation above base speed. No need to think about how to move through current circles and voltage ellipses.

Here's the whole positive-torque, positive-speed operating region, for the same hybrid car motor as above, with a 160V bus and 200 phase amp limit.  This map can be used to generate a lookup table for motor controller - for a given speed and torque command, what are the best d and q axis currents.  In this case I've formatted it as current magnitude and phase, instead of d and q current, because that makes the table a little more convenient to generate.

(currents numbers are larger than 200A peak just because of the way I've defined my dq0 transform, but this corresponds to 200 peak phase amps)  The dark-blue regions are unreachable operating points.

Switching Effects
I added a naive implementation of switching to the model.  I implemented this very much like how a microcontroller does PWM, with an up-down counting counter, like this.  Unfortunately this method means I have to simulate way more timesteps than necessary - 40 kHz pwm with 12 bits of PWM resolution, this method has to simulate 1/(40,000 * 2^12) steps per second, which is way faster than the actual dynamics of the motor behave.  There are cleverer ways around this, but I haven't gotten around to trying anything else yet.

Here's what phase currents look like with 40 kHz switching on one of my MultiStar Elite 5208 motors.  You can see the fuzz on top of the sinusoids from the switching.

Zooming in, you can see the double-peaked ripple caused by the center-aligned PWM:

Here's a link to all the repository of MATLAB code:  Beware, it's kind of an organizational disaster right now, but eventually it'll get organized and commented better, I promise.

Don't worry, I'll be back with more dyno progress and more building physical things soon.

## October 31, 2016

### Small Motor Controllers, Round 2

A few minor differences on this revision:

• Switched to lower-ESR through-hole electrolytic capacitors, from surface mount ones
• Added a MCP 2562 CAN transceiver, to make it more straightforward to build robots with lots of motors on them.
• Moved all the connectors to the edges of the board, and made them right-angle, for neater wiring
• 2 oz copper instead of 1 oz.
Stack o' boards from 3pcb:

One assembled (minus electrolytics):

And fully assembled attached, with a motor and absolute + encoder position sensing, programming header, and serial port attached:

And some files:

## October 9, 2016

### Motor Dyno Updates, First Tests

It's somehow already been over a month since I last posted motor dyno documentation, so a lot has happened in the meantime.

The basic architecture of the system looks like this:

Most importantly, the absorber and the test motor are run off the same power supply.  This means that the power supplies only have to output the losses in the system, not the absolute power of each motor.  And since the buck converter powering the test motor is synchronous, the dyno supports full 4-quadrant operation.

Absorber Testing

Here's the test setup for getting the absorber controller up and running.

There's a lot going on there.  The absorber is driving an even bigger brushless motor, which has a 3-phase rectifier on its phases.  The output of the rectifier is fed into the MITERS Dynaload to dissipate the power.  The torque sensor is fed into a USB DAQ (MCC USB-1608FS) and logged and plotted by some python scripts, and commands are sent to the absorber controller over serial from my laptop.

I had a crisis for a couple days while testing the absorber.  Inexplicably, I measured about 2 times less torque than I expected for a given phase current.  I went through my code and triple checked all the hardware gains.  I physically measured the torque sensor gain with a lever and a weighing scale, and it perfectly matched its datasheet.  Similarly, passing known current through the current sensors produced the expected output.  I checked the absorber's back-emf on the scope for the nth time, and it checked out.  After many hours of confusion, I realized that I was editing  the DAQ-sampling function in an old version of the python file, which was no longer being called.  So my display was showing just the raw torque sensor voltage, and not multiplying it by the 2.26 N-m/Volt scaling I thought I had entered.  Saved.  For a brief while I was worried I had somehow been fundamentally misunderstanding something about motors this whole time.

Buck Converter

The test motor is powered through a synchronous buck converter.  I targeted a power range of 200+ amps at 5-48 volts, to make it useful for powering a wide range of motors and controllers.  A buck converter of that size is a non-trivial design problem, so in the interest of having a working dyno, I skipped all the buck converter designing and hacked something together out of used Prius power electronics.  Everything is extremely oversized and poorly suited for my needs, but the parts are so oversized I at least know I probably won't blow anything up.  Eventually I'll get around to designing a more appropriately sized buck converter out of reasonable components.

The switching is done by a Prius half-bridge module, which in the car is used for the boost converter which steps up the battery voltage to 500-ish volts to run the motors.  Nick Kirkby has an excellent writeup of the boost module, and how to turn it on.  Output voltage is measured with an isolated voltage measurement module Peter found.  DC current is measured with an Allegro current sensor similar to the ones I used on the GigaTroller.  Eventually I plan on replacing this with a LEM (closed loop hall) current sensor to squeeze a bit more resolution out.

A Nucleo receives voltage commands over serial to set the duty cycle of the buck.  Right now the buck operates open-loop - I simply haven't had a chance to close the voltage loop yet.

Debugging the buck:

User Interface

To control they dyno, I've been putting together a UI which runs on a computer to control all the setpoints and log data.  Right now, it looks like this:

The GUI is built with QT in Python, with the library pyqtgraph (which is built on top of PyQt) used for the real-time plotting.  Data to and from the absorber controller are passed over serial, and the DAQ is sampled with a Python wrapper for MCC's Universal Library.

The dyno has a couple different operating modes right now:  Road load and speed control.

Road load simulates a mechanical load - an inertia, and two friction coefficients (coulomb, viscous).  It does so by measuring the torque on the torque sensor, computing what the net torque should be (measured torque - viscous friction - coulomb friction), computing what the acceleration should be (net torque/inertia), and integrating the velocity command with the acceleration.  This loop isn't incredibly responsive, as it only runs at the 100 Hz of the python gui, but it does work pretty well.

Speed mode just sets the velocity command to whatever value you want.

I plan on adding another operating mode which takes a CSV timeseries of operating points (speed setpoint, buck voltage setpoint, test motor command) and following that, to make running tests like  efficiency maps easy.

Here's a video demoing the inertia simulation of the road load mode:

Electrical Noise Problems

The torque sensor alone is an incredibly precise instrument.  There's less than 1/1000th of a Newton-meter of noise on its output, and it (when plugged into a nice 16-bit DAQ) can easily resolve few-1000'ths of a N-m torques.  Which I find incredibly impressive for a sensor which can measure >10 N-m peak.  The story changes, however, when all the power electronics in the dyno get turned on.  Plugging in the inverter for the absorber increased the noise on the torque sensor to a few 100'ths of a newton meter.  And plugging in the buck converter gave me another order-of-magnitude of noise.

Fixing the buck converter noise was relatively straight-forward.  The logic ground of the buck converter was connected to the ground of the DAQ through a USB cable.  Since the serial communication to the buck can be slow, and is only in one direction, I just stuck an opto-isolator on the TX line of the st-link I'm using for serial communication.

Inverter noise was much trickier.  The basic symptom was that, as soon as the inverter had power on the bus, the noise appeared.  The bus voltage was completely isolated from the DAQ and torque sensor, as was the logic voltage.  The noise appeared even when the inverter logic wasn't plugged in over USB.  So there was literally no shared ground or power between the DAQ/torque sensor and the inverter.  Fortunately, Kramnik was around while I was trying to debug, and was able to almost completely solve the problem.  It seems the problem was the motor windings capacitively coupling to the motor housing and metal frame of the dyno.  The solution was to 1:  put a common-mode choke on the windings, right next to the inverter, and 2: add shielding braid around the windings, which is connected at each end to the motor housing and inverter heatsink.  Doing this reduced the noise down to ~4/1000ths of a newton meter, which is definitely  acceptable, although there's a little room for improvement.

Magmotor Testing

To test everything, I've been using some big brushed DC motors, including one of the combat-robotting-favorite Magmotor S28-200.

These plots were generated in the simplest way possible: have the absorber simulate an inertia, and apply a voltage step to the DC motor.  Each line in the 3D scatter plot corresponds to a voltage step.  This only worked up to ~20 volts, at which point the initial power draw tripped the power supplies.  Either current limiting on the buck, or a current-controlled driver between the buck and dc motor would fix this.

Here's the same information, but re-scrambled for power vs speed.  Keep in mind, this is actual mechanical power coming out of the motor, not power going in.

In the long run, the more intelligent way to produce an efficiency map would be to automatically sweep torques and speeds at steady-state, by controlling the absorber speed and the torque command to the test motor.  The data above is a little bit wrong, because the test motor is accelerating - the measured torque is going to be off by the angular acceleration times rotor inertia of the test motor.

Such science.  Next steps include adding more automation to the test process, so I don't have to sit there and babysit the dyno to run tests.

## August 29, 2016

### Introducing the GigaTroller

Enjoy this unprecedented two posts in one day.  It probably won't happen again.

The GigaTroller is the motor controller designed for running the absorber on the motor dyno, named for the absurd mosfets it uses, the IXYS MMIX1F520N075T2 TrenchT2 GigaMOS HiperFET. What a mouthful.

This is an rediculous mosfet.  Rdson is quite good but not spectacular (given its size), typically 1.2 mΩ on a 75V FET, and it has quite a lot of gate charge, 550 nC at 10V on the gate.   However, mechanically it's pretty awesome.  It's some fancy IXYS surface mount package, 1" square, with an isolated metal pad on the top.  Thanks to the presumably gigantic die and excellent package, it's rated for 500 A at 25 C, 1700 A pulsed, and 830 (!) watts of package dissipation at 25 C.  It's also a whopping $17 per FET on Digikey, but fortunately Bayley found some rails of them for$5 each on Ebay.  At $17 they would be silly to buy but it's actually a pretty reasonable amount of transistor for$5.

Nominally I'm designing this controller for 200A, 50V operation, because that's all the dyno absorber can do.  This should be very safely within the ratings of these transistors.

A tricky thing about designing a high current controller like this is designing a PCB that can actually handle that current.  To make the controller as compact as possible, I designed it to use custom surface-mount busbar, which I waterjet cut out of copper plate and reflowed to the PCB.  Another interesting feature of this transistor package is that it physically has a cutaway underneath the package between the legs.  I'm using that space to route the busbar beneath the transistors, instead of next to them.

Here's a board from 3PCB and a pile of busbar.  Although I didn't ask for gold plating on the PCB, I got it for no extra cost.  Presumably my boards were tacked onto a big batch that was getting plated or something?

It took two tries to solder the bus bar.  First time around, I tried clamping everything at once and baking it in an oven, but that ended up warping the board and turning out poorly.  At Peter's suggestion, I soldered the busbars one-at-a-time, fixturing them with steel binder clips and heat-gunning them with solder paste underneath.  This worked out beautifully:

After sanding off excess solder and oxidation:

Here's the power board populated with FETs, Allegro current sensors, gate pulldowns and gate zeners, and one 22 uF film cap per leg.  After reading this paper, I decided to try out using just a small-ish set of film caps instead of the the huge pile of electrolytics I'm used to seeing.

Note the bus bar routed underneath the transistors:

Logic and gate drive stack on top of the controller, next to the capacitors.  An STM32F446 does all the motor-control stuff.  I know the F3 series are probably a better choice for motor control things, but I had a few F446's left lying around, and this way I could just port all my FOC code over from my small controllers with minimal editing.

The power supply section of this features 5 isolated 15V DC supplies - one for the low-side gate drivers, one for each of the three high-side gate drivers, and one one for the logic.  The gates are driven by 2.5A gate drive optoisolators.

Bayley actually designed the power supply and gate drive section of this for different motor control project of similar power level, so I borrowed his design.  A 555 generates a square wave, which is fed into the inputs of a dual inverting/non-inverting gate drive IC.  The gate driver swings around the primary of a 6-winding transformer, the 5 other windings of which are rectified to generate the 5 isolated 15V supplies.

The gate drive IC driving the transformer gets extremely warm, but its power dissipation is still within spec, and it's now operated 10's of hours at least with no problems.  When the controller gets mounted to a heatsink I'll make sure it gets some airflow too.

I used ethernet connectors for programming/serial and position sensor, because they're nice cheap, locking, small-footprint connectors.  And they're matte black.

To get the controller working, I spent a few days figuring out how to generate complimentary PWM signals with software-defined deadtime.  That was the only significant code change, though, other than re-doing the current loops as usual.

So far I've pushed the controller to 45V, 100 phase-amps entirely without drama.  The transistors get a little warm.  Here's what bus voltage looks like at ~80 phase amps.  Note this is running of a bench supply with several feet of wire between the supply and controller.  At 41 volts on the supply, the sharp spikes hit ~7.5 volts above nominal, and the bulk ripple is a couple volts.

Here are the Eagle files, if for some reason you want to build one of these:
Power Board
Logic Board

### Small Motor Dynamometer: The Beginnings

Where "Small" means around 6-7 kW.

I've been ruminating on building a small motor dyno for a while, since spending most of last summer working with really big dynamometers.  While strategies like listen to the strange noises your motor is making to find glitches in your control loop and use your hand to load the motor and see how it behaves are surprisingly informative (and certainly help develop some intuition), they don't really give you any quantitative information about motor performance (although I'm learning to calibrate my wrist as a torque sensor).

For those unfamiliar with what a dynamometer is and does, it's basically a system that lets you characterize a motor/engine/other torque-producing device.  The output of the motor is coupled to speed and torque sensors.  Speed times torque gives the actual mechanical power coming out of the motor.  At the same time, voltage and current going into the motor can be measured, giving the power going into the motor.  On the other side, another motor, called the "absorber", acts as a generator to absorb all the power put out the the motor being tested.

For several months my ebay home page was filled with a variety of different searches for torque sensors, and eventually I found an excellent deal on a used S. Himmelstein DC-operated torque transducer, good for 11.15 Nm of torque and 6000 RPM.  It's almost perfectly matched to one of the 80-100 sized brushless outrunners, which is just about the biggest motor used in the various contraptions my friends and I build.  The DC-operated versions of the Himmelstein torque sensors are especially convenient, since they just output analog DC voltage proportional to torque, which can be shoved into a DAQ for logging.  Lots of the ebay-torque transducers require extra (expensive) circuitry to run the rotary transformers which interface with the strain gauges.  I'm really impressed with this torque sensor - plugging it into a 16-big USB DAQ, I can easily distinguish 0.001 Nm changes in torque on the sensor, which is 1/10,000th of full scale.

For the absorber, I started out with the original 80-100 from the electric trike, which Charles gave to me 4 years ago.  When it originally stopped working due to some shorted windings, I pulled off the original windings and then it sat around until now.  I did the rewind with 4 parallel strands of 16 AWG, 200 C rated magnet wire, five turns-per-tooth, wye terminated.  There's still a little space between the stator slots, which I wanted for improved cooling (there will be a blower to force air through the stator).

After the rewind, the motor sits at:  13 mΩ  line-to-line, and a torque constant of 0.062 N-m/A.

I made a base plate out of a slab of 3/4" thick aluminum I found in the trash.  The motor shaft was carefully aligned to the shaft of the torque transducer, and they were coupled with some custom-machined lovejoy couplings (I bought the flexible inserts, and machined the aluminum bits):

Here's the coupling pair.  The absorber-side of the coupling bolts directly to the bolt pattern where one would normally attach a propeller mount, and locates on the 12mm shaft which passes through the length of the rotor.  On the torque transducer side, the coupling is keyed and axially retained by a set screw into the key.

On the input-side of the torque transducer, there's an ER-32 spindle, to make it easy to couple in arbitrary motors.  I got an cheap ER-32 straight shank collet holder on ebay, reamed out the inner bore, added a split, and made a big split collar to clamp the whole assembly together.  I measured runout on the inside of the spindle taper at one thousandth of an inch, which is definitely good enough.  The test-motor will be mounted on a compliant mount, to avoid over constraint.

The feet of the absorber and the torque sensor each locate to the base with three dowel pins, so I can repeatably assemble and disassemble the whole thing.

So that's a brief introduction to the motor dyno.  Next post (which will be soon) will go into the motor controller I'm using on the absorber, as well as some initial testing of the whole thing.

## August 26, 2016

### Desktop Inverted Pendulum Part 2: Control

With the mechanical bits out of the way, it's time to go through the process of controlling the inverted pendulum.  Brace yourself for babble about transfer functions and other controls stuff.  Video at the end.

First on the agenda is getting the brushless motor to actually produce torque.  While I'd already done 99% of the work required, I still had to redo the current d/q axis current loops.

After some decoupling the motors d and q axes look like independent RL circuits, and are thus easily tamed with PI controllers.  Since the control loops are running on a microcontroller, it makes sense to design them directly in discrete time, rather than in continuous time (although designing in continuous time, assuming you sample fast enough for it to not make a difference, and converting to discrete time can be valid too).  To do so, first find the zero-order hold equivalent of the RL circuit - basically, how the dynamics of the circuit appear to the microcontroller sampling at a fixed rate.

Remembering good ol' 2.004, the transfer function of a series RL circuit from voltage to current is $$\frac{I(s)}{V(s)} = \frac{1}{(Ls + R)}$$ which has a frequency response like this:

Now let's convert that to discrete time.  Some amount of algebra (or numerically in one line of Matlab using c2d) later, you get:
$$\frac{I(z)}{V(z)}=\frac{\frac{1}{R}\Big(1-e^{\frac{-R\dot{}Ts}{L}}\Big)}{z -e^{\frac{-R\dot{}Ts}{L}}}$$
Where R and L are the same resistance and inductance as before, and Ts is the sample period.  The frequency response looks a lot like the continuous time version, except that the phase (and magnitude, but to a lesser extent) gets kind of wacky towards π radians/sample.

Okay, now to design a discrete current loop.  I like to write a PI controller in the form
$$\frac{U(z)}{E(z)}=k\Big(1 + \frac{ki}{z-1}\Big)$$
which has a pole at z=1, a zero at z=1-ki, and a high-frequency gain of k.  This way, changing k only changes the loop gain, and changing ki only changes the zero location, unlike the typical parallel PI expression.

Set if we set the zero of the controller on top of the pole of the RL system, then the controller * RL just looks like an integrator.  This can be done by setting $$ki = 1-e^{\frac{-R\dot{}Ts}{L}}$$

Now just to choose a loop gain that achieves whatever crossover frequency (ωc) you want.  Let's say something like π/8 radians/sample, to be conservative, because, to quote a professor, "There are always higher-order poles".  This should be reasonably conservative and give us ~78 degrees of phase margin, as you can tell from the pole-zero map of the return ratio (in this case the circuit times the controller, which is just an integrator):

From the same geometry, you can see that crossing over at ωc = π/2 gives you 45 degrees of phase margin, etc.

We can calculate approximately the loop gain required by doing some more geometry on the bode plot of the return ratio.   We want the loop to cross over (magnitude = 1, or 0 dB if you prefer dB for some reason) at  π/8 radians/sample.  We also know that, at the frequency of the controller zero and circuit pole, the plot has a magnitude of k/R.  Since the slope of the magnitude plot is -1 (on a log/log scale), we can easily calculate the k required to join those two points with a line of the correct slope:

So k should be ...
$$k=r\Bigg(\frac{\omega_{c}}{1-e^{\frac{-R\dot{}Ts}{L}}}\Bigg)$$
This won't be quite exact, because the slope of the magnitude plot isn't perfectly -1 between those two points, but it gets you pretty darn close.

Here's some MATLAB plots verifying the frequency and step response of the current controller designed this way.

Hey, I just worked that all out symbolically.... that means that if my motor controllers could measure resistance and inductance on their own, they could auto-tune their current loops....(also note this isn't specific to current control of RL circuits.  It can just as easily apply to any 1st-order kind of system, like velocity control of a motor (inertia -> inductance, damping -> resistance), etc)

With torque (well, current, but close enough) control out of the way, next step is stabilizing the pendulum upright.  While there's always the "I heard PID controllers were good so lets throw PID at it and plug in arbitrary constants until it works" strategy, which people on the internet seem to like, that's not very interesting.  There are lots of reasonable approaches, but the one I've mostly been playing with is full state feedback with LQR-chosen gains.  While I've taken a class on state space modeling and controls, I had never actually implemented such a controller in hardware, on a microcontroller.

Like most good controls problems, the first step is coming up with a model of the system to design the controller around.  For the near-upright position, the furuta pendulum system can be greatly simplified from the extremely unpleasant full equations of motion.  Rather than doing the proper thing and linearizing those about the pendulum-up position, I was lazy and converted my rotary inverted pendulum into a cart-pole, and linearized those equations about vertical.  Except that has been done for me dozens of times.

The quantities that need to be converted to go from the rotary inverted pendulum to cart-pole:

The only trick is that care needs to be taken doing this conversion- both going from physical device to the model, and from controller gains back to the physical device when implementing the controller.  In the model, the units are meters, kilograms, and Newtons, whereas on the device they are radians, kilogram-meters^2, and Newton-meters respectively.  This model is only valid for small rotations on both axes, but it seems to be good enough to stabilize the system well.

The upright-controller, for reference states of zero, looks like this: $$\tau = 0\dot{}\theta - 0.1\dot{}\dot{\theta} + 2.52\dot{}\phi + 0.15\dot{}\dot{\phi}$$ where  tau is torque, theta is the angle of the base, and phi is the angle of the pendulum (from upright)

Something to note:  I put no weight on the location of the pendulum (i.e. the x-position of the cart in the cart-pole system), so the gain in front of theta is zero.  This was mostly an aesthetic choice - I thought it would look nice if the pendulum slowly coasted to a stop after being disturbed.

Positions are taken straight from the sensors.  Base velocity (theta dot) is measured with this trick, and pendulum velocity is the (heavily filtered) derivative of angle, since I can't time edges on the Allegro A1335 position sensor used for pendulum angle.

Here's the disturbance response (i.e. me flicking the pendulum) of pendulum angle with two different controllers, as compared to the model used to come up with the controller gains.

To actually get the pendulum to the upright position, I (roughly) used an energy-shaping controller.  Check out the 6.832 notes here for a more detailed description of how that works.  The basic idea is to apply a torque to the pendulum only when it adds energy to the pendulum, and only do so while the pendulum has less than its upright energy)  In other words, the sign of the torque applied to the pendulum should be the same as the sign of the angular velocity of the pendulum, so that torque times angular velocity (i.e. power) is positive.   By doing the opposite, you can remove energy from the system, if you want to swing down from upright, for example.  At the end of the day, my energy shaping controller looks like:
$$\tau{} = K\dot{}cos^4(\theta{})\dot{}\dot{\theta{}}\dot{}(9.81(1-cos(\theta{}) )-.0075\dot{\theta{}}^2)$$
Tau is torque, theta is the pendulum angle.  K is a constant found through experimentation.  The 9.81-.... term at the end is proportional to the additional energy needed to bring the pendulum upright.  The 4th power cosine term has the effect of skewing the controller output towards the bottom of the swing, when the pendulum velocity is the greatest.  This term was actually sort of an accident.  By some miracle, the swing-up controller worked the first time I implemented it, without that term.  But when I later went to write a linear swing-down controller, I realized that, at the bottom of the pendulum's swing, my angle measurement was rolling over.  The angle jump of 2 pi caused a spike in velocity which gave a "kick" to the energy-shaping controller.  Once I fixed that problem, the gain of the controller was too low to do anything, so I cranked up the gain.  I found this new controller to not be very effective, and would mostly cause the motor to spin around a bunch.  So I added the 4th power cosine term to add back the "kick" back to the bottom and that fixed it.

Here's what the angle and torque command during swing up look like, before it kicks into the linear controller.  This is actual measured data:

Finally, there's a simple state machine which chooses the controller to use at any given time - swing up, swing down, or stabilize up.

Here's a video showing the different control states: