Example Source Code:
Atmel AVR Processor using BASCOM-AVR
Simple Position and Velocity Monitoring


Description

This example ramps the speeds of the two servos back and forth, and while doing so, displays the actual current position and speed as measured using the WW-01s.
Note that this example uses interrupts heavily, and due to the use of function calls from the interrupt handlers, the default 32 byte hardware stack that BASCOM-AVR generates is too small. You must change it via the Options -> Compiler -> Chip dialog to a value of at least 40.
 

Download

ww01_bascomavr_monitor.bas - BASIC source
ww01_bascomavr_monitor.cfg - BASCOM-AVR configuration file
ww01_bascomavr_monitor.hex - Intel Hex file

 

Source Code


ww01_bascomavr_monitor.bas


'******************************************************************************
'* WW-01 BASCOM AVR for Atmel ATMEGA16                                        *
'* Simple Odometry Example Program                                            *
'*                                                                            *
'* Copyright 2004, Noetic Design, Inc.                                        *
'* v 1.0, written by Pete Skeggs                                              *
'*                                                                            *
'* 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    *
'*                                                                            *
'* NOTE: you MUST set the hardware stack size in Bascom's Options -> Compiler *
'* -> Chip >= 40, or else the stack will overflow during execution of INT1 or *
'* INT0 ISRs, due to the call made to get_timer().  The default of 32 is too  *
'* small in this case.                                                        *
'*                                                                            *
'******************************************************************************
'* Wiring:                                                                    *
'*                                                                            *
'* Right WW-01:                                                               *
'* Red (+5V)   - JP10 pin 3 / +5V                                             *
'* Black (Gnd) - JP10 pin 16 / GND                                            *
'* Orange (DIR)- JP3 pin 18 / PC0                                             *
'* Purple (CLK)- RIGHT motor connector pin 6 / INT0 / PD2                     *
'*                                                                            *
'* Left WW-01:                                                                *
'* Red (+5V)   - JP3 pin 9 / +5V                                              *
'* Black (Gnd) - JP3 pin 10 / GND                                             *
'* Orange (DIR)- JP3 pin 17 / PC1                                             *
'* Purple (CLK)- LEFT motor connector pin 6 / INT1 / PD3                      *
'*                                                                            *
'******************************************************************************
'* 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*
'* ---------------------------------------                                    *
'******************************************************************************

$regfile = "M16def.dat"
$crystal = 16000000
$baud = 19200


' timer variables
Defword Timer_ticks                                         ' in multiples of 128us; rolls over every 10 seconds
Deflng Seconds
Seconds = 0
Defbyte Seconds_fraction
Seconds_fraction = 0
Defbyte Seconds_decimal
Seconds_decimal = 0
Defbyte Pcount
Pcount = 0
Defword Tmrr
Tmrr = 0
Defword Tmrl
Tmrl = 0

' encoder variables
Defbit Enc_fwdir_r                                          ' most recent value read from the right WW-01's DIR line
Defbit Enc_fwdir_l

' position variables
Deflng Enc_pos_r
Deflng Enc_pos_l

' velocity control variables
Defword Out_r
Out_r = 0
Defword Out_l
Out_l = 0
Defbyte Direction_r
Defbyte Direction_r

' velocity measurement variables
Defword Enc_period_l_prev
Defword Enc_period_r_prev
Defword Enc_period_l
Defword Enc_period_r
Defint Enc_speed_r
Defint Enc_speed_l

' program state flags
Defbit Do_print
Do_print = 0
Defbyte Reset_source
Defword Temp
Temp = 0

' constants
Const Max_servo = 500
Const Min_servo = 250
Const Mid_servo = 375
Const False = 0
Const True = 1

' hardware constants
Rservo Alias Portc.5                                        ' right servo control line on ARC 1.1 board
Lservo Alias Portc.2                                        ' left servo control line on ARC 1.1 board
'Rdir Alias Portc.0                                          ' right WW-01 DIR signal
'Ldir Alias Portc.1                                          ' left WW-01 DIR signal
Prog_led Alias Portb.4

Const Up = 0
Const Down = 1
Const Steady = 2

' calculate the speed conversion factors
Const Dwh = 2.75                                            ' wheel diameter
Const Pi = 3.14159
Const Cwh =(dwh * Pi)                                       ' wheel circumference
Const Tscale = 60                                           ' seconds per minute
Const Invtick = 7812                                        ' this is 1 / 128 us, to avoid using floating point math
Const Nclks = 128                                           ' number of encoder clocks per wheel rotation
Const Kips =((cwh * Invtick) / Nclks)                       ' inches per second (IPS) = Kips / PER = 527 / PER
Const Krpm =((tscale * Invtick) / Nclks)                    ' revolutions per minute (RPM) = Krpm / PER = 3662 / PER

