Description

This example is quite complex,
as it demonstrates many advanced control loop techniques. It has two control
loops: an inner velocity loop and an outer position loop. It includes code
to drive RC servos in the background using the two TIMER1 Output Compare hardware
units.
It measures the actual velocity of each wheel by measuring the time between
pulses on the CLK line on the WheelWatchers. It also calculates a running
4 sample average of these clock pulses in order to filter out alignment errors.
This example was developed on a Barello.net
ARC 1.1 board for the MarkIII chassis.

Download

Source Code

ww01_avrgcc_square.c

/******************************************************************************/
/* WW-01 GNU C for Atmel ATMEGA16 */
/* Simple Odometry Example Program, v1.0 8/3/2004 */
/* */
/* Copyright 2004, Noetic Design, Inc. */
/* */
/* This demonstation program shows off the features */
/* of the WheelWatcher. */
/******************************************************************************/
/******************************************************************************/
/* TARGET NOTE: */
/* */
/* This example is for use with an ARC 1.1 board from barello.net. This */
/* uses the ATMEGA16 processor with a 16MHz resonator. The following */
/* fuses are required; use AVR Studio or GNU avrdude to reprogram: */
/* CKOPT = 1, CKSEL3 = CKSEL2 = CKSEL1 = 1, CKSEL0 = 1, SUT1 = 0, SUT0 = 0 */
/******************************************************************************/
/******************************************************************************/
/* The following note is just about all that remains of the original demo.c */
/* that came with WinAVR. We are retaining it as required, though little */
/* remains of the original code. */
/* --------------------------------------- */
/* "THE BEER-WARE LICENSE" (Revision 42): */
/* <joerg@FreeBSD.ORG> wrote this file. As long as you retain this notice you*/
/* can do whatever you want with this stuff. If we meet some day, and you */
/* think this stuff is worth it, you can buy me a beer in return. Joerg Wunsch*/
/* --------------------------------------- */
/******************************************************************************/

// encoder variables
uint8_t enc_fwdir_R; // most recent value read from the right WW-01's DIR line
uint8_t enc_fwdir_L;
uint8_t received_R; // this is set each time we get a new encoder clock (and thus an updated period)
uint8_t received_L;

// program state flags
volatile uint8_t do_print;
uint8_t reset_source;
uint8_t update_servo; // it is time to calculate the PI control loop
uint8_t update_pwm; // it is time to update the motors
uint8_t pos_reached;

/******************************************************************************/
/* RCLK / INTERRUPT0 ISR */
/* */
/* This interrupt service routine is called on each pulse of the */
/* right WW-01 CLK line. We grab the current time stamp, adjust */
/* the current position based on the RDIR pin, then calculate a */
/* new encoder clock period using the new time stamp. This is done */
/* by subtracting the previous time stamp from the current to get */
/* the current period, then using that and the last period value */
/* to calculate a new one using a running average algorithm. */
/******************************************************************************/
SIGNAL (SIG_RCLK)
{
uint16_t tmr = get_timer_ticks();
if (PINC & _BV(RDIR))
{
enc_fwdir_R = TRUE;
enc_pos_R++;
}
else
{
enc_fwdir_R = FALSE;
enc_pos_R--;
}
// this calculates a running average to filter alignment noise
enc_period_R = (enc_period_R * 3 + (tmr - enc_period_R_prev)) >> 2;
enc_period_R_prev = tmr;
received_R = TRUE;
}

/******************************************************************************/
/* LCLK / INTERRUPT1 ISR */
/* */
/* This interrupt service routine is called on each pulse of the */
/* left WW-01 CLK line. We grab the current time stamp, adjust */
/* the current position based on the LDIR pin, then calculate a */
/* new encoder clock period using the new time stamp. This is done */
/* by subtracting the previous time stamp from the current to get */
/* the current period, then using that and the last period value */
/* to calculate a new one using a running average algorithm. */
/******************************************************************************/
SIGNAL (SIG_LCLK)
{
uint16_t tmr = get_timer_ticks();
if (PINC & _BV(LDIR))
{
enc_fwdir_L = FALSE;
enc_pos_L--;
}
else
{
enc_fwdir_L = TRUE;
enc_pos_L++;
}
// this calculates a running average to filter alignment noise
enc_period_L = (enc_period_L * 3 + (tmr - enc_period_L_prev)) >> 2;
enc_period_L_prev = tmr;
received_L = TRUE;
}

/******************************************************************************/
/* OUTPUT COMPARE 1A ISR */
/* */
/* This interrupt occurs each time the TCNT1 value matches OCR1A. We use this*/
/* routine to lower the RSERVO pin on the ARC board. This is needed because */
/* the ARC board connects the right servo control signal to PC5 instead of the*/
/* OC1A / PD5 signal which is automatically output by the hardware. */
/******************************************************************************/
SIGNAL (SIG_OUTPUT_COMPARE1A)
{
PORTC &= ~_BV(RSERVO);
}

