Index: branches/encoders/code/behaviors/spline/server/speed_map.h
===================================================================
--- branches/encoders/code/behaviors/spline/server/speed_map.h	(nonexistent)
+++ branches/encoders/code/behaviors/spline/server/speed_map.h	(revision 1342)
@@ -0,0 +1,7 @@
+#ifndef SPEED_MAP_H
+#define SPEED_MAP_H
+
+double real_velocity(int, int); 
+int robot_velocity(int, double);
+
+#endif
Index: branches/encoders/code/behaviors/spline/server/internals.h
===================================================================
--- branches/encoders/code/behaviors/spline/server/internals.h	(revision 1341)
+++ branches/encoders/code/behaviors/spline/server/internals.h	(revision 1342)
@@ -6,6 +6,9 @@
 #define RS 2
 #define RD 3
 
+#define LEFT 0
+#define RIGHT 1
+
 #define FORWARD 1
 #define BACKWARD 0
 #define SENDER 1
@@ -12,14 +15,14 @@
 #define RECEIVER 0
 
 #define INFINITE -1
-#define L 10.0
+#define L 13.5
 
-#define DT .1
+#define DT .102
 
-#define DIST(x1, y1, x2, y2)    (sqrt(pow(((x1)-(x2)), 2) + pow(((y1)-(y2)), 2)))
-#define LOOK_AHEAD              1
-#define LOOK_AHEAD_ERR_THRES    0.05
-#define VELOCITY                1
+#define DIST(x1, y1, x2, y2)    (sqrt(((x1)-(x2))*((x1)-(x2)) + ((y1)-(y2))*((y1)-(y2))))
+#define LOOK_AHEAD              30
+#define LOOK_AHEAD_ERR_THRES    5
+#define VELOCITY                20
 
 typedef struct state
 {
@@ -27,11 +30,11 @@
 	double y;
 	double theta;
 
-	short vl;
-	short vr;
+	double vl;
+	double vr;
 	
-	int vl_ref;
-	int vr_ref;
+	double vl_ref;
+	double vr_ref;
 } state;
 
 typedef struct {
@@ -45,7 +48,7 @@
 
 void setAttriv(int file);
 
-void pos_control_curv(curv_t *ref, int len);
+void pos_control_curve(curv_t *ref, int len);
 
 void velocityControl(double*, double*);
  
Index: branches/encoders/code/behaviors/spline/server/server.c
===================================================================
--- branches/encoders/code/behaviors/spline/server/server.c	(revision 1341)
+++ branches/encoders/code/behaviors/spline/server/server.c	(revision 1342)
@@ -7,12 +7,23 @@
 #include "encoders.h"
 #include "internals.h"
 #include <math.h>
+#include "speed_map.h"
 
+#include <time.h>
 
-#define PG 150
-#define DG 100
-#define IG 10
+#define PG 8
+#define DG .1
+#define IG .1
 
+int VEL_S = 1;
+
+
+#define TICKS_PER_CM 48.03
+#define TIME_INTERVAL 1520
+
+#define LEN 30000
+
+
 state robot;      
 double vel_l_i;
 double vel_r_i;
@@ -27,7 +38,7 @@
 	robot.theta = theta;
 
 	robot.vl = 0;
-	robot.vr = 0;
+	robot.vr = 0;     
 	robot.vl_ref = 0;
 	robot.vr_ref = 0;
 }
@@ -42,7 +53,7 @@
 	attributes.c_cflag &= ~PARENB;
 	attributes.c_cflag &= ~CSTOPB;
 	attributes.c_cflag &= ~CSIZE;
-	attributes.c_cflag |= CS8;
+	attributes.c_cflag |= 	CS8;
 	
 	attributes.c_cflag &= ~ICRNL;
 	attributes.c_cflag &= ~OCRNL;
@@ -56,8 +67,27 @@
 	}
 }
 
