/ - Diff - Colony - Robotics Club

     #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 = {
 , 1621, 3242, 4863, 6485, 8106, 9727, 11348,
 , 14590, 16212, 17833, 19454, 21075, 22696, 24317,
 , 27560, 29181, 30802, 32423, 34044, 35666, 37287,
 , 40529, 42150, 43771, 45393, 47014, 48635, 50256,
 , 53498, 55119, 56741, 58362, 59983, 61604, 63225,
 , 66468, 68089, 69710, 71331, 72952, 74573, 76195,
 , 79437, 81058, 82679, 84300, 85922, 87543, 89164,
 , 92406, 94027, 95648, 97270, 98891, 100512, 102133
 , 1634, 3268, 4902, 6536, 8170, 9804, 11438,
 , 14706, 16340, 17974, 19608, 21242, 22876, 24510,
 , 27778, 29412, 31047, 32681, 34315, 35949, 37583,
 , 40851, 42485, 44119, 45753, 47387, 49021, 50655,
 , 53923, 55557, 57191, 58825, 60459, 62093, 63727,
 , 66995, 68629, 70263, 71897, 73531, 75165, 76799,
 , 80067, 81701, 83335, 84969, 86603, 88237, 89871,
 , 93140, 94774, 96408, 98042, 99676, 101310, 102944,
     };
     static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
 , 65516, 65456, 65356, 65215, 65035, 64815, 64556,
 , 63919, 63541, 63125, 62670, 62176, 61645, 61076,
 , 59826, 59146, 58430, 57678, 56890, 56068, 55212,
 , 53398, 52442, 51454, 50434, 49384, 48303, 47193,
 , 44886, 43691, 42470, 41222, 39949, 38652, 37331,
 , 34622, 33235, 31828, 30401, 28956, 27492, 26013,
 , 23006, 21481, 19943, 18393, 16831, 15260, 13679,
 , 10492, 8889, 7280, 5667, 4050, 2431, 811
 , 65516, 65455, 65353, 65210, 65027, 64804, 64540,
 , 63893, 63509, 63087, 62624, 62123, 61584, 61006,
 , 59736, 59046, 58319, 57555, 56756, 55921, 55052,
 , 53211, 52241, 51238, 50203, 49138, 48041, 46915,
 , 44576, 43364, 42126, 40861, 39571, 38256, 36918,
 , 34173, 32768, 31343, 29898, 28435, 26954, 25456,
 , 22415, 20872, 19317, 17750, 16171, 14583, 12986,
 , 9768, 8149, 6525, 4898, 3267, 1634, 0,
     };
     //FIXME Lazy implementations of tangent and sine.
-...
     	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;
+    	}
     	//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);
+    }
-...
     	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;
-...
     	//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]);
     	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);

     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));
-...
     		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

     #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);
+    }

     #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

     #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;
+    		}
+    	}
+    }

trunk/code/projects/fp_math/test/Makefile
	1
	2	all: serial.o
	3	gcc server_test.c serial.o -lm -o server_test
	4
	5	clean:
	6	rm *.o server_test
	7

 , 1634, 3268, 4902, 6536, 8170, 9804, 11438,
 , 14706, 16340, 17974, 19608, 21242, 22876, 24510,
 , 27778, 29412, 31047, 32681, 34315, 35949, 37583,
 , 40851, 42485, 44119, 45753, 47387, 49021, 50655,
 , 53923, 55557, 57191, 58825, 60459, 62093, 63727,
 , 66995, 68629, 70263, 71897, 73531, 75165, 76799,
 , 80067, 81701, 83335, 84969, 86603, 88237, 89871,
 , 93140, 94774, 96408, 98042, 99676, 101310, 102944,

     #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);
+    }

 , 65516, 65455, 65353, 65210, 65027, 64804, 64540,
 , 63893, 63509, 63087, 62624, 62123, 61584, 61006,
 , 59736, 59046, 58319, 57555, 56756, 55921, 55052,
 , 53211, 52241, 51238, 50203, 49138, 48041, 46915,
 , 44576, 43364, 42126, 40861, 39571, 38256, 36918,
 , 34173, 32768, 31343, 29898, 28435, 26954, 25456,
 , 22415, 20872, 19317, 17750, 16171, 14583, 12986,
 , 9768, 8149, 6525, 4898, 3267, 1634, 0,

     # 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

Project

General

Profile

Colony

Revision 1602