Search This Blog

Sunday, February 24, 2013

Floating Point Arithmetic

If you want to multiply a number by a fixed floating point constant, here is a
pretty efficient method.

The example that brought this up was trying to measure the distance from the Ping ultrasonic sensor.  On trigger, the Ping sends out a pulse.  200 us later, you can start timing, waiting for the trigger pin to go low.  This yields a value between 0-18500 at 4MHz clock, and it corresponds to the number of microseconds ( 1 per clock pulse ) that the round trip of the ultrasonic burst took to hit and come back.

To get the distance, you first divide this by 2 to get the single trip value.

Sound travels about 343.2 meters/second.  That is 34320 cm/sec.  Invert this to 2.91375e-5 * 1e6 = 29.13 microseconds per cm.  That means we need to divide the time value by 29.13 microseconds to get the distance. Another way to divide by 29.1375 is multiply by 1/29.1375 = .03432.

Well, that doesn't help much does it?  Here's the trick.  You can create something called a "fixed point" to simplify the math.  To do this, we figure out how to represent .03432 in a 16 bit number and remember the offset.  So, multiply .03432 * 65536 = 2249.19552.  Round to 2249.  We've used a 16 bit offset, so we will have to shift things back 16 places at the end.

Now, in C, you can assign things like this for our Ping sensor:

unsigned int sensorRaw = 18500;  // the 2 way time measured.  This is the max value.
unsigned long cm = 2249; // this is our fixed point, calculated above.
unsigned long distanceCm; // this will hold the distance in cm.

// first, get the single trip time by dividing by 2 ( shift right one ).
sensorRaw = sensor >> 1;
// now, multiply by our fixed point
distanceCm = sensorRaw * cm;

Doing it this way takes about 128 us at 4MHz.  It seems consistent in time for all numbers I tried.  Doing a raw multiply of sensorRaw * .03432 took anywhere from 220us to 450us.  Pretty good improvement.  You could probably get this down to under 30 us
if you did it in assembly with the hardware multiplier.  The 16 bit shift just means you only use the two upper bytes of the 32 bit answer.  The algorithm for this is in the datasheet for the 18f2455 and takes 28us at 4MHz.  128us was good enough for me!

Here is the original quote that helped me from the PicList from Byron A Jeff:

You may want to think about fixed point. You can create a fixed point number
simply by taking the original number and multiplying it by the range that
you wish to represent it. For example if you wanted to represent .1 in 16
bits, take .1 and multiply it by 65536 giving 6553.6. Round to 6554. Store
this in 16 bits and use a 16 bit multiply followed by a 16 bit right shift
(which means simply leave off the lower 16 bits).

Here an example. Say we wanted to multiply 20 by our .1. So multiply 20 by
6554 giving 131080. Then shift 16 bits, which effectively divides by 65536.
The result of 2.0001220703 is close enough for govt. work

Tuesday, February 12, 2013

Pic18F4550 Interface to Wii Nunchuck Over I2C in C

    It was time to test out some of the C18 hardware libraries.  First up was the USART.  I wanted to be able to talk to the PC instead of using the rather large LCD routines I usually use.  I also wanted a way to do output at 3.3V without having to buy a 3.3V LCD.  Next up was the I2C libraries.  They greatly simplify a lot of the work of transmission/reception, checking bytes for done, overflow, collision, etc.  To pull these together, the Pic wants to initialize a Wii Nunchuck extension controller, enumerate the device type, and print that device type number over USART to the PC.  It wants to take a reading of the nunchuck's values every second and print those as well.  The final step required a program or script to receive the values and print them.  Along the research road, several next steps became evident.

    I have a TTL to USART shifter from Sparkfun.  It's essentially just a pair of transistors, some resistors and diodes.  It connects to a serial to USB cable.  Hooking it up was trivial.  The C18 library is very simple to use.  After configuring the USART, writing to it simply requires an fprintf command.  The file in this case is a constant for the USART: _H_USART.  Using stty on the command line followed by a cat </dev/ttyUSB proved that all was working.

   After that, the Pic needed to be able to send converted data values as either decimals or hexidecimals.  The built in conversion formats of fprintf made that a snap.  For decimal, the command was simply:
