Zigroller project, software

As I promised the hardware section, I've now written up the software side of the Zigroller. This is mostly going to be a big block of text with some code snippets thrown in for flavor. I'm going to avoid going too deeply into the math, but I can't completely avoid it. The code is a bit of a mess right now, and there are a number of things that are in there because of the somewhat meandering route I took to this point and because it remains a work in progress. 

Our Tale So Far...

Since I originally conceived this project for a class, I made a point of not looking at Shaun's code ahead of time. I also decided that I'd like to try applying all these lovely techniques I learned for doing sensor and controls synthesis with a Kalman filter and state space control that I was learning in class. It is worth noting here that the Zigduino, since it has an Atmega core, is relatively slow and has no hardware floating point. This makes implementing more sophisticated control algorithms tricky or impossible. Specifically, a fast adaptive control system is out of the question.

To cut a very long story short (and avoid writing a mathematical treatise), this was pretty much an unmitigated disaster. It did not work at all, and when I dug back into my work with Matlab, it became clear that I had some a number of deficiencies, both hardware and software. The most glaring hardware deficiency I found was that the wheels were too small -- increasing their size increased stability. So I ordered some bigger wheels. They didn't really fix things because they did not correct the larger software issue, which was that the controller I had fashioned was only marginally stable... so marginal in fact that small modeling errors threw it into instability. I spent some time in denial about this, and tinkered with various sorts of noise suppression and the like. The only part of that that survived all the way through was the 1 uF caps on the motors. 

My professor suggested I add an inner loop with some other sort of compensator to help the stability along. I looked around for something useful, and I found an excellent presentation created by Shane Colton from MIT. He suggests using a complementary filter to synthesize the inputs from the accelerometer and the gyro. I was pretty desperate by this point, so I just copied his example code directly, constants and all:

angle = (0.98)*(angle + gyro*dt) + (0.02)*(x_acc);

Combined with straight proportional feedback, this did what my far more complicated attempts could not do: it balanced the bot for a little while. With a suprisingly small amount of tuning and the smallest touch of integral control, it balanced well, and reached the point where it is now. I was also able to add remote control, although that needs more tuning.

Since the overall system is more or less working, this is a good point at which to revisit the more complex control algorithm(s) to correct its tendency to wander. But that's for future work. Right now, let's dive a little bit into the structure of each of the major components of the software, starting with timing.

Timing

Timing is a key issue in any digital control system. The typical way to do this is with a timer interrupt. When I originally wrote the program, I just did everything in the timer interrupt. This is generally a bad idea, because it can cause extremely strange behavior if some other interrupt fires while the program is in interrupt context because the Atmega core lacks any memory protection.

Therefore, I just set a flag and have the loop() function keep checking for it. The interrupt handler just looks like this:

// ISR functions to do the steps
volatile byte stepFlag;
ISR (TIMER4_OVF_vect) {
  TCNT4 = timerVal; // reset the timer
  stepFlag++; // increment the step flag, to trigger the next step
}

The stepFlag variable must be declared volatile, because it's shared between the interrupt handler and the main body of the program. The loop() function to pick this up is then really simple, and we get to check if we overrun our alotted frame time:

void loop(void) {
  if (stepFlag) {
    stepFlag = 0;
    PORTE |= _BV(6); // flip the index pin up
    doStep(); // do the things required for this step
    getRadio(); // grab a command from the radio
    PORTE &= ~_BV(6); // flip the index pin down
    if (stepFlag) {
      Serial.print("Frame overflow!! ");
      Serial.println(stepFlag);
      stepFlag = 0;
    }
  }
}

The index pin that it refers to a pin that I flip on and off to keep track of timing. It's very convenient to be able to just hook up an oscilloscope and measure the timing of the different sections of code directly.

Timer selection is a matter of which processor you are using, and the setup is as follows:

  TIMSK4 = 0;                 // disable the overflow interrupt while we set things up
  TCCR4A = 0;                 // set counter mode to normal, counting up
  TCCR4B = 0x02;              // set counter mode to normal, counting up, prescaler 8
  TCNT4 = timerVal;           // set the counter to fire at the appropriate interval
  TIMSK4 = 1;                 // turn on the overflow interrupt

Sensor Acquisition & Synthesis

 There are two sensors in the Zigroller -- a two-axis accelerometer and a rate gyroscope. Both are mounted on the axis of rotation. This makes the math a bit easier because the accelerometer is not affected by the rotation of the bot around its axis. 

Angular velocity and the accelerations are clearly both floating point quantities that can be positive or negative. However, the output of the analog to digital converter is a 10-bit positive integer -- a number from 0 to 1023. The calibration constants (zero point and scaling factor) must be determined experimentally -- guessing what they should be will end in tears. 

The gyroscope is self-explanatory -- take the input signal, apply the offset and gain, and there's your angular velocity.