' function prototypes
Declare Function Get_timer_ticks() As Word
Declare Sub Setup()
Declare Sub Dump_reset()
Declare Sub Calc_enc_speeds()
Declare Sub Print_enc_speeds()

'******************************************************************************
'* Main Program Entry Point                                                   *
'*                                                                            *
'******************************************************************************
Main:
   Print "Atmel ATMEGA16 / ARC 1.1 WW-01 Monitor Example"
   Setup
   Dump_reset

   '* loop forever, the interrupts are doing the rest *

   Do
      If Do_print = 1 Then                                  ' only print this 10 times a second
         Do_print = 0
         Reset Prog_led
         Calc_enc_speeds
         Seconds_decimal = Seconds_fraction * 2
         Print Seconds ; "." ; Seconds_decimal;             ' display current run time
         Print " ldist " ; Enc_pos_l ; ", rdist " ; Enc_pos_r;       ' display current position
         Print ", lspd " ; Enc_speed_l ; ", rspd " ; Enc_speed_r;       ' display current speed
         Print ", out_L " ; Out_l ; ", out_R " ; Out_r;     ' display current servo control pulse values
         Print ", lper " ; Enc_period_l ; ", rper " ; Enc_period_r;       ' display the measured periods
         Print ", Ldir " ; Enc_fwdir_l ; ", Rdir " ; Enc_fwdir_r;       ' display direction lines from WW-01
         Temp = Get_timer_ticks()
         Print ", ticks " ; Temp                            ' display current timer tick (used to measure period of CLK lines)
         Set Prog_led
      End If
   Loop
End

'******************************************************************************
'*  Setup the Hardware                                                        *
'******************************************************************************
Sub Setup()

   '* as early as possible, grab the current reason for being reset and then clear it *
   Reset_source = Mcucsr                                    ' find out why we were reset
   Mcucsr = $1f                                             ' clear reset source

   '***********************
   ' set the direction
   ' control info for the
   ' I/O ports
   '***********************
   Config Porta = Input
   Config Portb = Input
   Config Pinb.4 = Output                                   ' Program LED
   Config Portc = Input
   Config Pinc.5 = Output                                   ' right servo
   Config Pinc.2 = Output                                   ' left servo
   Config Portd = Input
   Config Pind.5 = Output                                   ' OC1A and OC1B as output (you could hook the servos here instead)
   Config Pind.4 = Output


   '***********************
   ' set up WW-01 interface
   '***********************
   '* enable interrupts on timer 1 overflow and both output compares *
   Enable Ovf1
   On Ovf1 Overflow1_isr
   Enable Oc1a
   On Oc1a Oc1a_isr
   Enable Oc1b
   On Oc1b Oc1b_isr

   '* enable external interrupts 0 and 1 for falling edge triggering *
   Config Int0 = Falling
   Config Int1 = Falling
   Enable Int0
   On Int0 Rclk_isr
   Enable Int1
   On Int1 Lclk_isr

   '***********************
   ' set up hardware-based
   ' servo control pulse
   ' generation; replace
   ' this with code to
   ' drive the H bridges if
   ' you use DC motors
   ' instead of servos
   ' HOWEVER, you would still
   ' need to make timer_ticks
   ' work as a 16 bit 128 us
   ' resolution timer for
   ' measuring wheel speed.
   '***********************
   '* tmr1 is PWM with TOP set by ICR1, OC1A and OC1B output waveforms in fast PWM mode *
   Tccr1a = 0
   Tccr1a.com1a1 = 1
   Tccr1a.com1b1 = 1
   Tccr1a.wgm11 = 1

   '* tmr1 running on fosc/64 = 250,000 Hz clock *
   Tccr1b = 0
   Tccr1b.wgm13 = 1
   Tccr1b.wgm12 = 1
   Tccr1b.cs11 = 1
   Tccr1b.cs10 = 1

   '* set PWM frequency to 50Hz *
   Capture1 = 5000

   '* set duty cycle such that 1.5 ms pulses are generated; 1 ms = 250, 1.5ms = 375, 2ms = 500 *
   Out_r = 500
   Out_l = 250                                              ' 375;
   Pwm1a = Out_r
   Pwm1b = Out_l
   Direction_r = Down
   Direction_l = Up

  'finally we must turn on the global interrupt

   Enable Interrupts

End Sub


'******************************************************************************
'* 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.                  *
'******************************************************************************
Rclk_isr:
   Tmrr = Get_timer_ticks()
   If Pinc.0 = 1 Then
      Enc_fwdir_r = True
      Incr Enc_pos_r
   Else
      Enc_fwdir_r = False
      Decr Enc_pos_r
   End If
   ' this calculates a running average to filter alignment noise
   Enc_period_r = Enc_period_r * 3
   Enc_period_r = Enc_period_r + Tmrr
   Enc_period_r = Enc_period_r - Enc_period_r_prev
   Shift Enc_period_r , Right , 2
   Enc_period_r_prev = Tmrr
