Example Source Code:
Atmel AVR Processor using WinAVR / GCC
Advanced Velocity and Position Control


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

ww01_avrgcc_square.c - C source
Makefile - build file
ww01_avrgcc_square.hex - Intel Hex file

 

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*/
/* ---------------------------------------                                    */
/******************************************************************************/

#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>

#define DEBUG_POS 1
#define DEBUG_SNS 1

// timer variables
volatile uint16_t timer_ticks;                             // in multiples of 128us; rolls over every 10 seconds
volatile uint32_t seconds;
volatile int16_t run_counter;
volatile int8_t seconds_fraction;

// 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;

// position variables
volatile uint32_t enc_pos_R;
volatile uint32_t enc_pos_L;

// velocity measurement variables
volatile int16_t enc_period_L_prev;
volatile int16_t enc_period_R_prev;
volatile int16_t enc_period_L;
volatile int16_t enc_period_R;
volatile int16_t enc_speed_R;
volatile int16_t enc_speed_L;

// velocity control PI variables
int16_t err_R, err_L;                                      // total error
int16_t i_err_R, i_err_L;                                  // integral of error
int16_t out_R, out_L;                                      // value to output to the motor
uint8_t dir_R, dir_L;                                      // direction to drive the motor

// velocity control variables
int16_t req_speed_R;                                       // desired or commanded velocity, in tenths of an inch per second
int16_t req_speed_L;

int16_t req_err_R;                                         // velocity feedforward value
int16_t req_err_L;

// position control variables
int16_t req_pos_R;                                         // desired or commanded position, in encoder ticks
int16_t req_pos_L;
int16_t req_pos_err_R;                                     // current position error
int16_t req_pos_err_L;
int16_t req_pos_max_vel;                                   // desired maximum velocity while moving to the new position
int16_t req_pos_cur_max;                                   // current max value
int16_t req_pos_vel_incs;                                  // amount to add to velocity each increment (used for initial acceleration) 
int16_t req_pos_inc_down_count;                            // when we are done accelerating to the desired maximum speed
int16_t pos_i_err_R, pos_i_err_L;                          // integrator of position error (used to overcome frictional errors)

// 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;

// constants
#define TRUE 1
#define FALSE 0
#define MAX_SPEED 63                                       // maximum velocity in RPMs (depends on the servo brand and other factors)
#define INTEG_LIMIT (MAX_SPEED/3)                          // maximum error value that we allow the velocity integral term to grow to to prevent windup
#define POS_INTEG_LIMIT 45                                 // maximum velocity value that we allow the position integral term to grow to to prevent windup
#define ACCEL_INCREMENTS 1                                 // how fast to advance the maximum allowed velocity when doing position control
#define SYSCLK 16000000UL
#define MAX_SERVO 500
#define MIN_SERVO 250
#define MID_SERVO 375

// hardware constants
#define RSERVO PC5                                         // right servo control line on ARC 1.1 board
#define LSERVO PC2                                         // left servo control line on ARC 1.1 board
#define RDIR PC0                                           // right WW-01 DIR signal
#define LDIR PC1                                           // left WW-01 DIR signal
#define SIG_RCLK SIG_INTERRUPT0                            // right WW-01 CLK signal connects to INT0 / PD2
#define SIG_LCLK SIG_INTERRUPT1                            // left WW-01 CLK signal connects to INT1 / PD3

enum
{
   UP, DOWN, STEADY 
};

// constants used to calculate the speed conversion factors
#define Dwh 2.75                                           // wheel diameter
#define PI 3.14159
#define Cwh (Dwh * PI)                                     // wheel circumference
#define TSCALE 60                                          // seconds per minute
#define INVTICK 7812                                       // this is 1 / 128 us, to avoid using floating point math
#define NCLKS 128                                          // number of encoder clocks per wheel rotation
#define Kips ((Cwh * INVTICK) / NCLKS)                     // inches per second (IPS) = Kips / PER = 527 / PER
#define Krpm ((int16_t)(((int32_t)TSCALE * INVTICK) / NCLKS)) // revolutions per minute (RPM) = Krpm / PER = 3662 / PER

// function prototypes
extern uint16_t get_timer_ticks();
extern int uart_putchar(char c);


