Index: trunk/code/projects/fp_math/fp_math.c
===================================================================
--- trunk/code/projects/fp_math/fp_math.c	(revision 1601)
+++ trunk/code/projects/fp_math/fp_math.c	(revision 1602)
@@ -9,29 +9,32 @@
 #define FP_TWO_PI 411775
 
 #define TABLE_LENGTH 64
-#define TABLE_STEP 1621//.160757
-#define EXP_MAGICONSTANT
+#define TABLE_STEP 1635//.160757
+#define EXP_MAGICONSTANT 726817 //ln(2^16) * 2^16
 
+//#define DEBUG
+
+//TODO This can probably be removed at some point.
 static int32_t linspace[TABLE_LENGTH] PROGMEM = { 
-	0, 1621, 3242, 4863, 6485, 8106, 9727, 11348, 
-	12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317, 
-	25939, 27560, 29181, 30802, 32423, 34044, 35666, 37287, 
-	38908, 40529, 42150, 43771, 45393, 47014, 48635, 50256, 
-	51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225, 
-	64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195, 
-	77816, 79437, 81058, 82679, 84300, 85922, 87543, 89164, 
-	90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133
+	0, 1634, 3268, 4902, 6536, 8170, 9804, 11438, 
+	13072, 14706, 16340, 17974, 19608, 21242, 22876, 24510, 
+	26144, 27778, 29412, 31047, 32681, 34315, 35949, 37583, 
+	39217, 40851, 42485, 44119, 45753, 47387, 49021, 50655, 
+	52289, 53923, 55557, 57191, 58825, 60459, 62093, 63727, 
+	65361, 66995, 68629, 70263, 71897, 73531, 75165, 76799, 
+	78433, 80067, 81701, 83335, 84969, 86603, 88237, 89871, 
+	91506, 93140, 94774, 96408, 98042, 99676, 101310, 102944, 
 };
 
 static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
-	65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, 
-	64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, 
-	60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, 
-	54322, 53398, 52442, 51454, 50434, 49384, 48303, 47193, 
-	46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, 
-	35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, 
-	24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, 
-	12089, 10492, 8889, 7280, 5667, 4050, 2431, 811
+	65536, 65516, 65455, 65353, 65210, 65027, 64804, 64540, 
+	64237, 63893, 63509, 63087, 62624, 62123, 61584, 61006, 
+	60390, 59736, 59046, 58319, 57555, 56756, 55921, 55052, 
+	54148, 53211, 52241, 51238, 50203, 49138, 48041, 46915, 
+	45760, 44576, 43364, 42126, 40861, 39571, 38256, 36918, 
+	35556, 34173, 32768, 31343, 29898, 28435, 26954, 25456, 
+	23943, 22415, 20872, 19317, 17750, 16171, 14583, 12986, 
+	11380, 9768, 8149, 6525, 4898, 3267, 1634, 0, 
 };
 
 //FIXME Lazy implementations of tangent and sine.
@@ -48,7 +51,7 @@
 	int32_t xsquare = fp_mult(x,x);
 	int32_t result, i;
 	
-	if(theta > EXP_MAGICONSTANT) { 
+	if(x > EXP_MAGICONSTANT) { 
 		usb_puts("Output value is out of range.\n\r");
 		return -1;
 	}
@@ -55,17 +58,17 @@
 	
 	//This is again, massive amounts of lazyness.
 	//The approximation fails outside this range (approximately).
-	if(theta < -17)
+	if(x < (-17l << 16))
 		return 0;
 	
 	result = xsquare;
-	for(i= (22 << 16); i > (2 << 16); i-= (4 << 16)) { 
+	for(i= (22l << 16); i > (2l << 16); i-= (4l << 16)) { 
 		result += i;
 		result = fp_div(xsquare, result);
 	}
 	
-	result += (2 << 16) - x;
-	return (1 << 16) + fp_div(fp_mult((2 << 16), x), result);
+	result += (2l << 16) - x;
+	return (1l << 16) + fp_div(fp_mult((2l << 16), x), result);
 
 }
 
@@ -76,7 +79,11 @@
 	int32_t x_n, x_np1, x_np2;
 	int32_t y_n, y_np1, y_np2;
 	int32_t dd_n, dd_np1, second_dd, result;
-	
+
+#ifdef DEBUG
+	char buf[128];
+#endif
+
 	//Move theta into [0, pi/2] w/ appropriate sign.
 	theta = ABS(theta) % FP_TWO_PI; 
 
@@ -90,29 +97,34 @@
 
 	//Find the nearest table values. FIXME
 	n = theta / TABLE_STEP;
-	while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) 
+	while( n < TABLE_LENGTH - 1 
+		&& (x_np1 = pgm_read_dword(&linspace[n+1]) < theta)) 
 		n++;
 	
 	//theta is between x_{n} and x_{n+1}