Return


'******************************************************************************
'* 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.                  *
'******************************************************************************
Lclk_isr:
   Tmrl = Get_timer_ticks()
   If Pinc.1 = 1 Then
      Enc_fwdir_l = False
      Decr Enc_pos_l
   Else
      Enc_fwdir_l = True
      Incr Enc_pos_l
   End If
   ' this calculates a running average to filter alignment noise
   Enc_period_l = Enc_period_l * 3
   Enc_period_l = Enc_period_l + Tmrl
   Enc_period_l = Enc_period_l - Enc_period_l_prev
   Shift Enc_period_l , Right , 2                           ' divide by 4
   Enc_period_l_prev = Tmrl
Return


'******************************************************************************
'******************************************************************************
'*                                                                            *
'* SERVO SUPPORT CODE                                                         *
'*                                                                            *
'******************************************************************************
'******************************************************************************

'******************************************************************************
'* 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.           *
'******************************************************************************
Oc1a_isr:
   Rservo = 0
Return

'******************************************************************************
'* 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.           *
'******************************************************************************
Oc1b_isr:
   Lservo = 0
Return

'******************************************************************************
'* 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.                                                  *
'******************************************************************************
Overflow1_isr:

   '******************************
   ' take the servo control pulses
   ' high at the start of the
   ' 20ms period
   '******************************
   Rservo = 1
   Lservo = 1

   '******************************
   ' generate a new value for the
   ' control pulse lengths for
   ' fun -- this is just a demo
   '******************************
   Select Case Direction_r
      Case Up:
         Incr Out_r : If Out_r >= Max_servo Then : Direction_r = Down : End If
      Case Down:
         Decr Out_r : If Out_r <= Min_servo Then : Direction_r = Up : End If
   End Select
   Select Case Direction_l
      Case Up:
         Incr Out_l : If Out_l >= Max_servo Then : Direction_l = Down : End If
      Case Down:
         Decr Out_l : If Out_l <= Min_servo Then : Direction_l = Up : End If
   End Select
   Pwm1a = Out_r
   Pwm1b = Out_l

   '******************************
   ' take care of timers needed
   ' to display WW-01 information
   ' periodically
   '******************************
   Incr Seconds_fraction
   If Seconds_fraction = 50 Then
      Seconds_fraction = 0
      Seconds = Seconds + 1
   End If
   Incr Pcount
   If Pcount = 10 Then
      Pcount = 0
      Do_print = 1
   End If

   '******************************
   ' CRITICAL: generate running
   ' 16 bit, 128us resolution
   ' timer value for use in
   ' measuring wheel speed with
   ' the WW-01s
   '******************************
   Timer_ticks = Timer_ticks + 156                          ' this is the max count of 5000 / 32; unit of time is 128us
Return

'******************************************************************************
'******************************************************************************
'*                                                                            *
'* SUPPORT ROUTINES FOR WW-01                                                 *
'*                                                                            *
'******************************************************************************
'******************************************************************************

'******************************************************************************
'* 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.                                                               *
'******************************************************************************
Function Get_timer_ticks() As Word
   Local Tmp As Word

   Tmp = Tcnt1
   Shift Tmp , Right , 5
   Get_timer_ticks = Timer_ticks + Tmp                      ' add current count to overflow
End Function

'******************************************************************************
'* 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.                                                      *
'******************************************************************************
Sub Calc_enc_speeds()
   Enc_speed_r = Krpm / Enc_period_r
   If Enc_fwdir_r = 0 Then
      Enc_speed_r = -enc_speed_r
   End If

   Enc_speed_l = Krpm / Enc_period_l
   If Enc_fwdir_l = 1 Then
      Enc_speed_l = -enc_speed_l
   End If
End Sub

'******************************************************************************
'* Print Encoder Speeds                                                       *
'******************************************************************************
Sub Print_enc_speeds()
   Print "EncSpdR " ; Enc_speed_r ; " RFwd " ; Enc_fwdir_r;
   Print "EncSpdL " ; Enc_speed_l ; " LFwd " ; Enc_fwdir_l
End Sub



'******************************************************************************
'* Print Reset Source                                                         *
'*                                                                            *
'* This Dumps Strings Over The Serial Port To Indicate Why We Recently        *
'* Reset.                                                                     *
'******************************************************************************
Sub Dump_reset()
   Print "Reset Source:"
   If Reset_source.4 = 1 Then
      Print "JTAG Reset"
   End If
   If Reset_source.wdrf = 1 Then
      Print "Watchdog Reset"
   End If
   If Reset_source.borf = 1 Then
      Print "Brownout Reset"
   End If
   If Reset_source.extrf = 1 Then
      Print "External Reset"
   End If
   If Reset_source.porf = 1 Then
      Print "Power-on Reset"
   End If
End Sub

 

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