/ - Diff - Colony - Robotics Club

     #include "odometry.h"
     #include <encoders.h>
     #include <math.h>
     #include <avr/interrupt.h>
     #include <dragonfly_lib.h>
     long encoder_ltm1, encoder_rtm1;
     long diff_x,diff_y;
     double angle;
     int odometry_dx(void){
     	return diff_x;
+    }
     int odometry_dy(void){
     	return diff_y;
+    }
     double odometry_angle(void){
     	return angle;
+    }
     void odometry_init(void){
     	encoders_init();
     	odometry_reset();
     	//CTC Mode. CLK / 1024
     	TCCR2 = 0; // (Fully disconnected)
     	TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode.
     	TIMSK |= _BV(OCIE2);
     	OCR2 = ODOMETRY_CLK;
+    }
     void odometry_reset(void){
     	diff_x = 0;
     	diff_y = 0;
     	angle = 0.0;
+    }
     /*Currently assumes robot only goes forward.*/
     void odometry_run(void){
     	double theta;
     	int encoder_left, encoder_right,dl,dr;
     	long distance_um;
     	char buf[100];
     	//Relative to the inner wheel, the radius of its turn.
     	double turning_radius;
     	encoder_left = encoder_read(LEFT);
     	encoder_right = encoder_read(RIGHT);
     	dl = encoder_left - encoder_ltm1;
     	dr = encoder_right - encoder_rtm1;
     	dl = dl > 512 ? dl - 1024 :dl;
     	dl = dl < -512 ? dl + 1024 :dl;
     	dr = dr > 512 ? dr - 1024 :dr;
     	dr = dr < -512 ? dr + 1024 :dr;
     	//idle?
     	if(dl < 2 && dr < 2) return;
     	if(dl == dr){
     		distance_um = dl*CLICK_DISTANCE_UM;
     		//sprintf(buf,"Distance in um: %ld\n\r",distance_um);
     		//usb_puts(buf);
     		diff_x += distance_um*cos(angle)/1000;
     		diff_y += distance_um*sin(angle)/1000;
     		return;
+    	}
     	if(dr > dl){ //CCW positive.
     		turning_radius = ROBOT_WIDTH_UM * dl / (dr - dl); //um
     		theta = ((double)(dr-dl)*CLICK_DISTANCE_UM) / ((double)ROBOT_WIDTH_UM);
     		distance_um = theta*((double)(turning_radius + ROBOT_WIDTH_UM/2));
     		//sprintf(buf,"Distance in um: %d\n\r",distance_um);
     		//usb_puts(buf);
+    	}
     	else{ //CW negative.
     		turning_radius = ROBOT_WIDTH_UM * dr / (dl - dr); //um
     		theta = -((double)(dl-dr)*CLICK_DISTANCE_UM) / ((double)ROBOT_WIDTH_UM);
     		distance_um = -theta*((double)(turning_radius + ROBOT_WIDTH_UM/2));
     		//sprintf(buf,"Distance in um: %d\n\r",distance_um);
     		//usb_puts(buf);
+    	}
     	diff_x += distance_um * cos(angle)/1000;
     	diff_y += distance_um * sin(angle)/1000;
     	angle += theta;
     	encoder_ltm1 = encoder_left;
     	encoder_rtm1 = encoder_right;
+    }
     ISR(TIMER2_COMP_vect){
     	odometry_run();
+    }

     #ifndef __ODOMETRY_C__
     #define __ODOMETRY_C__
     //Odometry resolution, *64 microseconds.
     //= approx. 100 ms
     #define ODOMETRY_CLK 255u
     //Wheel = 2.613 in.
     //Circumference = 208.508133 mm
     //Distance per encoder click (circumference / 1024)  = 203.621224 um.
     //Robot width = 5.3745 in. = 136.5123 mm
     #define ROBOT_WIDTH_UM 137000  //um
     #define CLICK_DISTANCE_UM 204 //um
     //Standard measures will be mm and us
     int odometry_dx(void);
     int odometry_dy(void);
     double odometry_angle(void);
     void odometry_init(void);
     void odometry_reset(void);
     #endif

     #include <dragonfly_lib.h>
     #include <wireless.h>
     #include <encoders.h>
     //#include "odometry.h"
     #include "odometry.h"
     #define POLLING_INTERVAL 50	/* interval in ms to get sensor data */
     #define POLLING_INTERVAL 200	/* interval in ms to get sensor data */
     #define MAP 1	// packet group for sending data points
     #define POINT_RAW 1 // packet type for map data points w/ raw encoder data
     #define POINT_ODO 2 // packet type for map data points w/ odometry data
-...
     #define CNTL_LEFT_CURVE 4
     #define CNTL_RIGHT_CURVE 5
     #define CNTL_STOP    6
     #define VEL_UP  7
     #define VEL_DOWN 8
     #define CNTL_VEL_UP  7
     #define CNTL_VEL_DOWN 8
     int state;
     int velocity;