-int main()
+
+int main(int argc, char** args)
 {
+    init(50, 0, M_PI/2);
+	//init(0,0,0);
+	double x_ref[LEN];
+	double y_ref[LEN];
+    int i;
+	for (i = 0; i < LEN; i++)  {
+    	x_ref[i] = 50 * cos((double)(i+1) / LEN * M_PI);
+    	y_ref[i] = 50 * sin((double)(i+1) / LEN * M_PI);
+   // 	x_ref[i] = (double)i / LEN * 100;
+   //` 	y_ref[i] = (double)i / LEN * 100;
+		//printf("x_ref %g \t y_ref %g\n", x_ref[i], y_ref[i]);
+		//getchar();
+	}
+	
+	curv_t ref;                         
+	ref.x = x_ref;
+	ref.y = y_ref;
+	
 	int serialFileIn = open("/dev/ttyUSB0", O_RDWR);
 	int serialFileOut = open("/dev/ttyUSB1", O_RDWR);
 	if(serialFileIn < 1 || serialFileOut < 1)
@@ -68,9 +98,10 @@
     	
 	setAttrib(serialFileOut);
 	setAttrib(serialFileIn);
-        
-	unsigned char encoders[4] = {1,1,1,1};
-	
+                                           
+	//unsigned char encoders[4] = {0,0,0,0};
+	char ref_vels[2] = {0,0};
+
 	char senderNum = SENDER;
 	char receiverNum = RECEIVER;
 	
@@ -79,121 +110,109 @@
 	write(serialFileOut, &senderNum, 1);
 	
 	//Sending velocity as LS LD RS RD
-	int tempCount = 190;
+	int tempCount = 0;
 
-	int dx, left_v;
+	int dx;
 	double Ul, Ur;
 
-	while(1)
-	{
+	while(1) {
 
-		printf("SENDING DATA\n");
-
-        velocityControl(&Ul, &Ur);
-	    if(Ul > 0)
-			encoders[LD] = FORWARD;
-		else
-			encoders[LD] = BACKWARD;
-		encoders[LS] = (unsigned char)abs(Ul);
+        pos_control_curve(&ref, LEN);
+	    
+//		ref_vels[LEFT] = 20;
+//		ref_vels[RIGHT] = 20;
 		
-        if(Ur > 0)
-			encoders[RD] = FORWARD;
-		else
-			encoders[RD] = BACKWARD;
-		encoders[RS] = (unsigned char)abs(Ur);
+		ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);
+		ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);
+	
+//        printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
+       // printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
 
-
+		//tempCount++;
+		//printf("RECEIVED %d\n", tempCount);
 		int temp = 0;
 		int count = 0;
 		do
 		{
-			temp = write(serialFileOut, encoders + count, 1);	
+			temp = write(serialFileOut, ref_vels + count, 1);	
 			if(temp < 0)
 				perror("Write Error");
 			count += temp;
-			printf("sent: %d\n", count);
-			usleep(200);
-		}while(count < 4);
+			//printf("sent: %d\n", count);
+			usleep(10000);
+		   // sleep(1);
+		} while(count < 2);
+		
 		count = 0;
-		printf("READING DATA\n");
+		//printf("READING DATA\n");
 		do
 		{
-			count += read(serialFileIn, encoders, 4);
-		}while(count < 4);
+			count += read(serialFileIn, ref_vels, 2);
+		}while(count < 2);                         
 		
-		robot.vl = (encoders[0] << 8) | encoders[1];
-		robot.vr = (encoders[2] << 8) | encoders[3];
 		
-		//printf("%d %d\n", robot.vl, robot.vr);
+		robot.vl = real_velocity(1, -ref_vels[LEFT]);
+		robot.vr = real_velocity(1, -ref_vels[RIGHT]);
+		
+  //  	printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
 
 		updatePosition();
-		printf("Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
-
+		//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
+          printf("%g\t%g\n", robot.x, robot.y);
 	}
+    
+	return 0;
 
 }
 
 void scaleInput(double* input)
 {
+	printf("INPUT: %g\n", *input);
   	if(*input > 0)
 	{
-		(*input) += 170;
+		//(*input) += 170;
 		if(*input > 210)
 			*input = 210;
 	}
-	else
+	else if((*input) < 0)
 	{
-		(*input) -= 170;
+		//(*input) -= 170;
 		if(*input < -210)
 			*input = -210;
 	}
 }
 
-void velocityControl(double* Ul, double* Ur)
-{
-	double vel_l_e = robot.vl_ref - robot.vl;
-	double vel_r_e = robot.vr_ref - robot.vr;
-
-	vel_l_i = vel_l_i + vel_l_e;
-	vel_r_i = vel_r_i + vel_r_e;
-
-	(*Ul) = PG * vel_l_e + DG * (vel_l_e - vel_l_old) + IG * vel_l_i;
-	(*Ur) = PG * vel_r_e + DG * (vel_r_e - vel_r_old) + IG * vel_r_i;
-
-	scaleInput(Ul);
-	scaleInput(Ur);
-
-	vel_l_old = vel_l_e;
-	vel_l_old = vel_r_e;
-
-}
-
+//Todo: This makes no sense, 4am
 void updatePosition()
 {
    	double wdt = (robot.vr - robot.vl)*DT/L;
    	double R;           
 
-   	if(robot.vr - robot.vl > 0)
+   	if(robot.vr - robot.vl != 0)
    		R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
     
 	if(wdt != 0)
 	{
 		double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)};