/******************************************************************************/
/*  Setup the Hardware                                                        */
/******************************************************************************/
void setup()
{
   uint16_t baud;

   /* as early as possible, grab the current reason for being reset and then clear it */
   reset_source = MCUCSR;
   MCUCSR = 0x1f;                                          // clear reset source

   /* tmr1 is PWM with TOP set by ICR1, OC1A and OC1B output waveforms in fast PWM mode */
   TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV (WGM11);

   /* tmr1 running on fosc/64 = 250,000 Hz clock */
   TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11) | _BV (CS10);

   /* set PWM frequency to 50Hz */
   ICR1 = 5000;                                            // 5000 * 4us = 20ms

   /* set duty cycle such that 1.5 ms pulses are generated; 1 ms = 250, 1.5ms = 375, 2ms = 500 */
   out_R = 375;
   out_L = 375;                                            // 375;
   OCR1A = out_R;
   OCR1B = out_L;

   /* enable OC1A and OC1B as output */
   DDRD = _BV (PD5) | _BV(PD4);
   DDRC = _BV (PC5) | _BV(PC2);                            // mirror on PC5 and PC2 for ARC board

   /* enable interrupts on timer 1 overflow and both output compares */
   timer_enable_int (_BV (TOIE1) | _BV(OCIE1A) | _BV(OCIE1B));

   /* enable external interrupts 0 and 1 for falling edge triggering */
   MCUCR = _BV(ISC11) | _BV(ISC01);
   GICR = _BV(INT1) | _BV(INT0);

   /* enable serial port */
   UCSRB = _BV(TXEN);                                      /* tx enable */
   baud = (SYSCLK / (16 * 38400UL)) - 1;                   /* 38400 Bd */
   UBRRH = (unsigned char)(baud >> 8);
   UBRRL = (unsigned char)baud;
   UCSRC = (1<<URSEL)|(3<<UCSZ0);

   /* enable interrupts */
   sei ();

   /* set up serial printing */
   fdevopen(uart_putchar, NULL, 0);                        // associate uart output function with stdio
}

/******************************************************************************/
/* RS232 Character Sender                                                     */
/*                                                                            */
/* Send character c down the UART Tx, wait until tx holding register          */
/* is empty.                                                                  */
/******************************************************************************/
int
uart_putchar(char c)
{
   if (c == '\n')
      uart_putchar('\r');
   loop_until_bit_is_set(UCSRA, UDRE);
   UDR = c;
   return 0;
}

/******************************************************************************/
/* Print Reset Source                                                         */
/*                                                                            */
/* This dumps strings over the serial port to indicate why we recently        */
/* reset.                                                                     */
/******************************************************************************/
void
dump_reset()
{
   if (reset_source & _BV(JTRF))
      printf("JTAG Reset\n");
   if (reset_source & _BV(WDRF))
      printf("Watchdog Reset\n");
   if (reset_source & _BV(BORF))
      printf("Brownout Reset\n");
   if (reset_source & _BV(EXTRF))
      printf("External Reset\n");
   if (reset_source & _BV(PORF))
      printf("Power-on Reset\n");
}



/******************************************************************************/
/* 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;

   PORTC |= _BV(RSERVO) | _BV(LSERVO);
   if (update_pwm)
   {
      update_pwm = 0;
      if (dir_R != 0)
         OCR1A = (375 - out_R);
      else
         OCR1A = (out_R + 375);
      if (dir_L != 0)
         OCR1B = (out_L + 375);
      else
         OCR1B = (375 - out_L);
   }
   update_servo = 1;                                       // 50 Hz control loop rate
   if (srv == 1)
   {
      srv = 0;
      run_counter++;
   }
   else
      srv = 1;

   if (seconds_fraction++ == 50)
   {
      seconds_fraction = 0;
      seconds++;
   }
   if (pcount++ == 5)
   {
      pcount = 0;
      do_print = 1;
   }
   timer_ticks += 156;                                     // this is the max count of 5000 / 32; unit of time is 128us
}


/******************************************************************************/
/* 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
}

/******************************************************************************/
/* Calc Encoder Speeds                                                        */
/*                                                                            */
/* speed in RPM = (1 / NCLKS) * TSCALE / (TICK * PER),  where NCLKS = 128     */
/* (32 stripe disk) per rotation, TSCALE = 60 seconds per minute, TICK =128us */
/* per timer tick, and PER is the measured period in timer ticks;             */
/* so RPMs = 3662 / PER.                                                      */
/******************************************************************************/
void calc_enc_speeds()
{
   if (received_R)
   {
      enc_speed_R = (int16_t)(Krpm / enc_period_R);
      if ((PINC & _BV(RDIR)) == 0)
         enc_speed_R = (int16_t)-enc_speed_R;
   }

   if (received_L)
   {
      enc_speed_L = (int16_t)(Krpm / enc_period_L);
      if ((PINC & _BV(LDIR)) != 0)
         enc_speed_L = (int16_t)-enc_speed_L;
   }
}


