Index: branches/encoders/code/behaviors/spline/server/internals.h
===================================================================
--- branches/encoders/code/behaviors/spline/server/internals.h	(revision 1343)
+++ branches/encoders/code/behaviors/spline/server/internals.h	(revision 1344)
@@ -1,16 +1,9 @@
 #ifndef INTERNALS_H
 #define INTERNALS_H
 
-#define LS 0
-#define LD 1
-#define RS 2
-#define RD 3
-
 #define LEFT 1
 #define RIGHT 0
 
-#define FORWARD 1
-#define BACKWARD 0
 #define SENDER 1
 #define RECEIVER 0
 
@@ -21,9 +14,9 @@
 #define DT .088
 
 #define DIST(x1, y1, x2, y2)    (sqrt(((x1)-(x2))*((x1)-(x2)) + ((y1)-(y2))*((y1)-(y2))))
-#define LOOK_AHEAD              40
+#define LOOK_AHEAD              30
 #define LOOK_AHEAD_ERR_THRES    5
-#define VELOCITY                10
+#define VELOCITY                15
 
 typedef struct state
 {
Index: branches/encoders/code/behaviors/spline/server/server.c
===================================================================
--- branches/encoders/code/behaviors/spline/server/server.c	(revision 1343)
+++ branches/encoders/code/behaviors/spline/server/server.c	(revision 1344)
@@ -66,23 +66,66 @@
 	}
 }
 
-#define RADIUS          100
+//#define FORWARD
+
+#ifdef FORWARD
+#undef LEFT
+#undef RIGHT
+#define LEFT 0
+#define RIGHT 1
+#endif
+
+#define CIRCLE	
+//#define SINE
+//#define COSINE
+//#define LINE45
+//#define LINE
+
+#define RADIUS          70
+#define AMP				70
+
 int main(int argc, char** args)
 {
-    //init(RADIUS, 0, M_PI/2);
-	init(0, 50, 0);
-	//init(0,0,0);
+#ifdef CIRCLE
+    init(20, 30, 0);
+#endif
+#ifdef SINE
+	init(0, 0, M_PI/4);
+#endif
+#ifdef COSINE
+	init(0, AMP, 0);
+#endif
+#ifdef LINE45
+	init(0,0,0);
+#endif
+#ifdef LINE
+	init(0,0,0);
+#endif
 	double x_ref[LEN];
 	double y_ref[LEN];
     int i;
 	for (i = 0; i < LEN; i++)  {
-//        x_ref[i] = RADIUS * cos((double)(i+1) / LEN * 5.0 / 2.0 * M_PI);
-//        y_ref[i] = RADIUS * sin((double)(i+1) / LEN * 5.0 / 2.0 * M_PI);
- 
+#ifdef CIRCLE
+      	x_ref[i] = RADIUS * cos((double)(i) / LEN * 2.0 * M_PI);
+        y_ref[i] = RADIUS * sin((double)(i) / LEN * 2.0 * M_PI);
+#endif
+#ifdef SINE
 		x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;
-    	y_ref[i] = 50 *cos (x_ref[i] / 16.6667);//(double)(i+1) / LEN * 3.0 / 2.0 * M_PI);
-   // 	x_ref[i] = (double)i / LEN * 100;
-   // 	y_ref[i] = (double)i / LEN * 100;
+    	y_ref[i] = AMP *sin (x_ref[i] / 50);
+#endif		 
+#ifdef COSINE
+		x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;
+    	y_ref[i] = AMP *cos (x_ref[i] / 50);
+#endif		 
+#ifdef LINE45
+    	x_ref[i] = (double)i / LEN * 1000;
+    	y_ref[i] = (double)i / LEN * 1000;
+#endif
+#ifdef LINE
+    	x_ref[i] = (double)i / LEN * 1000;
+    	y_ref[i] = 0;
+#endif
+
         //printf("%g \t%g\n", x_ref[i], y_ref[i]);
 		//getchar();
 	}
@@ -127,9 +170,14 @@
   //  	ref_vels[LEFT] = 20;
   //  	ref_vels[RIGHT] = 20;
 		
+
 		ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);
 		ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);
-	
+	#ifdef FORWARD
+		ref_vels[LEFT] = -ref_vels[LEFT];
+		ref_vels[RIGHT] = -ref_vels[RIGHT];
+	#endif
+
         //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]);
 
@@ -156,10 +204,15 @@
 		}while(count < 2);                         
 		
 		
+#ifndef FORWARD
 		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);
+#else
+        robot.vl = real_velocity(1, ref_vels[LEFT]);
+		robot.vr = real_velocity(1, ref_vels[RIGHT]);            
+#endif
+	
+    	//printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
 
 		updatePosition();
 		//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
@@ -199,12 +252,12 @@
     
 void pos_control_curve(curv_t *ref, int len) {
     int goal_idx = find_goal(ref, len);
+    //printf("\tGOAL: x: %g,  y: %g\n", ref->x[goal_idx], ref->y[goal_idx]);
     //did a transformation here.
     double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) 
         - sin(robot.theta)*(ref->x[goal_idx]-robot.x);
     double goal_rad = 2*goal_y / LOOK_AHEAD;
     
