Index: slave.c
===================================================================
--- slave.c	(revision 1343)
+++ 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);
 	}
 }
-