unsigned char value = 0xC8;
fprintf( _H_USART, "Value: %d\n", value );

The commands for hex were:
unsigned char value = 0xC8;
fprintf( _H_USART, "Value: %X\n", value );

PC Script
   The final program ended working as a simple bash script.  It sets up a handler for when the user presses Ctrl + C to end the program.  It then sets up a special file pointer to /dev/ttyUSB.  This is the device file the PC listens to for serial signals from the serial to USB device.  This is set up as a udev rule, granting full permissions to the regular user.  The script then loops forever, reading from the serial device.  If the device sends the magic word 'clear', it clears the screen.  Otherwise, it prints the line verbatim.

Here is the script:


 echo ' '
 echo 'The user has ended listening on ttyUSB.'
 exec 3<&-
 exit $?

# Set up to trap Ctrl-C
trap cleanUp SIGINT

# Port setting
stty -F /dev/ttyPIC raw ispeed 19200 ospeed 19200 cs8 -cstopb -parenb -echo
exec 3</dev/ttyPIC

echo 'Listening on ttyUSB:'

while [ 1 ]
 read line 0<&3 

 if [ "$line" = "clear" ]
  echo 'Listening on ttyUSB:'
  echo $line

   The only real magic in this script is the exec command, the trap, and the 'clear' signal.  The exec sets up the /dev/ttyUSB as a file pointer.  This allows the read command to work as expected.  I also use this for reading files in simple scripts.  The trap captures Ctrl+C, allowing the program to exit gracefully.  The 'clear' command allows the Pic to control the output somewhat.

   The C18 libraries seem to make working with I2C very easy.  They handle all the flag checking and bit setting/clearing needed to communicate over I2C.  There are a few caveats to this.

   First, some Pics have quirks when using I2C.  Some of these may be alleviated by following the following steps on start:

239     // This sillyness prevents weird stuff on the Latch from screwing up I2C
240     TRISB = 0x00;
241     LATB = 0x00;
242     TRISB = 0xFF;

   This snippet is from the main function.  The Tris register is set to all output.  The Latch of the register is cleared.  The Tris is then set to all input.  My code did not notice this addition.  Then again, enough posts are provide enough evidence that these steps will stay right where they are.

   The code ended up using the multi-byte read function, getsI2C.  This worked well to read in the six ID bytes as well as the 6 data bytes.  The code rejected the multi-byte write function, putsI2C.  This routine exits prematurely when the code wants to write value zero ( 0x00 ).  That was a pesky "feature" to discover!

   Something else that may trip up beginners like me:  you have to specify the speed of the communication after executing an OpenI2C function.  The datasheets don't really tell you how to calculate the value.  They do provide the values for some common speeds.  In this case, the datasheet for the Pic18F4550 did not have a value for 48MHz.  Hmmm.  I found the answer in two great places.  The first was on the PyroElectric website.  They have a short tutorial on I2C:  ( handy tip: click the read link in the top box to see the full tutorial.  This trips everybody up. ).  The other place was in Myke Predko's classic Programming and Customizing PICmicro Microcontrollers.  They both have the same formula:
   (FOsc / 4 / Desired Speed ) - 1

   For example, to get 100KHz standard speed at 48MHz, I use:
   (4.8e7 / 4 / 100000 ) - 1 = 119 or 0x77 

   Not a lot to say here.  The nunchuck is a pretty cool little gadget.  It consists of a microcontroller, an accelerometer, two potentiometers for the joystick, and two buttons.  The microcontroller handles reading the values and converting them.  It communicates via I2C at 3.3V and (nominally) 400KHz speed.

   The nunchuck used was a genuine Nintendo model.  I did not take the initialization shortcut because of this.  The code wants to read any nunchuck.  In fact, it will eventually read any extension controller whatsoever.  At least, it will read the nunchuck, the motion plus, the two together in pass-through mode, and the classic controller.

   For the physical interface, a piece of the blade of an old PCI modem was used to make the connector.  See the references below for ideas as well as pinouts.  As a safer, more robust alternative, all the usual web sites sell adapter boards.  Of them, Sparkfun's was the cheapest when I checked.  Another great idea is to buy a nunchuck extension cable and cut off an end.

   A good amount of research preceded this project.  No one site provided all the answers.  The best site by far was the site.  Here are the relevant pages:
All of the following were also of great help: ( Italian )

The comments in the page were very useful, providing insight and links to help with many small issues.

   Since I am still new to C for microcontrollers, I started with the convertRawValues and printValues functions.  These needed to be correct before the code got bogged down in the problems of talking to the nunchuck.  The packet format from the research was consistent across all sites.  The first bit of code set constant values for the data packets, focusing on the conversion and printing over USART.  Once this worked, it was time to try talking to the nunchuck.

   The research led to the following assumptions.  First, the's information seemed the most solid and consistent.  It was assumed that the initialization and device type enumeration procedures were the most recent and correct.  They would avoid any problems with encryption.

   A decision had to be made regarding the address of the device.  It was decided that the address was the seven bit value 0x52.  This would need to be shifted up one bit.  The lowest bit would hold a one or zero for reading(0xA5) or writing(0xA4) ( respectively ).

   The approach would always require a successful enumeration of the device type.  This would be crucial toward reading other devices.  It would be impossible to read multiple devices on the same line without it.

   The sites researched varied in many crucial points.  The controllers used, libraries imported, modifed, etc. all varied greatly.  There were too many variables to sort out.  Even with what turned out to be some solid assumptions and really good SWAGs ( Scientific, Wicked Ass Guesses ), some puzzling difficulties still cropped up.

   The first of these was speed.  All of the sites seemed to agree that the nunchuck operated at 400 KHz.  There were some vague references to logic analyzers showing they always ran at 100 KHz.  This project received very inconsistent results in initialization and device type enumeration at 400 KHz.  The project was operating at that speed with 1k resistors on the clock and data lines.  When this was changed to 100 KHz with 10k resistors, the results became much more consistent.

   This issue really slowed development.  After many tries, it became apparent that a minimum of 100us was needed between an I2C Start and Stop to produce consistent results.  The application was unable to break this barrier when operating at 48 MHz.  Perhaps a slower speed controller could lower or drop this required delay?

   The current application samples the data once every second and then (slowy) writes the results over USART.  Several posts and comments suggested that the highest sample rate was rather low.  This makes sense given that the interface is a Human Interface Device.  It would seem probable that the data could be sample every few hundred milliseconds.  This may not be enough for a really delicate control device, but it could be enough for simple applications.  The nunchuck has been used on some moderately successful self-balancing robots.  More research is needed here.

   Here are the specs for what worked in the project.  This has been a really long post.  It warrants some pictures and/or video.  These will be added as soon as time allows.  [ I'm proud of you for hanging with me this far!].  'Nuff said.  Here are the specs:

   Pic18F4550, running at 48 MHz via PLL from a 4 MHz ceramic resonator.
   USART @ 19200 baud through a TTL to Serial converter to a Serial to USB cable.
   Linux Laptop running OpenSuse - udev rule set up to map the serial/USB converter to /dev/ttyUSB.
   Bash script to display output from /dev/ttyUSB, clear on command, exit gracefully on Ctrl+C.
   Program written in C for Microchip C18 compiler(non-extended mode ) with built-in libraries.
   I2C at 100 KHz with 10k resistors as pull-ups on data and clock lines.
   Device Init with two writes:
      StartI2C 0xA4 0xF0 0x55 StopI2C (delay100us) Start 0xA4 0xFA 0x00 StopI2C
   Enumerate Device Type with:
      StartI2c 0xA4 0xFB StopI2C (delay200us) Start 0xA5 [ read 6 bytes with Ack/Nack ] StopI2C
   Program always reads device type immediately after init sequence.
   Read data with:
      StartI2C 0xA4 0x00 StopI2C (delay200us) StartI2C 0xA5 [read 6 bytes] StopI2C
   The conversion and print functions are pretty straight forward and commented in the code.

Here is the mainline code.  The missing bits are the config fuses and the delay routines.  The config fuses are pretty standard.  They are set up to run on XS_PLL.  There is a ceramic resonator of value 4 MHz.  Phase Lock Loop drives that to a practical 48 MHz ( 12 MIPs ).  The delays are a set of macros that use the Delay routines from delays.h.  I can post them if anyone asks.


  1 /*
  2  * File:  nunchuck.c
  3  * Author:  Tom Hunt
  4  *
  5  * Created on February 8, 2013, 9:51 PM
  6  *
  7  *  Running Pic 18F4550 @48 MHz
  8  *  Communicate with Wii Nunchuck over I2C, convert values,
  9  *  and send them to PC over USART at 19,200 baud
 10  *
 11  *  There seem to be several schools of thought on how to talk to Wii extension
 12  *  devices.  This program eventually wants to talk to the nunchuck and the
 13  *  motion plus in pass-through mode.  This will yield a 6 DoF IMU.  To this
 14  *  end, the program will try to use the protocols as outlined in the pages:
 15  *
 16  *
 17  *
 18  *
 19  *  It will attempt to use the universal initializations and run in unencrypted mode.
 20  *
 21  *  The main routine will initialize and go into a forever loop.  The loop will take
 22  *  a reading, convert it, send it, and delay about 1 second.
 23  *
 24  *  This works pretty well with the following caveats.
 25  *  First, it works much better at 100 KHz I2C speed.
 26  *  [Other issues have been fixed. :) ]
 27  *  
 28  *
 29  */
 31 #include 
 32 #include 
 33 #include 
 34 #include 
 35 #include 
 36 #include 
 38 #include "configFuses.h"
 39 #include "mydelays.h"
 41 // For 400 KHz: ( 48MHz / 4 / 400 KHz ) - 1 = 29 or 0x1D
 42 // For 100 KHz: ( 48MHz / 4 / 100 KHz ) - 1 = 119 or 0x77
 43 #define I2C_Speed                       0x77
 44 #define Slew                                SLEW_OFF
 46 #define extensionWriteAddress   0xA4
 47 #define extensionReadAddress    0xA5
 49 #define extensionInitRegister      0xF0
 50 #define extensionInitData           0x55
 52 #define extensionTypeReadRegister   0xFA
 53 #define extensionTypeWriteRegister   0xFB
 54 #define extensionTypeData        0x00
 56 #define extensionReadRegister   0x00
 58 #pragma udata access accessRam
 59 near unsigned char values[6];
 60 near unsigned int unpackedValues[7];
 62 #pragma romdata nunchuck
 63 const far rom unsigned char fail[] = "Read data failed.\n";
 65 #pragma code
 67 /**
 68  *  Convert the 6 raw bytes of data into the 7 nunchuck values.
 69  *  Store the converted values in finalValues.
 70  *  Raw Values:
 71  *  Byte                 Value
 72  * ---------------------------------------------------------------------------
 73  *       0                              SX<7:0>
 74  *       1                              SY<7:0>
 75  *       2                              AX<9:0>
 76  *       3                              AY<9:0>
 77  *       4                              AZ<9:0>
 78  *       5                              PackedByte(see below)
 79  *
 80  *   Bits:          7:6             5:4               3:2           1         0
 81  *Value:    AZ<1:0>   AY<1:0>    AX<1:0>   BC      BZ
 82  *
 83  * @param rawValues  - the 6 bytes retrieved from the nunchuck
 84  * @param finalValues - the array to hold the 7 converted values.
 85  */
 86 void convertRawValues( unsigned char* rawValues, unsigned int* finalValues )
 87 {
 88     int i = 0;
 89     unsigned char mask = 0x0C;
 90     unsigned char shiftAmount = 0x02;
 91     unsigned char packedByte = *(rawValues+5);
 93     // SX - Joystick X value --- set the value and increment raw values pointer
 94     *finalValues++ =*rawValues++ & 0x00FF;
 95     // SY - Joystick Y value --- set the value and increment raw values pointer
 96     *finalValues++ = *rawValues++ & 0x00FF;
 97     // This loop takes care of the 3 acelerometer values.  It gets the 7 High bits from each array value in turn.
 98     // It shifts these up 2, casting to an int as it goes.  C18 doesn't follow ANSI here, so the cast is needed.
 99     // Then the LSbs are shifted in.  As we go in order, all we need to do increment the shift by 2 and shift the
100     // mask by 2.
101     for( i = 0; i < 3; i++ ) {
102         *finalValues++ = ( ((unsigned int) *rawValues++) << 2 ) + ( ( packedByte & mask ) >> shiftAmount );
103          mask = mask<<2;
104          shiftAmount += 2;
105     }
106     // Need to do the buttons
107     // if the button bit is set, the button is not pressed.
108     // N.B. The value is inverted here to be like normal C.  Pressed = 1 = true
109     // The C button is bit 1 of the 6th byte
110     *finalValues++ = ( ( packedByte >> 1 ) & 1 ) ? 0x000 : 0x0001;
111     // The Z button is bit 0 of the 6th byte
112     *finalValues++ = ( ( packedByte >> 0 ) & 1 ) ? 0x000 : 0x0001;
114 }
116 void printDeviceType( unsigned char* values )
117 {
118     unsigned char i;
120     fprintf( _H_USART, "clear\n" );
121     fprintf( _H_USART, "Device Type:\n" );
122     for( i = 0; i < 6; i++ ) {
123         fprintf( _H_USART, "0x%X\n", *values++ );
124     }
125 }
127 /*
128  * Spew out the values over USART.
129  */
130 void printValues(  unsigned int* printValues )
131 {
132     fprintf( _H_USART, "clear\n" );
133     // Joystick:
134     fprintf(  _H_USART, "     X Joystick: %d\n", *printValues++ );
135     fprintf(  _H_USART, "     Y Joystick: %d\n", *printValues++ );
136     // Accelerometer:
137     fprintf(  _H_USART, "X Accelerometer: %d\n", *printValues++ );
138     fprintf(  _H_USART, "Y Accelerometer: %d\n", *printValues++ );
139     fprintf(  _H_USART, "Z Accelerometer: %d\n", *printValues++ );
141     // Now do the buttons
142     // C button
143     if( *printValues++ ) {
144         fprintf( _H_USART, "       C Button: Pressed\n" );
145     } else {
146         fprintf( _H_USART, "       C Button: Not Pressed\n" );
147     }
149     // Z button
150     if( *printValues ) {
151         fprintf( _H_USART, "       Z Button: Pressed\n"  );
152     } else {
153         fprintf( _H_USART, "       Z Button: Not Pressed\n"  );
154     }
156 }
158 /**
159  *  initialize the extension controller with universal method.
160  *  Ensure that the type identification block will be unencrypted.
161  *
162  * @return error - 0 on success, -2 on NotAck, -3 on WCOL
163  */
164 unsigned char initExtension( void )
165 {
166     unsigned char error = 0;
167     delay100us();
168     StartI2C();
169     error = putcI2C( extensionWriteAddress );
170     if( error ) return error;
171     error = putcI2C( extensionInitRegister );
172     if( error ) return error;
173     error = putcI2C( extensionInitData );
174     if( error ) return error;
175     StopI2C();
176     // introduce an artificial wait before sending new packet
177     delay100us();
178     StartI2C();
179     error = putcI2C( extensionWriteAddress );
180     if( error ) return error;
181     error = putcI2C( extensionTypeWriteRegister );
182     if( error ) return error;
183     error = putcI2C( extensionTypeData );
184     if( error ) return error;
185     StopI2C();
187     return 0;
188 }
190 unsigned char readSixBytes( unsigned char reg, unsigned char* values )
191 {
192     unsigned char error = 0;
193     unsigned char i;
195      // write the register from which we will read
196     delay200us();
197     StartI2C();
198     error = putcI2C( extensionWriteAddress );
199     if( error ) return error;
200     error = putcI2C( reg );
201     if( error ) return error;
202     StopI2C();
204     // Read the six data bytes at the read address
205     delay200us();
206     StartI2C();
207     error = putcI2C( extensionReadAddress );
208     if( error ) return error;
209     // Now, read in the 6 bytes
210     error = getsI2C( values, 6 );
211     if( error ) return error;
212     StopI2C();
213     delay200us();
214     return error;
215 }
217 // Get the type of the extension
218 unsigned char retrieveType( unsigned char* values )
219 {
220     unsigned char error = 0;
221     // Read the six data bytes
222     error = readSixBytes( extensionTypeReadRegister, values );
223     return error;
224 }
226 // Get the values for the extension and pack the raw data in values array.
227 unsigned char retrieveExtensionData( unsigned char* values )
228 {
229     unsigned char error = 0;
230     // Read the six data bytes
231     error = readSixBytes( extensionReadRegister, values );
232     return error;
233 }
235 void main( void )
236 {
237     unsigned char error = 0;
239     // This sillyness prevents weird stuff on the Latch from screwing up I2C
240     TRISB = 0x00;
241     LATB = 0x00;
242     TRISB = 0xFF;
244     INTCON = 0x00; // turn off all interrupts
245     // make sure we are all digital
246     ADCON0 = 0x00;
247     ADCON1 |= 0x0F;
248     // initialize the USART to 19,200 baud
249     // spbrgh = ( 48,000,000 / 19200 / 16 ) - 1 = 155.25
251                            & USART_ASYNCH_MODE & USART_EIGHT_BIT
252                            & USART_CONT_RX & USART_BRGH_HIGH,
253                              155 );
255     // initialize the I2C bus 
256     OpenI2C( MASTER, Slew );
257     // Now, set the speed in SSPADD.
258     SSPADD = I2C_Speed;
259     fprintf( _H_USART, "Starting.\n" );
260     // initialize the extension device
261     error = initExtension();
262     if( error == -2 ) {
263         fprintf( _H_USART, (const far rom char*) "Init Failed.  NOTACK\n" );
264     } else if( error  ) {
265         fprintf( _H_USART, "Init Failed. Write Collision\n" );
266     } else {
267         error = retrieveType( values );
268         if( ! error ) {
269             printDeviceType( values );
270             delay1sec();
271             delay1sec();
272             delay1sec();
273             delay1sec();
274             delay1sec();
276             while( 1 ) {
277                 // take reading, convert, and display every second
278                 error = retrieveExtensionData( values );
279                 if( error ) {
280                     fprintf( _H_USART, "clear\n" );
281                     fprintf( _H_USART, fail );
282                 } else {
283                     convertRawValues( values, unpackedValues );
284                     printValues( unpackedValues );
285                 }
286                 delay1sec();
287             }
288         }
289     }
291     fprintf( _H_USART, "I'm not doing anything right now...\n" );
292     // init error zone
293     while(1) {
294         Nop();
295     }
297 }

syntax highlighted by Code2HTML, v. 0.9.1

Next Steps
   First step:  Bask in the glow of having gotten it to work!  [ yea, me!]  Now, look to do some cool stuff.  Next up will be a quick one to get the classic controller working.  In theory, this should only require the data values arrays to expand in size, a handler based on the device type, and additional convert and print routines.  Let's see how that one works out!

   After that, it's time to tackle the Wii MotionPlus!  First up on that is to get the gyro reporting by itself.  That will mean a change to the addressing scheme ( maybe init, too.  Gotta research that ).  Then, add new get data bytes, convert, and print routines.  Once all that is working ( if ever ), it's on to getting the nunchuck and Motion Plus on the same wire in pass through mode.  To do anything useful with the Motion Plus, I'm going to need an extension cable.  This will allow the Motion Plus to be moved around freely.  It will also provide a much more stable interface cable.  The hack job of the PCI pins currently in use is pretty shaky!

   Thanks for reading.  I hope that this was helpful in some small way.  This deserves some eye candy.  Pictures really wouldn't be exciting or helpful.  A video might be cool.  I'll try to make one soon.