-		robot.x = cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0];
-		robot.y = sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1];
+		robot.x = (cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0]);
+		robot.y = (sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1]);
 		robot.theta = robot.theta + wdt;  
 	}
 	else
 	{
-	   	robot.x += cos(robot.theta)*robot.vr;
-		robot.y += sin(robot.theta)*robot.vr;
+	   	robot.x += cos(robot.theta)*robot.vr*DT;
+		robot.y += sin(robot.theta)*robot.vr*DT;
 	}
+	if (robot.theta > 2*M_PI)
+		robot.theta -= 2*M_PI;
+	if (robot.theta < -2*M_PI)
+		robot.theta += 2*M_PI;
 }
 
 
 static int find_goal(curv_t *ref, int len);
-
-void pos_control_curv(curv_t *ref, int len) {
+    
+void pos_control_curve(curv_t *ref, int len) {
     int goal_idx = find_goal(ref, len);
     //did a transformation here.
     double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) 
@@ -200,8 +219,21 @@
         - sin(robot.theta)*(ref->x[goal_idx]-robot.x);
     double goal_rad = 2*goal_y / LOOK_AHEAD;
     
-    robot.vl_ref = VELOCITY*(1-0.5*goal_rad);
-    robot.vr_ref = VELOCITY*(1+0.5*goal_rad);
+    //printf("GOAL: y: %g,  o: %g\n", goal_y, goal_rad);
+
+    robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad);
+    robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad);
+    /*
+	if(robot.vl_ref > 127)
+		robot.vl_ref = 127;
+	else if(robot.vl_ref < -128)
+		robot.vl_ref = -128;
+	if(robot.vr_ref > 127)
+		robot.vr_ref = 127;
+	else if(robot.vr_ref < -128)
+		robot.vr_ref = -128;
+		*/
+
 }
 
 static int find_goal(curv_t *ref, int len) {
@@ -216,13 +248,20 @@
             min_idx = i;
         }
     }
+	//printf("%d min %f\n", min_idx, min_dist);
+	//getchar();
     goal_idx = -1;
     for (i = min_idx; i < len; i++) {
         temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
         if (abs(temp - LOOK_AHEAD) < LOOK_AHEAD_ERR_THRES) {
             goal_idx = i;
-            break;
-        }
+        	return goal_idx;
+		}
     }
+	while(1) {
+		printf("eol\n");
+	    exit(0);
+	}
     return goal_idx;
-}             
+} 
+
Index: branches/encoders/code/behaviors/spline/server/Makefile
===================================================================
--- branches/encoders/code/behaviors/spline/server/Makefile	(revision 1341)
+++ branches/encoders/code/behaviors/spline/server/Makefile	(revision 1342)
@@ -1,5 +1,5 @@
 all: server.c
-	gcc -lm encoders.c server.c -o server
+	gcc -lm -Wall speed_map.c server.c -o server
 
 clean:
 	rm server
Index: branches/encoders/code/behaviors/spline/server/speed_map.c
===================================================================
--- branches/encoders/code/behaviors/spline/server/speed_map.c	(nonexistent)
+++ branches/encoders/code/behaviors/spline/server/speed_map.c	(revision 1342)
@@ -0,0 +1,70 @@
+#include "speed_map.h"
+
+double robot_1[41] = 
+{
+	0,
+	0.495185695,
+	0.935064935,
+	1.385681293,
+	1.834862385,
+	2.278481013,
+	2.739726027,
+	3.20855615,
+	3.629032258,
+	4.295942721,
+	4.7,
+	5.013927577,
+	5.487804878,
+	5.940594059,
+	6.382978723,
+	6.844106464,
+	7.317073171,
+	7.75862069,
+	8.181818182,
+	8.653846154,
+	9.183673469,
+	9.677419355,
+	10.05586592,
+	10.34482759,
+	10.97560976,
+	11.39240506,
+	11.84210526,
+	12.4137931,
+	12.76595745,
+	13.23529412,
+	13.63636364,
+	14.17322835,
+	14.51612903,
+	15,
+	15.65217391,
+	15.78947368,
+	16.51376147,
+	16.66666667,
+	17.47572816,
+    17.84,
+	18.3
+};
+
+int robot_velocity(int robot, double velocity)
+{
+	if(velocity > 0)
+		return (int)((velocity+0.2757)/0.4645);
+	
+	velocity = -velocity;
+	return -(int)((velocity+0.2757)/0.4645);
+}
+
+double real_velocity(int robot, int velocity)
+{
+	if(velocity > 40 || velocity < -40)
+		return velocity < 0?-18.3:18.3;
+
+	switch(robot)
+	{
+		case 1:
+			return velocity < 0?-robot_1[-velocity]:robot_1[velocity];
+		default:
+			exit(-15410);
+	}
+
+}