-...
+    {
         /* initialize components and join the token ring */
         dragonfly_init(ALL_ON);
         range_init();
         //range_init();
         //encoders_init();
         odometry_init();
         wl_init();
         encoders_init();
         wl_set_channel(0xF);
         wl_register_packet_group(&love_handle);
         wl_register_packet_group(&packetHandler);
-...
         //wl_token_ring_register();
         //wl_token_ring_join();
         //run_around_init();
         //odometry_init();
         velocity = 160;
         state = CNTL_STOP;
         //int x, y; // use when sending odometry data
         //float theta;
         int x, y; // use when sending odometry data
         double theta;
         int store[7];
         int store[9];
         char *send;   // for transmission
         while(1)
-...
     	wl_do();
     	//run_around_FSM();	/* traverse the environment and avoid obstacles ... */
     	delay_ms(POLLING_INTERVAL);	/* until it's time to grab map data */
     	//x = odometry_dx(); // use when sending odometry data
     	//y = odometry_dy();
     	//theta = odometry_angle();
     	store[0] = encoder_get_dx(LEFT);    // left encoder distance
     	store[1] = encoder_get_dx(RIGHT);   // right encoder distance
     	store[2] = range_read_distance(IR1);	// IR1 range
     	store[3] = range_read_distance(IR2);	// IR2 range
     	store[4] = range_read_distance(IR3);	// IR3 range
     	store[5] = range_read_distance(IR4);	// IR4 range
     	store[6] = range_read_distance(IR5);	// IR5 range
     	x = odometry_dx(); // use when sending odometry data
     	y = odometry_dy();
     	theta = odometry_angle();
     	//store[0] = encoder_get_dx(LEFT);    // left encoder distance
     	//store[1] = encoder_get_dx(RIGHT);   // right encoder distance
     	store[0] = x;
         store[1] = y;
         *((double*)((int*)store + 2)) = theta;
         store[4] = range_read_distance(IR1);	// IR1 range
     	store[5] = range_read_distance(IR2);	// IR2 range
     	store[6] = range_read_distance(IR3);	// IR3 range
     	store[7] = range_read_distance(IR4);	// IR4 range
     	store[8] = range_read_distance(IR5);	// IR5 range
         sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
               *((double *)((int*)store+2)), store[4], store[5], store[6],
               store[7], store[8] );
         usb_puts(buf);
     	// convert to chars for transmission
     	//for (int i = 0; i < 7; i++)
-...
     	send = (char*) store;
     	// send point w/ raw encoder data
     	wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);
     	// wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);
     	// send point w/ odometry data
     	// wl_send_global_packet(MAP, POINT_ODO, send, 8, 0);
     	wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
     	// send raw encoder data point over serial as a string
     	// send[7] = 0;
     	// usb_puts(send);	// this is probably ugly, but I don't know any better
     	// send raw encoder data point over serial as integers
     	for (int i = 0; i < 7; i++)
     	/*for (int i = 0; i < 7; i++)
+    	{
     	    usb_puti(store[i]);
     	    usb_putc(' ');
+    	}
     	usb_putc('\n');
     	usb_puti(range_read_distance(IR1));
     	usb_putc('\n');
         usb_putc('\n');*/
+        }
         wl_terminate();
-...
             case CNTL_STOP:
                 state = CNTL_STOP;
                 break;
             case VEL_UP:
             case CNTL_VEL_UP:
                 if (velocity > 250)
                     velocity = 250;
                 else
                     velocity += 5;
                 break;
             case VEL_DOWN:
             case CNTL_VEL_DOWN:
                 velocity -= 5;
                 if (velocity < 150)
                     velocity = 150;

     PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt
     # If this is left blank, then it will use the Standard printf version.
     PRINTF_LIB =
     #PRINTF_LIB =
     #PRINTF_LIB = $(PRINTF_LIB_MIN)
     #PRINTF_LIB = $(PRINTF_LIB_FLOAT)
     PRINTF_LIB = $(PRINTF_LIB_FLOAT)
     # Minimalistic scanf version

     #include <curses.h> // you need to install the ncurses library
     #include "../../libwireless/lib/wireless.h"
     #define MAP 1   // packet group for receiving points
     #define POINT_RAW 1 // packet type for map data points w/ raw encoder data
     #define POINT_ODO 2 // packet type for map data points w/ odometry data
     #define WL_CNTL_GROUP 4
     #define CNTL_FORWARD 0
-...
             return 1;
+        }
         WINDOW* win = initscr();
         nodelay(win, TRUE);
         printf("Beginning: robot=%d\r\n", robot);
     	wl_set_com_port("/dev/ttyUSB0");
     	wl_set_com_port("/dev/ttyUSB1");
     	wl_init();
     	wl_set_channel(0xF);
     	printf("Wireless initialized.\r\n");
-...
+    }
     void packet_receive (char type, int source, unsigned char* packet, int length) {
         //expected input: x y theta IR1 IR2 IR3 IR4 IR5
         int i, temp;
         for (i = 0; i < length; i += 2) {
             temp = (char)packet[i+1] << 8;
             temp |= (char)packet[i];
             fprintf(file, "%d ", temp);
         if (type == POINT_RAW) {
             //expected input: enc_l enc_r IR1 IR2 IR3 IR4 IR5
             int i, temp;
             for (i = 0; i < length; i += 2) {
                 temp = (char)packet[i+1] << 8;
                 temp |= (char)packet[i];
                 fprintf(file, "%d ", temp);
+            }
             fprintf(file, "\n");
+        }
         fprintf(file, "\n");
         else if (type == POINT_ODO) {
             // expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5
             int x,y,ir1,ir2,ir3,ir4,ir5;
             //double theta;
             float theta;
             long theta_temp;
             x = (char)packet[0] + ((char)packet[1] << 8);
             y = (char)packet[2] + ((char)packet[3] << 8);
             theta_temp = (char)packet[4] + ((char)packet[5] << 8)
                 + ((char)packet[5] << 16) + ((char)packet[6] << 24);
             theta = (float)theta_temp;
             ir1 = (char)packet[8] + ((char)packet[9] << 8);
             ir2 = (char)packet[10] + ((char)packet[11] << 8);
             ir3 = (char)packet[12] + ((char)packet[13] << 8);
             ir4 = (char)packet[14] + ((char)packet[15] << 8);
             ir5 = (char)packet[16] + ((char)packet[17] << 8);
             fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta, ir1, ir2, ir3, ir4, ir5);
+        }
+    }

Project

General

Profile

Colony

Revision 915