[LieAfterLie]

It's not extremely hard, it's called operator (or field) oriented control, as opposed to robot oriented control. You need a gyro (comes in the KOP) and simple trig.

Operator oriented means that controller directions are defined (with the help of the gyro) so that 'up' is always the operator's 'up', not the robot's 'up'. Field oriented is a little more specific and means it's zeroed relative to the field's directions, so that the robot's 'up' is the same as what you might call the field's 'up'.

You can't just have it do what you say, at least not in a practical way that I can think of, without being operator oriented. When the robot is spinning and going in a straight line at the same time, 'forward' is defined relative to a reference heading kept track of with the gyro. So 'forward' can't also be whichever direction the robot is facing, not without having a switch to turn the operator oriented control on and off.