Deriving the angle at which the bot is standing from the two accelerometer axes is a little more tricky. The accelerometer is arranged with the Y axis pointing vertically downward. Therefore, Xacc = g * sin(theta) and Yacc = g * cos(theta), where g is the acceleration of gravity and theta is the angle from the vertical. Since tan t = (sin t)/(cos t), tan(theta) = Xacc/Yacc. It turns out that as long as theta is small, tan(theta) ~ theta. Since the bot won't stand up unless theta is small, we can just use the approximation theta = Xacc/Yacc. 

Complementary Filter

 So now we know the angular velocity and the angle, and somehow we have to synthesize some sort of estimate of the angular position of the bot out of this. Shane Colton provided an excellent description of the theoretical basis of the complementary filter. To make a somewhat long story short, the complementary filter is the sum of a high pass filter and a low pass filter on two different measurements of the same value, in this case the angle of the bot. The cutoff frequencies of both are selected such that they form an all-pass filter with unity gain. The effect is to take two different measurements of the same quantity that have different time-domain behavior to develop a better estimate of the desired value than can be achieved from either alone. 

In our case, the gyroscope responds very quickly to movements off center. However, it is prone to drift -- the ADXRS613 has a drift rate of at least 40 degrees per hour. If it was our only sensor, the Zigroller would fall over inside of ten minutes. The accelerometers are relatively slow to respond to changes; however, they give an absolute angular reference. 

Therefore, we feed the integral of the gyro (so we get an angle rather than a rate) into the high-pass side of the complementary filter and the accelerometer derived angle measurement into the low-pass side. That sounds complex, but it's dead simple to implement:

angle = (COMP)*(angle + thetaDot*0.0262) + (1-COMP)*(theta);

COMP is the complementary constant and it needs to be empirically tuned for good performance. The integration constant that thetaDot is multiplied by is likewise determined by trial and error.

Feedback Control

The question then becomes how do we turn this angle estimate into motor commands. I chose a simple proportional-integral (PI) feedback scheme, as shown below:

angleIntegral += angle * IGAIN;
angleIntegral *= ILEAK;
// feedback plus motor control
motor1DIR = getDIR(GAIN*angle + angleIntegral + (forward*FGAIN));
motor2DIR = motor1DIR;
motor1PWM = getPWM(GAIN*angle + angleIntegral + (forward*FGAIN));
motor2PWM = motor1PWM + (steer);
motor1PWM += -(steer);

In addition to the integrator constant, I also have a deliberate leak in the integrator, represented by ILEAK. ILEAK is a number that is very slightly less than unity, and it is intended to prevent the integrator from saturating, i.e. becoming so large in relation to any possible control inputs that it swamps the control system and prevents it from stabilizing the bot. Like all of the other constants in this code, it was determined experimentally.

You can also see where the steering and control inputs are mixed in. The FGAIN value is empirically chosen so as to produce a usable control response from what's coming across the air. The steering value is added to one motor command and subtracted from the other to produce a steering response. Since the motor control is a direction and magnitude, we need a little more code to correct the motor direction if the steering command calls for it to reverse. 

The getDIR() and getPWM() functions are just scaling functions that turn commands into direction and speed commands for the motors. These commands are sent to the direction control pins for the motors and the PWM inputs to the motor shield, respectively.

Radio Control

The bot receives commands transmitted by the controller using Frank Zhao's ZigduinoRadio library. It's dead simple to use. In order to make life easy, the controller just transmits the raw ADC output from the two axes of the joystick. Since this is a 10-bit value, it is transmitted as four bytes and assembled into the throttle and steering values and has the pre-calibrated offset applied.

void getRadio(void) {
  uint8_t lowSteer, highSteer, lowFore, highFore;
    if (ZigduinoRadio.available()) {
    lowSteer = ZigduinoRadio.read();
    highSteer = ZigduinoRadio.read(); 
    lowFore = ZigduinoRadio.read(); 
    highFore = ZigduinoRadio.read();
    steer = (((highSteer << 8) + lowSteer) >> 3) - STEERZERO;
    forward = (((highFore << 8) + lowFore) >> 3) - FORWARDZERO;
 
    Serial.print("Steer: ");
    Serial.print(steer);
    Serial.print("\tForward: ");
    Serial.println(forward);
  }
}

The transmission code in the controller is therefore very simple:

steer = analogRead(0);
forward = analogRead(1);
ZigduinoRadio.beginTransmission();
ZigduinoRadio.write(lowByte(steer));
ZigduinoRadio.write(highByte(steer));
ZigduinoRadio.write(lowByte(forward));
ZigduinoRadio.write(highByte(forward));
ZigduinoRadio.endTransmission();

Conclusion

Once I was on the right track with the control algorithm, the rest of the code went together in a very straightforward fashion. The code has a fair amount of tidbits from the development process, but should be self-explanatory.

 

 

The Zigroller control sketch is called JoustSketch.pde, because I am working on putting together a set of rules for balance bot jousting. But that is a different blog post.