/******************************************************************************/
/* Print Encoder Speeds                                                       */
/******************************************************************************/
void print_enc_speeds()
{
#ifdef DEBUG_SNS
   int fwdv;
   fwdv = (int)enc_fwdir_R;
   printf("EncSpdR %d; RFwd %d; EncPerR %u; ", enc_speed_R, fwdv, enc_period_R);
   fwdv = (int)enc_fwdir_L;
   printf("EncSpdL %d; LFwd %d; EncPerL %u\r\n", enc_speed_L, fwdv, enc_period_L);
#endif
}

/******************************************************************************/
/* Sat16                                                                      */
/* Limit value to be between positive and negative limits.                    */
/******************************************************************************/
int16_t sat16(int16_t value, int16_t limit)
{
   if (value > limit)
      value = limit;
   else if (value < -limit)
      value = -limit;
   return value;
}

/******************************************************************************/
/* Sat32                                                                      */
/* Limit value to be between positive and negative limits.                    */
/******************************************************************************/
int32_t sat32(int32_t value, int32_t limit)
{
   if (value > limit)
      value = limit;
   else if (value < -limit)
      value = -limit;
   return value;
}


/******************************************************************************/
/* Set Velocity                                                               */
/*                                                                            */
/* Set velocity in terms of RPMs.                                             */
/******************************************************************************/
void set_velocity(int16_t speed_R, int16_t speed_L)
{
   speed_R = sat16(speed_R, MAX_SPEED);
   if (((req_speed_R >= 0) && (speed_R < 0)) || ((req_speed_R < 0) && (speed_R >= 0)))
      i_err_R = 0;                                         // reset integral on direction change
   req_speed_R = speed_R;

   speed_L = sat16(speed_L, MAX_SPEED);
   if (((req_speed_L >= 0) && (speed_L < 0)) || ((req_speed_L < 0) && (speed_L >= 0)))
      i_err_L = 0;                                         // reset integral on direction change
   req_speed_L = speed_L;
}


/******************************************************************************/
/* Print Control                                                              */
/*                                                                            */
/******************************************************************************/
void print_control()
{
#ifdef DEBUG_SNS
   printf("errR %d; ierrR %d; reqR %d; outR %d; dirR %d\r\n", err_R, i_err_R, req_err_R, out_R, dir_R);
   printf("errL %d; ierrL %d; reqL %d; outL %d; dirL %d\r\n", err_L, i_err_L, req_err_L, out_L, dir_L);
#endif
}

/******************************************************************************/
/* 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);
}

/******************************************************************************/
/* Calculate Motor Output                                                     */
/*                                                                            */
/* This converts the total control error to PWM or servo control value.       */
/*                                                                            */
/* Futaba S3003 servo rotates at 43RPM max; GWS S03NF rotates at 63 RPM;      */
/* a command of 127 + the midpoint value of 375 is equivalent to this;        */
/* so Kp = 127 / 63 = ~2; however, at 8.4v instead of 6, need to reduce       */
/******************************************************************************/
int16_t calc_vel_out(int16_t value)
{
   return (value * (int16_t)7)/8;
}