-    //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);
@@ -234,12 +287,22 @@
             min_idx = i;
         }
     }
+	/*
+    if (min_dist > LOOK_AHEAD) {
+		printf("toooooo far away \n");
+		return min_idx;
+	}
+    */
 	//printf("%d min %f\n", min_idx, min_dist);
 	//getchar();
     goal_idx = -1;
 	err = LOOK_AHEAD_ERR_THRES;
-	while(goal_idx == -1 && ctr < 3) {
+	while(ctr < 3) {
 	    for (i = min_idx; i < len; i++) {
+#ifdef CIRCLE
+			if (i == len-1)
+				i = 0;
+#endif
     	    temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
         	if (abs(temp - LOOK_AHEAD) < err) {
             	goal_idx = i;
@@ -250,7 +313,7 @@
 		ctr++;
 	}
 	while(1) {
-		//printf("eol\n");
+		printf("eol\n");
 	    exit(0);
 	}
     return goal_idx;
Index: branches/encoders/code/behaviors/spline/server/speed_map.c
===================================================================
--- branches/encoders/code/behaviors/spline/server/speed_map.c	(revision 1343)
+++ branches/encoders/code/behaviors/spline/server/speed_map.c	(revision 1344)
@@ -1,4 +1,5 @@
 #include "speed_map.h"
+#include <stdlib.h>
 
 double robot_1[41] = 
 {
Index: branches/encoders/code/behaviors/spline/slave/slave.c
===================================================================
--- branches/encoders/code/behaviors/spline/slave/slave.c	(revision 1343)
+++ branches/encoders/code/behaviors/spline/slave/slave.c	(revision 1344)
@@ -3,49 +3,98 @@
 
 #define GROUP 1
 #define MOTOR 0
-#define ENCODER 1
+#define ENCODER 132
 
-#define LS 0
-#define LD 1
-#define RS 2
-#define RD 3
+void packet_receive (char type, int source, char* packet, int length);
+void ctrl(void);
 
-#define H0 0
-#define L0 1
-#define H1 2
-#define L1 3
-
-void packet_receive (char type, int source, unsigned char* packet, int length);
-
 PacketGroupHandler packetHandler = {GROUP, NULL, NULL, &packet_receive, NULL};
 
 char buf[32];
-int count = 0;
-int left_dir = FORWARD;
-int right_dir = FORWARD;
-int left_speed = 0;
-int right_speed = 0;
+long unsigned int count = 0;
+volatile int left_dir = FORWARD;
+volatile int right_dir = FORWARD;
 
+volatile int left_ref;
+volatile int right_ref;
+
+volatile int left_old;
+volatile int right_old;
+
+volatile int left_i = 0;
+volatile int right_i = 0;
+
+volatile int left_u;
+volatile int right_u;
+
+volatile int left_vel = 0;
+volatile int right_vel = 0;
+
+#define PL	1
+#define DL	0.1
+#define IL	0.1
+
+#define PR	1.5
+#define DR	0.2
+#define IR	0.1
+
+
+#define INTERVAL	50
+
 /* receive motor speed from master: 
  * left_speed left_dir right_speed right_dir
  * then sent raw encoder values back to master: 
  * high_left low_left high_right low_right 
  */
-void packet_receive (char type, int source, unsigned char* packet, int length) {
+void packet_receive (char type, int source, char* packet, int length) {
 	if (type != MOTOR) return;
-	//orb_set_color(WHITE);
-	left_dir = packet[LD];
-	left_speed = packet[LS];
-	right_dir = packet[RD];
-	right_speed = packet[RS];
-	sprintf(buf, "Received: %d %d %d %d\n", packet[LS], packet[LD], packet[RS], packet[RD]);
+	
+	left_ref = packet[0];
+	right_ref = packet[1];
+
+	sprintf(buf, "received: %d %d\n", left_ref, right_ref);
 	usb_puts(buf);
-	//motor1_set(packet[LD],packet[LS]);
-	//motor2_set(packet[RD],packet[RS]);
 	//orb_set_color(GREEN);
 }
 
+void ctrl() {
+	int left_err = left_ref - left_vel;
+	int right_err = right_ref - right_vel;
+	
+	left_err = left_vel == 2048 ? left_old : left_err;
+	right_err = right_vel == 2048 ? right_old : right_err;
+
+	left_i += left_err;
+	left_u = (int)(PL*(float)left_err + DL*(float)(left_err - left_old) + IL*(float)left_i);
+	left_old = left_err;
+	
+	right_i += right_err;
+	right_u = (int)(PR*(float)right_err + DR*(float)(right_err - right_old) + IR*(float)right_i);
+	right_old = right_err;
+
+	if (left_u < 0) {
+		left_dir = 0;
+		left_u = -left_u;
+	}
+	else
+		left_dir = 1;
+
+	if (right_u < 0) {
+		right_u = -right_u;
+		right_dir = 0;
+	}
+	else
+		right_dir = 1;
+
+	left_u += 160;
+	right_u += 160;
+
+	left_u = left_u > 210 ? 210 : left_u;
+	right_u = right_u > 210 ? 210 : right_u;
+}
+
 int main (void) {
+	char encoder[2];
 	dragonfly_init(ALL_ON);
 	usb_init();
 	encoders_init();
@@ -52,28 +101,42 @@
 	//usb_puts("xbee id ");
 	//usb_putc(wl_get_xbee_id());
 	wl_init();
-
+    left_ref = 20;
+	right_ref = 20;
 	wl_register_packet_group(&packetHandler);
 	//orb_set_color(RED);
 	//motor1_set(FORWARD,230);
 	//motor2_set(FORWARD,230);
-
+	
 	while (1) {
-		int encoder_left = encoder_get_v(LEFT);
-		int encoder_right = encoder_get_v(RIGHT);
-		unsigned char encoder[4];
-		encoder[H0] = (encoder_left >> 8) & 0xFF;
-		encoder[L0] = (encoder_left) & 0xFF;
-		encoder[H1] = (encoder_right >> 8) & 0xFF;
-		encoder[L1] = (encoder_right) & 0xFF;
-		wl_send_global_packet(GROUP, ENCODER, encoder, 4, 0);
-		sprintf(buf, "Sent (%d): %d %d\n", count++, encoder_left, encoder_right);
-		usb_puts(buf);
+		usb_puts("We Win\n");
+	    //left_vel = encoder_get_v(0);
+		//right_vel = encoder_get_v(1);
+		 delay_ms(1000);
+		//if (!(count % INTERVAL)) {
+		//	wl_send_global_packet(GROUP, ENCODER, encoder, 2, 0);
+			encoder[0] = left_vel;
+			encoder[1] = right_vel;
+		
+		   // usb_puti(left_vel);
+		  //  sprintf(buf, "sent (%d): %d %d %d %d\n", count, left_vel, right_vel, encoder[0], encoder[1]);
+		   // usb_puts(buf);
+		   // usb_puti(encoder[0]);
+	  //  }
+		
 
-		wl_do();
-		motor1_set(left_dir, left_speed);
-		motor2_set(right_dir, right_speed);
-		delay_ms(100);
+		//ctrl();
+	
+	  //  if (!(count % INTERVAL))
+	  //  	wl_do();
+		
+//		sprintf(buf, "motor (%d): %d %d %d %d\n", count, left_dir, left_ref, right_dir, right_ref);
+//		usb_puts(buf);
+
+	    //motor1_set(left_dir, left_u);
+		//motor2_set(right_dir, right_u);
+		
+		count++;
+	    delay_ms(1000);
 	}
 }
-
Index: branches/encoders/code/behaviors/spline/master/master.c
===================================================================
--- branches/encoders/code/behaviors/spline/master/master.c	(revision 1343)
+++ branches/encoders/code/behaviors/spline/master/master.c	(revision 1344)
@@ -6,29 +6,35 @@
 
 #define GROUP 1
 #define MOTOR 0
-#define ENCODER 1
+#define ENCODER 132
 
 #define SENDER 1
 #define RECEIVER 0
 
-unsigned char data[4];
+int id = -1;
 
+char data[2];
+
 sentCount = 0;
 char position = 0;
 
-void packet_receive (char type, int source, unsigned char* packet, int length);
+void packet_receive (char type, int source, char* packet, int length);
 
 PacketGroupHandler packetHandler = {GROUP, 0, 0, &packet_receive, 0};
 
-void packet_receive (char type, int source, unsigned char* packet, int length) {
+void packet_receive (char type, int source, char* packet, int length) {
 
 	if(type == ENCODER)
-	{
+	{ 
+		if(id == -1)
+			id = source;
+		if(source != id)
+			motor1_set(1, 200);
 		usb_putc(packet[0]);
 		usb_putc(packet[1]);
-		usb_putc(packet[2]);
-		usb_putc(packet[3]);
-	}
+//		usb_putc(packet[2]);
+//		usb_putc(packet[3]);
+	}                                  
 	
 }
 
@@ -49,12 +55,12 @@
 		lcd_puts("starting init");
 		data[0] = usb_getc(); 
 		data[1] = usb_getc();
-		data[2] = usb_getc();
-		data[3] = usb_getc();
+		//data[2] = usb_getc();
+		//data[3] = usb_getc();
 		
 		lcd_puts("Done with init\n");
 
-		wl_send_global_packet(GROUP, MOTOR, data, 4, 0);
+		wl_send_global_packet(GROUP, MOTOR, data, 2, 0);
 	}
 	else
 	{
@@ -67,10 +73,10 @@
   {
 		data[0] = usb_getc(); 
 		data[1] = usb_getc();
-		data[2] = usb_getc();
-		data[3] = usb_getc();
-		wl_send_global_packet(GROUP, MOTOR, data, 4, 0);
-}	
+		//data[2] = usb_getc();
+		//data[3] = usb_getc();
+		wl_send_global_packet(GROUP, MOTOR, data, 2, 0);
+	}	
 
 	  wl_do();
   }
