Index: serial.c
===================================================================
--- serial.c	(revision 1344)
+++ serial.c	(revision 1345)
@@ -1,10 +1,38 @@
-/*
-	serial.c - Functions for using the RS232 serial port
-	
-	authors: Robotics Club, Colony Project, pkv
-	much code taken from FWR's library, author: Tom Lauwers
-*/
+/**
+ * Copyright (c) 2007 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
 
+
+/**
+ * @file serial.c
+ * @brief Serial Input and Output
+ *
+ * Implementation of functions for serial input and output.
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
 #include <avr/io.h>
 #include <stdio.h>
 #include "serial.h"
@@ -21,29 +49,29 @@
   //Set baud rate
   // - 115200 (both wired and wireless) is UBRR=8, U2X=1
   // - 9600 is U2X =1, UBRR = 107.
-	#if (USB_BAUD == 115200)
-    UBRR0H = 0x00;
-    UBRR0L = 8;
-    UCSR0A |= _BV(U2X0);
-  #elif (USB_BAUD == 9600)
-    UBRR0H = 0x00;
-    UBRR0L = 103;
-    UCSR0A |= _BV(U2X0);
-  #else //Baud rate is defined in the header file, we should not get here
-    return;
-  #endif
+#if (USB_BAUD == 115200)
+  UBRR0H = 0x00;
+  UBRR0L = 8;
+  UCSR0A |= _BV(U2X0);
+#elif (USB_BAUD == 9600)
+  UBRR0H = 0x00;
+  UBRR0L = 103;
+  UCSR0A |= _BV(U2X0);
+#else //Baud rate is defined in the header file, we should not get here
+  return;
+#endif
 
   /*Enable receiver and transmitter */
-	UCSR0B |= (1<<RXEN0)|(1<<TXEN0);
+  UCSR0B |= (1<<RXEN0)|(1<<TXEN0);
 	
-	/* Set frame format: 8data, 1stop bit, asynchronous normal mode */
-	UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);
+  /* Set frame format: 8data, 1stop bit, asynchronous normal mode */
+  UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);
   
   // if we have enabled the stdio stuff, then we init it here
-  #ifdef USE_STDIO
-    /* Open the stdio stream corresponding to this port */
-    usb_fd = fdevopen(usb_putc, usb_getc);
-  #endif
+#ifdef USE_STDIO
+  /* Open the stdio stream corresponding to this port */
+  usb_fd = fdevopen(usb_putc, usb_getc);
+#endif
 }
 
 /**
@@ -55,29 +83,29 @@
   //Set baud rate
   // - 115200 (both wired and wireless) is UBRR=8, U2X=1
   // - 9600 is U2X =1, UBRR = 107.
-	#if (XBEE_BAUD == 115200)
-    UBRR1H = 0x00;
-    UBRR1L = 8;
-    UCSR1A |= _BV(U2X1);
-  #elif (XBEE_BAUD == 9600)
-    UBRR1H = 0x00;
-    UBRR1L = 103;
-    UCSR1A |= _BV(U2X1);
-  #else //Baud rate is defined in the header file, we should not get here
-    return;
-  #endif
+#if (XBEE_BAUD == 115200)
+  UBRR1H = 0x00;
+  UBRR1L = 8;
+  UCSR1A |= _BV(U2X1);
+#elif (XBEE_BAUD == 9600)
+  UBRR1H = 0x00;
+  UBRR1L = 103;
+  UCSR1A |= _BV(U2X1);
+#else //Baud rate is defined in the header file, we should not get here
+  return;
+#endif
 
-	//Enable receiver and transmitter
-	UCSR1B |= (1<<RXEN1)|(1<<TXEN1);
+  //Enable receiver and transmitter
+  UCSR1B |= (1<<RXEN1)|(1<<TXEN1);
 	
-	// Set frame format: 8data, 1stop bit, asynchronous normal mode
-	UCSR1C |= (1<<UCSZ10) | (1<<UCSZ11);
+  // Set frame format: 8data, 1stop bit, asynchronous normal mode
+  UCSR1C |= (1<<UCSZ10) | (1<<UCSZ11);
   
   // if we have enabled the stdio stuff, then we init it here
-  #ifdef USE_STDIO
-    /* Open the stdio stream corresponding to this port */
-    xbee_fd = fdevopen(xbee_putc, xbee_getc);
-  #endif
+#ifdef USE_STDIO
+  /* Open the stdio stream corresponding to this port */
+  xbee_fd = fdevopen(xbee_putc, xbee_getc);
+#endif
 }
 
 /**
@@ -86,8 +114,7 @@
  * @param c the character to send
  * @return 0 for success, nonzero for failure
  **/