/******************************************************************************/
/* OUTPUT COMPARE 1B ISR */
/* */
/* This interrupt occurs each time the TCNT1 value matches OCR1B. We use this*/
/* routine to lower the LSERVO pin on the ARC board. This is needed because */
/* the ARC board connects the left servo control signal to PC2 instead of the*/
/* OC1B / PD4 signal which is automatically output by the hardware. */
/******************************************************************************/
SIGNAL (SIG_OUTPUT_COMPARE1B)
{
PORTC &= ~_BV(LSERVO);
}

/******************************************************************************/
/* TIMER 1 OVERFLOW ISR */
/* */
/* This interrupt routine is called at the end of every 20ms servo control */
/* pulse period, which is 5000 ticks of TCNT1 with a prescale value of 64. */
/* Here is where the RSERVO (PC5) and LSERVO (PC2) lines are manually dropped,*/
/* a new control pulse value is calculated and written to the corresponding */
/* OCR1 register, and we also do some timekeeping work for use in measuring */
/* the encoder clock period. */
/******************************************************************************/
SIGNAL (SIG_OVERFLOW1)
{
static int8_t pcount = 0;
static int8_t srv = 0;

/******************************************************************************/
/* Get Timer Ticks */
/* */
/* This routine uses the overflow value calculated during the */
/* overflow ISR as well as the current TCNT1 value to calculate */
/* a new time stamp for use in measuring an encoder period. */
/* One timer_tick unit is equivalent to 128us, and so it over- */
/* flows every 10 seconds. This allows us to measure the */
/* speed of a running servo very accurately, and measure it */
/* over the range of a maximum of 3600 RPM(!) and a minimum */
/* of 0.05 RPM. */
/******************************************************************************/
uint16_t
get_timer_ticks()
{
return timer_ticks + (TCNT1 >> 5); // add current count to overflow
}

/******************************************************************************/
/* Calculate Velocity Integral Term */
/* */
/* This adds 1/6th of the current error to the integral, but saturates */
/* it to a limit of INTEG_LIMIT. This prevents the integral term from growing*/
/* so large that it takes a long time to reduce it once the desired velocity */
/* is reached. */
/******************************************************************************/
int16_t calc_vel_integ(int16_t integ, int16_t err)
{
return sat16(integ + (err)/6, INTEG_LIMIT);
}

/******************************************************************************/
/* Control Velocity */
/* */
/* This performs the PI (Proportional / Integral) control loop calculations. */
/* Note that the calculations are not performed if we have not received any */
/* new information from a given servo's encoders since the last update. */
/* This helps with stability of the integral term. */
/******************************************************************************/
void control_velocity()
{
update_servo = 0; // wait until next interval to do this again

/******************************************************************************/
/* Calculate Velocity Based on Position Error */
/* */
/* The constant '125' is empirically found for a given system. In effect, */
/* it sets how quickly the robot decelerates as it gets close to the desired */
/* position. */
/* Since req_pos_cur_max (the current allowed maximum velocity) is in tenths */
/* per second, and value is the error term in units of position in encoder */
/* ticks, the constant 150 is in units of encoder ticks too. Thus, the */
/* value returned is velocity in tenths of an inch per second. */
/******************************************************************************/
int16_t calc_pos_speed(int16_t value)
{
return (int16_t)(((int32_t)req_pos_cur_max * value) / 125);
}

/******************************************************************************/
/* Calculate Position Integral Term */
/* */
/* This adds 3/2 of the current error to the integral, but saturates */
/* it to a limit of POS_INTEG_LIMIT. This prevents the integral term from */
/* growing so large that it takes a long time to reduce it once the desired */
/* velocity is reached. */
/******************************************************************************/
int16_t calc_pos_integ(int16_t integ, int16_t err)
{
return sat16(integ + (err * 3) / 2, POS_INTEG_LIMIT); // saturate the integrator to prevent windup / overshoot / instability
}

/******************************************************************************/
/* Control Position */
/* */
/* This routine calculates the position PI control loop. It then uses */
/* the resulting velocity values to set the inner velocity control loop. */
/******************************************************************************/
void control_position()
{
int16_t spdR;
int16_t spdL;

req_pos_err_R = req_pos_R - enc_pos_R; // calculate current error in position
pos_i_err_R = calc_pos_integ(pos_i_err_R, req_pos_err_R); // integrate the error to ensure we reach the destination despite various frictional loads
spdR = calc_pos_speed(req_pos_err_R + pos_i_err_R); // convert the total position error term to desired velocity
if ((spdR != req_speed_R) || req_pos_inc_down_count)
received_R = TRUE; // force velocity loop update, since the command to it has now changed