Aktuelle Version vom 29. Juni 2015, 12:20 Uhr

This page shows example code, common code techniques etc. that can be useful for your own programming experiments with the Arduino.

Integration

To get the new position at each tick, you take the old position and add the change in position. The change in position is just the speed of the robot multiplied by the time since the last tick, which you can get from the timers on the microcontroller or some other known timer.

position += speed * dt;  
angle += gyro * dt;

Low pass filter

• Problem: you have multiple ADC measurements (battery, ultrasonic etc.) that can quickly change. What value is the 'smooth value' (low-pass filter)? The goal of the low-pass filter is to only let through long-term changes, filtering out short-term fluctuations.
• Solution: This code will apply a 'low-pass' filter to your values. A 'weight' constant specifies the cut-off frequency, so how much your values are 'smoothed': if the weight is lower, the cutt-off frequency is lower (so more is filtered out), however the more slowly the system adapts to the last measurements.

float value = 0.0; 
float weight = 0.1; // how much your values are 'smoothed'

void loop(){   
  value = (1.0-weight) * value   +   weight * analogRead(A0);   // low-pass values
  Serial.println(value); // print value
}

The time it takes to reach the full value depends on both the filter constants (0.9 and 0.1 in the example) and the sample rate of the loop (dt).

High pass filter

It allows short-duration signals to pass through while filtering out signals that are steady over time. This can be used to cancel out drift.

Time constant

The time constant of a filter is the relative duration of signal it will act on. For a low-pass filter, signals much longer than the time constant pass through unaltered while signals shorter than the time constant are filtered out. The opposite is true for a high-pass filter.

The time constant 'tau' of a digital low-pass filter,

y = (a)*(y) + (1-a)*(x);

running in a loop with sample period 'dt', can be found like this:

tau = a * dt / (1-a)    <=>     a = tau / (tau+dt)

So if you know the desired time constant and the sample rate, you can pick the filter coefficient 'a'.

Complementary filter

This just means the two parts of the filter always add to one, so that the output is an accurate, linear estimate in units that make sense. The filter presented here is not exactly complementary, but is a very good approximation when the time constant is much longer than the sample rate (a necessary condition of digital control anyway).

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

If this filter were running in a loop that executes 100 times per second, the time constant for both the low-pass and the high-pass filter would be:

tau = a*dt/(1-a) = 0.98*0.01sec/0.02 = 0.49sec

This defines where the boundary between trusting the gyroscope and trusting the accelerometer is. For time periods shorter than half a second, the gyroscope integration takes precedence and the noisy horizontal accelerations are filtered out. For time periods longer than half a second, the accelerometer average is given more weighting than the gyroscope, which may have drifted by this point.

Median filter

• Problem: you have multiple ADC measurements (battery, ultrasonic etc.) that can contain outliers. What value is the 'most seen value'?
• Solution: This code will sort and choose the center value.
• Example measurements: 32,7,7,1,6,8,7,9,23,7,8,9,7
• Sorted: 1,6,7,7,7,7,7,8,8,9,9,23,32
• Returned: 7 (center value)

unsigned int ADCmedian(int pin, int count) {
  int values[count], last;
  uint8_t j, i = 0;
  values[0] = 0;
  while (i < count) {
    last = analogRead(pin);
    if (i > 0) {       
       for (j = i; j > 0 && values[j - 1] < last; j--) // Insertion sort loop.
   values[j] = values[j - 1]; // Shift ping array to correct position for sort insertion.
    } else j = 0;              // First ping is starting point for sort.
    values[j] = last;              // Add last ping to array in sorted position.
    i++;                       // Move to next ping.    
    if (i < count) delay(1); 
   }
   return (values[count/2]); // Return the median.
}

• 

  Filter example

Further links

http://robottini.altervista.org/tag/complementary-filter