/******************************************************************************/
/* 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

   calc_enc_speeds();                                      // how fast are we going?

   if (received_R)                                         // skip calculations if we have not received any new information since the last update period
   {
      received_R = FALSE;
      err_R = (req_speed_R - enc_speed_R);                 // calculate proportional term
      i_err_R = calc_vel_integ(i_err_R, err_R);            // calculate integral term, with saturation to prevent windup
      req_err_R = (req_speed_R * 8) / 10;                  // calculate velocity feed forward term
      out_R = calc_vel_out(req_err_R + err_R + i_err_R);   // calcuate motor outputs based on total error term
      if (out_R >= 0)                                      // convert signed values to sign/magnitude for motor drivers
         dir_R = TRUE;
      else
      {
         dir_R = FALSE;
         out_R = -out_R;
      }
      if (out_R > 125)                                     // limit to maximum of 255
         out_R = 125;
   }

   if (received_L)                                         // skip calculations if we have not received any new information since the last update period
   {
      received_L = FALSE;
      err_L = (req_speed_L - enc_speed_L);
      i_err_L = calc_vel_integ(i_err_L, err_L);
      req_err_L = (req_speed_L * 8) / 10;
      out_L = calc_vel_out(req_err_L + err_L + i_err_L);
      if (out_L >= 0)
         dir_L = TRUE;
      else
      {
         dir_L = FALSE;
         out_L = -out_L;
      }
      if (out_L > 125)
         out_L = 125;
   }

   // print_control();
   update_pwm = 1;                                         // tell interrupt handler to update servo; this ensures it happens at a fixed rate
}



/******************************************************************************/
/* Set Position                                                               */
/*                                                                            */
/* This sets the left and right wheel positions.  If delta is 1, then the     */
/* position values are assumed to be relative to the current position, other- */
/* wise they are assumed to be absolute.  max_velocity is the upper limit     */
/* at which the caller wishes the robot to go while reaching that position.   */
/*                                                                            */
/* If the wheels are 8.64" in circumference, this is 0.0675" per              */
/* position increment, or 14.8 ticks per inch.                                */
/* Position is controlled in terms of encoder ticks.                          */
/******************************************************************************/
void set_position(int16_t posR, int16_t posL, int16_t max_velocity, uint8_t delta)
{
   pos_reached = FALSE;
   if (delta)
   {
      req_pos_R += posR;
      req_pos_L += posL;
   }
   else
   {
      req_pos_R = posR;
      req_pos_L = posL;
   }
   set_velocity(max_velocity, max_velocity);               // do this to calculate goal max vel
   received_R = received_L = TRUE;                         // force initial velocity update
   req_pos_max_vel = req_speed_R;

   req_pos_inc_down_count = req_speed_R / ACCEL_INCREMENTS; // calculate the number of steps to accelerate in
   if (req_pos_inc_down_count < 0)                         // bit_test(req_pos_inc_down_count, 15))
      req_pos_inc_down_count = -req_pos_inc_down_count;    // make sure it is a positive value

   if (!req_pos_inc_down_count)                            // not enough speed, so just go in one jump
   {
      req_pos_inc_down_count = 1;
      req_pos_vel_incs = req_speed_R;                      // otherwise, go in one step
   }
   else
   {
      if (req_speed_R < 0)                                 // bit_test(req_speed_R, 15)) // go in multiple steps, of the correct sign
         req_pos_vel_incs = -ACCEL_INCREMENTS;
      else
         req_pos_vel_incs = ACCEL_INCREMENTS;
   }
   req_pos_cur_max = req_pos_vel_incs;
   pos_i_err_R = pos_i_err_L = 0;
}


/******************************************************************************/
/* Print Position Control Values                                              */
/*                                                                            */
/******************************************************************************/
void print_pos_control()
{
#ifdef DEBUG_POS
   uint8_t tmp;
   printf("  req enc edir err ier spd pwm dir\r\n");
   tmp = enc_fwdir_R;
   printf("R %d, %ld, %d, %d, ",
      req_pos_R, enc_pos_R, tmp, req_pos_err_R);
   tmp = dir_R;
   printf("%d, %d, %d, %d\r\n",
      pos_i_err_R, req_speed_R, out_R, tmp);

   tmp = enc_fwdir_L;
   printf("L %d, %ld, %d, %d, ",
      req_pos_L, enc_pos_L, tmp, req_pos_err_L);
   tmp = dir_L;
   printf("%d, %d, %d, %d\r\n",
      pos_i_err_L, req_speed_L, out_L, tmp);
#endif
}


