/ - Diff - Colony - Robotics Club

     #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
-...
     #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
+    {

+    	}
+    }
     #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();
+    	}
-...
       //  	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]);
-...
     		}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);
-...
     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);
-...
                 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;
-...
     		ctr++;
+    	}
     	while(1) {
     		//printf("eol\n");
     		printf("eol\n");
     	    exit(0);
+    	}
         return goal_idx;

branches/encoders/code/behaviors/spline/server/speed_map.c
1	1	#include "speed_map.h"
	2	#include <stdlib.h>
2	3
3	4	double robot_1[41] =
4	5	{

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

     #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]);
+    	}
+    }
-...
     		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
+    	{
-...
+      {
     		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();
+      }

Project

General

Profile

Colony

Revision 1344