+
 	if(n == TABLE_LENGTH - 1) { 
 		//Perform linear interpolation, since we're close to zero anyway.
 		x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
 		y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
 
-		result = fp_mult(fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
+		result = fp_mult(
+			fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
+		
 		return negative ? -result : result;
 	}
 
 	if(n == TABLE_LENGTH) { 
 		//We didn't find a value! Oh no!
-		usb_puts("fp_math: We couldn't find surrounding table values! \n\r
-				  This should never happen!!!\n\r\n\r");
+		usb_puts("fp_math: Fatal! We couldn't find surrounding table values! \n\r");
+				  
 		return 0;
 	}
-	
+
 	//Address the general case. Quadratic interpolation.
 	//Load in the necessary values.
 	x_n = pgm_read_dword(&linspace[n]);
+	x_np1 = pgm_read_dword(&linspace[n + 1]);
 	x_np2  = pgm_read_dword(&linspace[n + 2]);
 	
 	y_n = pgm_read_dword(&fp_cos_table[n]);
@@ -119,6 +131,11 @@
 	y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
 	y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
 
+#ifdef DEBUG
+	sprintf(buf, "x_n = %ld, theta = %ld, x_{n+1} = %ld", x_n, theta, x_np1);
+	usb_puts(buf);
+#endif
+
 	//Calculate first divided differences.
 	dd_n = fp_div(y_np1 - y_n, x_np1 - x_n);
 	dd_np1 = fp_div(y_np2 - y_np1, x_np2 - x_np1);
Index: trunk/code/projects/fp_math/gentables.m
===================================================================
--- trunk/code/projects/fp_math/gentables.m	(revision 1601)
+++ trunk/code/projects/fp_math/gentables.m	(revision 1602)
@@ -1,11 +1,11 @@
 
-space = int32(round(linspace(0,pi,128) * 2^16))
-costable = int32(round(cos(linspace(0,pi,128)) * 2^16));
-sintable = int32(round(sin(linspace(0,pi,128)) * 2^16));
+TABLE_LENGTH = 64;
 
+space = int32(round(linspace(0, pi/2, TABLE_LENGTH) * 2^16));
+costable = int32(round(cos(linspace(0, pi/2, TABLE_LENGTH)) * 2^16));
+
 space_file = fopen('space_table.txt', 'w');
 cos_file = fopen('cos_table.txt', 'w');
-sin_file = fopen('sin_table.txt', 'w');
 
 for i=1:length(space)
 	fprintf(space_file, '%d, ', space(i));
@@ -20,11 +20,3 @@
 		fprintf(cos_file, '\n');
 	end
 end
-
-for i=1:length(sintable)
-	fprintf(sin_file, '%d, ', sintable(i));
-	if ~mod(i,8)
-		fprintf(sin_file, '\n');
-	end
-end
-
Index: trunk/code/projects/fp_math/test/serial.c
===================================================================
--- trunk/code/projects/fp_math/test/serial.c	(nonexistent)
+++ trunk/code/projects/fp_math/test/serial.c	(revision 1602)
@@ -0,0 +1,96 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+#include <unistd.h>
+#include "serial.h"
+
+/* open and configure the port */
+int openport(char* portname){
+  struct termios oldtio, newtio;
+  char *error = 0;
+  int fd;
+
+  /* open read/write, non-controlling, non-blocking */
+  fd = open(portname, O_RDWR | O_NOCTTY | O_NONBLOCK);
+  if (fd == -1){
+    fprintf(stderr, "Unable to open %s: ", portname);
+    perror(error);
+    exit(-1);
+  }
+
+  tcgetattr(fd, &oldtio);          //save current port settings
+  bzero(&newtio, sizeof(newtio));  //clear struct for new settings
+
+  newtio.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
+  newtio.c_iflag = IGNPAR | ICRNL;
+
+  newtio.c_oflag = 0;
+  newtio.c_lflag = 0;
+
+  newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */
+  newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
+  newtio.c_cc[VERASE]   = 0;     /* del */
+  newtio.c_cc[VKILL]    = 0;     /* @ */
+  newtio.c_cc[VEOF]     = 4;     /* Ctrl-d */
+  newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
+  newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives 
+*/
+  newtio.c_cc[VSWTC]    = 0;     /* '\0' */
+  newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */
+  newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
+  newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
+  newtio.c_cc[VEOL]     = 0;     /* '\0' */
+  newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
+  newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
+  newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
+  newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
+  newtio.c_cc[VEOL2]    = 0;     /* '\0' */
+
+  /* flush the port, and apply the new settings now */
+  tcflush(fd, TCIFLUSH);
+  tcsetattr(fd, TCSANOW, &newtio);
+
+  return (fd);
+}
+
+int putbyte (int fd, char c)
+{
+   int result;
+   result= write(fd, &c, 1);
+//   Sleep(25);
+   return result;
+}
+
+unsigned char getbyte (int fd)
+{
+  char c;
+  ssize_t retval;
+
+  retval = read(fd, &c, 1); 
+
+  if (retval <= 0)
+    return 255;
+
+  return c;
+}
+
+unsigned char getbyteblock (int fd)
+{
+  char c;
+  ssize_t retval;
+
+  while ((retval = read(fd, &c, 1)) <= 0){}
+//    usleep(5000);
+
+  return c;
+}
+
+void clearbuf(int fd)
+{
+  tcflush(fd, TCIFLUSH);
+}
+
Index: trunk/code/projects/fp_math/test/serial.h
===================================================================
--- trunk/code/projects/fp_math/test/serial.h	(nonexistent)
+++ trunk/code/projects/fp_math/test/serial.h	(revision 1602)
@@ -0,0 +1,10 @@
+#ifndef __SERIAL_H
+
+int openport(char *portname);
+int putbyte (int fd, char c);
+unsigned char getbyte (int fd);
+unsigned char getbyteblock (int fd);
+void clearbuf(int fd);
+
+#define __SERIAL_H
+#endif
Index: trunk/code/projects/fp_math/test/server_test.c
===================================================================
--- trunk/code/projects/fp_math/test/server_test.c	(nonexistent)
+++ trunk/code/projects/fp_math/test/server_test.c	(revision 1602)
@@ -0,0 +1,88 @@
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "serial.h"
+
+#define X_IN 0
+#define Y_OUT 1
+
+#define FP_COS 2
+#define FP_SIN 3
+#define FP_TAN 4
+
+#define EXIT 5
+
+typedef union fp_t { 
+	char d[4];
+	int v;
+} fp_t;
+
+int main(int argc, char** argv) { 
+	
+	int fd, i, n = 0;
+	float input, output, error, avg_error, max_error;
+	fp_t fp_input, fp_output;
+	
+	
+	fd = openport("/dev/ttyUSB0");
+	
+	printf("Beginning test loop.");
+
+	while(1) {
+
+		switch(getbyte(fd)) { 
+		
+		case X_IN:
+			for(i=0; i < 4; i++)
+				fp_input.d[i] = getbyte(fd);
+			
+			input = ((float)fp_input.v) / 65536.;
+
+			break;
+
+		case Y_OUT:
+			for(i=0; i < 4; i++)
+				fp_output.d[i] = getbyte(fd);
+			
+			output = ((float)fp_output.v) / 65536.;
+
+			break;
+
+		case FP_COS: 
+
+			n++;
+			printf("%f = cos(%f)\n", output, input);
+			error = abs(cos(input) - output);
+			if(error > max_error) max_error = error;
+			avg_error = (avg_error * (n - 1) + error) / n;
+			
+			break;
+
+		case FP_SIN:
+			
+			break;
+
+		case FP_TAN:
+			
+			break;
+
+		case EXIT:
+		
+			printf("Average error: %f\n", avg_error);
+			printf("Max error: %f\n", max_error);
+			exit(EXIT_SUCCESS);
+
+			break;
+
+		default: break;
+			printf("Uh oh - defauled\n");
+			
+			break;
+
+		}
+	}
+}
+
+
+
Index: trunk/code/projects/fp_math/test/Makefile
===================================================================
--- trunk/code/projects/fp_math/test/Makefile	(nonexistent)
+++ trunk/code/projects/fp_math/test/Makefile	(revision 1602)
@@ -0,0 +1,7 @@
+
+all:  serial.o
+	gcc server_test.c serial.o -lm -o server_test
+
+clean: 
+	rm *.o server_test
+
Index: trunk/code/projects/fp_math/space_table.txt
===================================================================
--- trunk/code/projects/fp_math/space_table.txt	(nonexistent)
+++ trunk/code/projects/fp_math/space_table.txt	(revision 1602)
@@ -0,0 +1,8 @@
+0, 1634, 3268, 4902, 6536, 8170, 9804, 11438, 
+13072, 14706, 16340, 17974, 19608, 21242, 22876, 24510, 
+26144, 27778, 29412, 31047, 32681, 34315, 35949, 37583, 
+39217, 40851, 42485, 44119, 45753, 47387, 49021, 50655, 
+52289, 53923, 55557, 57191, 58825, 60459, 62093, 63727, 
+65361, 66995, 68629, 70263, 71897, 73531, 75165, 76799, 
+78433, 80067, 81701, 83335, 84969, 86603, 88237, 89871, 
+91506, 93140, 94774, 96408, 98042, 99676, 101310, 102944, 
Index: trunk/code/projects/fp_math/main.c
===================================================================
--- trunk/code/projects/fp_math/main.c	(revision 1601)
+++ trunk/code/projects/fp_math/main.c	(revision 1602)
@@ -1,17 +1,47 @@
 #include <serial.h>
 #include <stdint.h>
+#include <stdlib.h>
 #include "fp_math.h"
 
+#define X_IN 0
+#define Y_OUT 1
+
+#define FP_COS 2
+#define FP_SIN 3
+#define FP_TAN 4
+
+#define EXIT 5
+
 int main(int argc, char** argv) { 
-	int32_t i,j;
+	int32_t i, x, y;
 	char buf[128];
+
+	usb_init();
 	
-	usb_init();
-	usb_puts("USB Initialized\n\r");
+	for(i=0; i < 10000; i++) { 
+
+		//Random number / sign
+		x = random();
+		x = ((rand() >> 2) & 1) ? x : -x;
 		
-	for(i=10000; i < 11000; i+=5) { 
-		sprintf(buf, "fp_cos(%ld) = %ld\n\r", i,  fp_cos(i));
-		usb_puts(buf);
+		usb_putc(X_IN);
+		for(i=0; i < 32; i += 8) 
+			usb_putc((x >> i)  & 0xFF);
+
+		y = fp_cos(x);	
+		
+		usb_putc(Y_OUT);
+		for(i=0; i < 32; i += 8) 
+			usb_putc((y >> i)  & 0xFF);
+	
+		usb_putc(FP_COS);
+		
+		//sprintf(buf, "%ld = cos(%ld)\n\r", y, x);
+		//usb_puts(buf);
 	}
+	
+	usb_putc(EXIT);
+
+	while(1);
 }
 
Index: trunk/code/projects/fp_math/cos_table.txt
===================================================================
--- trunk/code/projects/fp_math/cos_table.txt	(nonexistent)
+++ trunk/code/projects/fp_math/cos_table.txt	(revision 1602)
@@ -0,0 +1,8 @@
+65536, 65516, 65455, 65353, 65210, 65027, 64804, 64540, 
+64237, 63893, 63509, 63087, 62624, 62123, 61584, 61006, 
+60390, 59736, 59046, 58319, 57555, 56756, 55921, 55052, 
+54148, 53211, 52241, 51238, 50203, 49138, 48041, 46915, 
+45760, 44576, 43364, 42126, 40861, 39571, 38256, 36918, 
+35556, 34173, 32768, 31343, 29898, 28435, 26954, 25456, 
+23943, 22415, 20872, 19317, 17750, 16171, 14583, 12986, 
+11380, 9768, 8149, 6525, 4898, 3267, 1634, 0, 
Index: trunk/code/projects/fp_math/Makefile
===================================================================
--- trunk/code/projects/fp_math/Makefile	(revision 1601)
+++ trunk/code/projects/fp_math/Makefile	(revision 1602)
@@ -12,7 +12,7 @@
 
 # com1 = serial port. Use lpt1 to connect to parallel port.
 AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
-AVRDUDE_PORT = /dev/tty.usbserial-A4001gUV
+AVRDUDE_PORT = /dev/ttyUSB0
 
 
 else