/******************************************************************************/
/* 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

   req_pos_err_L = req_pos_L - enc_pos_L;
   pos_i_err_L = calc_pos_integ(pos_i_err_L, req_pos_err_L); // integrate the error to ensure we reach the destination despite various frictional loads
   spdL = calc_pos_speed(req_pos_err_L + pos_i_err_L);
   if ((spdL != req_speed_L) || req_pos_inc_down_count)
      received_L = TRUE;                                   // force velocity loop update, since the command to it has now changed

   if (!pos_reached)                                       // check to see if we are close enough
   {
      if ((abs(req_pos_err_R) < 5) && (abs(req_pos_err_L) < 5)) // allow up to 1 stripe in error (too small, and the system is unstable because we can't stop that accurately)
      {
         if ((abs(spdR) < 20) && (abs(spdL) < 20))         // also make sure we've slowed down enough
            pos_reached = TRUE;
      }
      if (req_pos_inc_down_count)                          // have we finished accelerating?
      {
         req_pos_inc_down_count--;
         req_pos_cur_max += req_pos_vel_incs;              // not yet -- bump up the speed
      }
      else
         req_pos_cur_max = req_pos_max_vel;                // we've reached the speed limit
   }

   set_velocity(spdR, spdL);                               // output our calculated speeds
   control_velocity();                                     // then use them to drive the motors.
}



/******************************************************************************/
/* Speed Control Test                                                         */
/*                                                                            */
/* This ramps the speed of each wheel down to zero from MAX_SPEED forward, to */
/* MAX_SPEED in reverse.  The robot should go in a straight line.             */
/******************************************************************************/
void speed_control_test()
{
   int16_t spd_R, spd_L;
   int i;

#ifdef DEBUG_SNS
   printf("Speed ramp test\r\n");
#endif
   spd_L = spd_R = MAX_SPEED;
   set_velocity(spd_R, spd_L);
   run_counter = 0;
   for (i = 0; i < ((MAX_SPEED * 2) / 5); )
   {
      if (update_servo)
         control_velocity();
      if (!(run_counter & 0x1f))
      {
#ifdef DEBUG_SNS
         calc_enc_speeds();
         print_enc_speeds();
         print_control();
#endif
      }
      if (run_counter > 60)
      {
         calc_enc_speeds();
         print_enc_speeds();
         print_control();
         run_counter = 0;
         spd_R -= 5;
         spd_L -= 5;
         if (spd_R < -MAX_SPEED)
         {
            spd_R = spd_L = MAX_SPEED;
         }
         set_velocity(spd_R, spd_L);
#ifdef DEBUG_SNS
         printf("--> Requested SpdR = %d; SpdL = %d\r\n", spd_R, spd_L);
#endif
         i++;
      }
   }
   set_velocity(0, 0);
   out_L = out_R = 0;
   update_pwm = 1;
}

/******************************************************************************/
/* Run to Position                                                            */
/*                                                                            */
/* Keep executing the control loops until the desired position is reached.    */
/******************************************************************************/
void run_to_position()
{
   run_counter = 0;
   while (!pos_reached)
   {
      if (update_servo)
         control_position();
      if (run_counter == 4) 
         print_enc_speeds();
      if (run_counter == 8)
         print_control();
      if (run_counter >= 12)
      {
         print_pos_control();
         run_counter = 0;
      }
   }
   set_velocity(0, 0);
   out_L = out_R = 0;
   update_pwm = 1;
}


/******************************************************************************/
/* Wait Seconds                                                               */
/*                                                                            */
/* This uses timer2's interrupt handler to delay for a while.                 */
/******************************************************************************/
void wait_seconds(long int seconds_to_wait)
{
   seconds_to_wait += seconds;
   while (seconds_to_wait > seconds)
   {
   }
}

/******************************************************************************/
/* Position Control Test                                                      */
/*                                                                            */
/* This drives the robot 12" forward, then turns 90 degrees counter clock     */
/* wise, then repeats.                                                        */
/* There are 14.8 ticks per inch, so 12" = 12 * 14.8 = 178 ticks.             */
/* To turn 90 degrees, we need to turn 1/4 of the wheel base forward on       */
/* one wheel and backward on the other.  The wheelbase is 3.5", which means   */
/* the circumference of wheel base is 11". 11/4 * 14.8 = 41 ticks.            */
/******************************************************************************/
void position_control_test()
{
   int i;

#ifdef DEBUG_POS
   printf("Position test\r\n");
#endif

   enc_pos_R = 0;
   enc_pos_L = 0;
   for (;;)
   {
      for (i = 0; i < 16; i++)
      {
#ifdef DEBUG_POS
         printf("Fwd 1 foot\r\n");
#endif
//       output_bit(RED_LED, 1);
         set_position(297, 297, MAX_SPEED, TRUE);          // forward 20"
         run_to_position();
//       output_bit(RED_LED, 0);
         wait_seconds(2);
#ifdef DEBUG_POS
         printf("Turning 90CC\r\n");
#endif
//       output_bit(RED_LED, 1);
         set_position(41, -41, 50, TRUE);                  // rotate counter clockwise 90 degrees
         run_to_position();
//       output_bit(RED_LED, 0);
         wait_seconds(2);
      }
   }
#ifdef DEBUG_POS
   printf("Done.\r\n");
#endif
   set_velocity(0, 0);
   out_R = out_L = 0;
   update_pwm = 1;
}