-int usb_putc(char c) 
-{
+int usb_putc(char c) {
   // Wait until buffer is clear for sending
   loop_until_bit_is_set(UCSR0A, UDRE0);
 	
@@ -102,8 +129,7 @@
  * @param c the character to send
  * @return 0 for success, nonzero for failure
  **/
-int xbee_putc(char c) 
-{
+int xbee_putc(char c) {
   // Wait until buffer is clear for sending
   loop_until_bit_is_set(UCSR1A, UDRE1);
 	
@@ -118,18 +144,32 @@
  * @param s the string to send
  * @return 0 for success, nonzero for failure
  **/
-int usb_puts(char *s)
-{
-	char *t = s;
-	while (*t != 0)
-	{
-		usb_putc(*t);
-		t++;
-	}
+int usb_puts(char *s) {
+  char *t = s;
+  while (*t != 0) {
+    usb_putc(*t);
+    t++;
+  }
   return 0;
 }
 
 /**
+ * Sends a sequence of characters from program space over USB.
+ *
+ * @param s the string to send
+ **/
+void usb_puts_P (PGM_P s) {
+    char buf;
+	
+    while (memcpy_P (&buf, s, sizeof (char)), buf!=0) {
+        usb_putc (buf);
+        s++;
+    }
+}
+
+
+
+/**
  * Returns the first character in the buffer received from USB.
  * This function blocks execution until a character has been received.
  * xbee_init must be called before this function may be used.
@@ -138,13 +178,12 @@
  *
  * @see usb_init, usb_getc_nb
  **/
-int usb_getc(void)
-{
+int usb_getc(void) {
   // Wait for the receive buffer to be filled
   loop_until_bit_is_set(UCSR0A, RXC0);
 	
-	// Read the receive buffer
-	return UDR0;
+  // Read the receive buffer
+  return UDR0;
 }
 
 /**
@@ -157,13 +196,12 @@
  * 
  * @see xbee_init, xbee_getc_nb
  **/
-int xbee_getc(void)
-{
-	// Wait for the receive buffer to be filled
-    loop_until_bit_is_set(UCSR1A, RXC1);
+int xbee_getc(void) {
+  // Wait for the receive buffer to be filled
+  loop_until_bit_is_set(UCSR1A, RXC1);
 	
-	// Read the receive buffer
-	return UDR1;
+  // Read the receive buffer
+  return UDR1;
 }
 
 /**
@@ -178,18 +216,16 @@
  * 
  * @see usb_init, usb_getc
  **/
-int usb_getc_nb(char *c)
-{
-	// check if the receive buffer is filled
+int usb_getc_nb(char *c) {
+  // check if the receive buffer is filled
   if (UCSR0A & _BV(RXC0)) {
     // Read the receive buffer
     (*c) = UDR0;
     return 0;
-  }
-  else {
+  } else {
     // Return empty
     return -1;
-	}
+  }
 }
 
 /**
@@ -204,26 +240,24 @@
  *
  * @see xbee_init, xbee_getc
  **/
-int xbee_getc_nb(char *c)
-{
-	// check if the receive buffer is filled
+int xbee_getc_nb(char *c) {
+  // check if the receive buffer is filled
   if (UCSR1A & _BV(RXC1)) {
     // Read the receive buffer
     (*c) = UDR1;
     return 0;
-  }
-  else {
+  } else {
     // Return empty
     return -1;
-	}
+  }
 }
 
 
 /*
-prints an int to serial
+  prints an int to serial
 
-code adapted from Chris Efstathiou's code (hendrix@otenet.gr)
-uses usb_putc
+  code adapted from Chris Efstathiou's code (hendrix@otenet.gr)
+  uses usb_putc
 */
 /**
  * Prints an integer, converted to ASCII, to usb. usb_init must be called
@@ -236,21 +270,74 @@
  * @see usb_init, usb_putc
  **/
 int usb_puti(int value ) {
-	unsigned char usb_data[6]={'0','0','0','0','0','0' }, position=sizeof(usb_data), radix=10; 
+  unsigned char usb_data[6]={'0','0','0','0','0','0' }, position=sizeof(usb_data), radix=10; 
 
-        /* convert int to ascii  */ 
-        if(value<0) { usb_putc('-'); value=-value; }    
-        do { position--; *(usb_data+position)=(value%radix)+'0'; value/=radix;  } while(value); 
+  /* convert int to ascii  */ 
+  if(value<0) { 
+    usb_putc('-'); 
+    value=-value; 
+  }    
+  do { 
+    position--; 
+    *(usb_data+position)=(value%radix)+'0'; 
+    value/=radix;  
+  } while(value); 
 
     
-        /* start displaying the number */
-        for(;position<=(sizeof(usb_data)-1);position++)
-          {
+  /* start displaying the number */
+  for(;position<=(sizeof(usb_data)-1);position++) {
             
-            usb_putc(usb_data[position]);
-          }
+    usb_putc(usb_data[position]);
+  }
 
-	return 0;
+  return 0;
 }
 
+/**
+ * Determines a hexadecimal digit in ASCII code.
+ *
+ * @param value the value of the digit (0<=value<=15)
+ * 
+ * @return the hexadecimal digit in ASCII code, or '?'
+ * if the input is invalid.
+ **/
+uint8_t hex_digit (uint8_t value)
+{
+    if (value>15) return '?';
+    // Postcondition: 0<=x<=15
 
+    return "0123456789ABCDEF"[value];
+}
+
+/**
+ * Prints a fixed width hexadecimal representation of an unsigned
+ * 16 bit integer in ASCII code to USB.
+ * usb_init must be called before this function can be used.
+ *
+ * @param value the value to print
+ * 
+ * @see usb_init, usb_puti, usb_puts, usb_puth8, hex_digit
+ **/
+void usb_puth16 (uint16_t value)
+{
+    usb_putc (hex_digit((value >>12)&0xF));
+    usb_putc (hex_digit((value >>8 )&0xF));
+    usb_putc (hex_digit((value >>4 )&0xF));
+    usb_putc (hex_digit( value      &0xF));
+}
+
+/**
+ * Prints a fixed width hexadecimal representation of an unsigned
+ * 8 bit integer in ASCII code to USB.
+ * usb_init must be called before this function can be used.
+ *
+ * @param value the value to print
+ * 
+ * @see usb_init, usb_puti, usb_puts, usb_puth16, hex_digit
+ **/
+void usb_puth8(uint8_t value)
+{
+    usb_putc (hex_digit ((value)>>4 &0xF));
+    usb_putc (hex_digit ( value     &0xF));
+}
+