/******************************************************************************/
/* Main Program Entry Point                                                   */
/*                                                                            */
/******************************************************************************/
int
main (void)
{
   setup();
   printf("\nAtmel ATMEGA16 / ARC 1.1 WW-01 Odometry Example\n");
   dump_reset();
   //printf("speed test\r\n");
   //speed_control_test();
   printf("position_test\r\n");
   position_control_test();

   // stop
   for (;;)
   {
   }
   return (0);
}


Makefile


PRG            = ww01_avrgcc_square
OBJ            = ww01_avrgcc_square.o
MCU_TARGET     = atmega16
OPTIMIZE       = -O2
AVRDUDE        = avrdude
# Output format. (can be srec, ihex, binary)
FORMAT         = ihex
DEFS           =
LIBS           =

# You should not have to change anything below here.

CC             = avr-gcc

# Override is only needed by avr-lib build system.

override CFLAGS        = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) $(DEFS)
override LDFLAGS       = -Wl,-Map,$(PRG).map

OBJCOPY        = avr-objcopy
OBJDUMP        = avr-objdump


# Programming hardware: alf avr910 avrisp bascom bsd 
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
#
# Type: avrdude -c ?
# to get a full listing.
#
#AVRDUDE_PROGRAMMER = stk500
AVRDUDE_PROGRAMMER = bascom
# com1 = serial port. Use lpt1 to connect to parallel port.
#AVRDUDE_PORT = com1
# programmer connected to serial device
AVRDUDE_PORT = lpt1

AVRDUDE_WRITE_FLASH = -U flash:w:$(PRG).hex
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(PRG).eep


# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
# see avrdude manual.
#AVRDUDE_ERASE_COUNTER = -y

# Uncomment the following if you do /not/ wish a verification to be
# performed after programming the device.
#AVRDUDE_NO_VERIFY = -V

# Increase verbosity level.  Please use this when submitting bug
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> 
# to submit bug reports.
#AVRDUDE_VERBOSE = -v -v

AVRDUDE_FLAGS = -p $(MCU_TARGET) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)
AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)
AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)





all: $(PRG).elf lst text eeprom

$(PRG).elf: $(OBJ)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS)

clean:
rm -rf *.o $(PRG).elf *.eps *.png *.pdf *.bak *.hex *.eep
rm -rf *.lst *.map $(EXTRA_CLEAN_FILES)

lst:  $(PRG).lst

%.lst: %.elf
$(OBJDUMP) -h -S $< > $@

# Rules for building the .text rom images

text: hex bin srec

hex:  $(PRG).hex
bin:  $(PRG).bin
srec: $(PRG).srec

%.hex: %.elf
$(OBJCOPY) -j .text -j .data -O $(FORMAT) -R .eeprom $< $@

%.srec: %.elf
$(OBJCOPY) -j .text -j .data -O srec $< $@

%.bin: %.elf
$(OBJCOPY) -j .text -j .data -O binary $< $@

# Rules for building the .eeprom rom images

eeprom: $(PRG).hex $(PRG).eep

%.eep: %.elf
@echo
@echo $(MSG_EEPROM) $@
-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
--change-section-lma .eeprom=0 -O $(FORMAT) $< $@


# Program the device.  
program: $(PRG).hex $(PRG).eep
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)


# Every thing below here is used by avr-libc's build system and can be ignored
# by the casual user.

FIG2DEV                 = fig2dev
EXTRA_CLEAN_FILES       = *.hex *.bin *.srec


dox: eps png pdf

eps: $(PRG).eps
png: $(PRG).png
pdf: $(PRG).pdf

%.eps: %.fig
$(FIG2DEV) -L eps $< $@

%.pdf: %.fig
$(FIG2DEV) -L pdf $< $@

%.png: %.fig
$(FIG2DEV) -L png $< $@



 

 © 2004-2013 Noetic Design, Inc. All Rights Reserved. Nubotics, Unicoder, WheelCommander, and WheelWatcher are trademarks of Noetic Design, Inc.