/ - Diff - Colony - Robotics Club

     /**
      * Copyright (c) 2007 Colony Project
+     *
      * Permission is hereby granted, free of charge, to any person
      * obtaining a copy of this software and associated documentation
      * files (the "Software"), to deal in the Software without
      * restriction, including without limitation the rights to use,
      * copy, modify, merge, publish, distribute, sublicense, and/or sell
      * copies of the Software, and to permit persons to whom the
      * Software is furnished to do so, subject to the following
      * conditions:
+     *
      * The above copyright notice and this permission notice shall be
      * included in all copies or substantial portions of the Software.
+     *
      * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
      * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
      * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
      * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
      * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
      * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
      * OTHER DEALINGS IN THE SOFTWARE.
      **/
     /**
+     *
      * @file math.h
      * @brief Contains math function(s)
+     *
      * @author Colony Project, CMU Robotics Club, James Kong
      */
     #ifndef _MATH_H_
     #define _MATH_H_
     /**
      * @addtogroup math
      * @{
      **/
     /** @brief absolute value **/
     int abs_int(int x);
     /**@}**/ //end group
     #endif

trunk/code/lib/include/libdragonfly/dragonfly_lib.h
87	87	#include <bom.h>
88	88	#include <move.h>
89	89	#include <reset.h>
90		#include <math.h>
91	90
92	91	#endif
93	92

     #include <avr/interrupt.h>
     #include <dragonfly_lib.h>
     long encoder_ltm1, encoder_rtm1;
     int encoder_ltm1, encoder_rtm1;
     long diff_x,diff_y;
     double angle;
     int odometry_dx(void){
     long odometry_dx(void){
     	return diff_x;
+    }
     int odometry_dy(void){
     long odometry_dy(void){
     	return diff_y;
+    }
-...
+    }
     void odometry_init(void){
     	encoders_init();
     	odometry_reset();
     	//CTC Mode. CLK / 1024
     	TCCR2 = 0; // (Fully disconnected)
-...
+    }
     void odometry_reset(void){
     	char buf[40];
     	diff_x = 0;
     	diff_y = 0;
     	encoder_ltm1 = encoder_read(LEFT);
     	encoder_rtm1 = encoder_read(RIGHT);
     	sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_ltm1,encoder_rtm1);
     	usb_puts(buf);
     	angle = 0.0;
+    }
     /*Currently assumes robot only goes forward.*/
     /*Currently assumes robot only goes forward.
     void odometry_run(void){
     	double theta;
     	int encoder_left, encoder_right,dl,dr;
-...
     	encoder_ltm1 = encoder_left;
     	encoder_rtm1 = encoder_right;
     }*/
     /*Generalized version of odometry.*/
     void odometry_run(void){
     	//Angle swept through in a time step CCW-
     	double theta, rl, dc;
     	long dr,dl;
     	char buf[40];
     	//Relative to the inner wheel, turn radius.
     	//Get the change in wheel positions
     	int encoder_right = encoder_read(RIGHT);
     	int encoder_left = encoder_read(LEFT);
     	sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_left,encoder_right);
     	usb_puts(buf);
     	dl = encoder_left - encoder_ltm1;
     	dr = encoder_right - encoder_rtm1;
     	if(dl >=-1 && dl <= 1  && dr >= -1 && dr <= 1) return;
     	encoder_ltm1 = encoder_left;
     	encoder_rtm1 = encoder_right;
     	//sprintf(buf,"(dl,dr) in clicks: (%ld,%ld)\n\r",dl,dr);
     	//usb_puts(buf);
     	// Try to avoid over/underflow.
     	dl = dl > 512 ? dl - 1024 :dl;
     	dl = dl < -512 ? dl + 1024 :dl;
     	dr = dr > 512 ? dr - 1024 :dr;
     	dr = dr < -512 ? dr + 1024 :dr;
     	sprintf(buf,"(dl,dr) in um: (%ld,%ld)\n\r",dl,dr);
     	usb_puts(buf);
     	//Convert "clicks" to um
     	dl *= CLICK_DISTANCE_UM; //um
     	dr *= CLICK_DISTANCE_UM;
     	if(dl == dr){
     		//usb_putc('#');
     		diff_x += dl*cos(angle)/1000;
     		diff_y += dl*sin(angle)/1000;
     		//sprintf(buf,"Distance in um: %ld\n\r",dl);
     		//usb_puts(buf);
     		return;
+    	}
     	//usb_putc('*');
     	//sprintf(buf,"(dl,dr) in um: (%ld,%ld)\n\r",dl,dr);
     	//usb_puts(buf);
     	//Call the left wheel 0, clockwise positive.
     	rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um
     	//sprintf(buf,"Radius from left wheel: %ld\n\r",rl);
     	//usb_puts(buf);
     	theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM); //rad
     	dc = (rl - ROBOT_WIDTH_UM/2)*theta; //um
     	//sprintf(buf,"Distance in um: %ld\n\r",dc);
     	//usb_puts(buf);
     	diff_x += ((dc * cos(angle))/1000); //mm
     	diff_y += ((dc * sin(angle))/1000); //mm
     	//sprintf(buf,"(dx, dy): (%ld, %ld)\n\r",diff_x, diff_y);
     	//usb_puts(buf);
     	//Change state variables.
     	//angle is necessarily CCW+, so subtract.
     	angle -= theta;
+    }
     ISR(TIMER2_COMP_vect){
     	odometry_run();
     	odometry_run();
+    }

     	odometry_init();
     	char buf[100];
     	while(1){
     		sprintf(buf,"X: %i, Y: %i, Theta: %f\n\r",odometry_dx(),odometry_dy(),odometry_angle());
     		usb_puts(buf);
     		//sprintf(buf,"X: %ld, Y: %ld, Theta: %lf\n\r", odometry_dx(), odometry_dy(), odometry_angle());
     		//usb_puts(buf);
     		delay_ms(500);
+    	}
     	return 0;

     //Standard measures will be mm and us
     int odometry_dx(void);
     int odometry_dy(void);
     long odometry_dx(void);
     long odometry_dy(void);
     double odometry_angle(void);
     void odometry_init(void);
     void odometry_reset(void);

     /**
      * Copyright (c) 2007 Colony Project
+     *
      * Permission is hereby granted, free of charge, to any person
      * obtaining a copy of this software and associated documentation
      * files (the "Software"), to deal in the Software without
      * restriction, including without limitation the rights to use,
      * copy, modify, merge, publish, distribute, sublicense, and/or sell
      * copies of the Software, and to permit persons to whom the
      * Software is furnished to do so, subject to the following
      * conditions:
+     *
      * The above copyright notice and this permission notice shall be
      * included in all copies or substantial portions of the Software.
+     *
      * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
      * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
      * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
      * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
      * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
      * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
      * OTHER DEALINGS IN THE SOFTWARE.
      **/
     /**
      * @file math.c
      * @brief Math Functions
+     *
      * Useful math functions.
+     *
      * @author Colony Project, CMU Robotics Club
      **/
     #include "math.h"
     /**
      * @defgroup math Math
      * Function(s) for performing math operations
+     *
      * @{
      **/
     /**
      * finds the absolute value of x
+     *
      * @param x the int of which we want to take its absolute value
+     *
      * @return the absolute value of x
      **/
     int abs_int (int x) {
       if(x < 0)
         return -x;
       return x;
+    }
     /**@}**/ //end defgroup

     /**
      * Copyright (c) 2007 Colony Project
+     *
      * Permission is hereby granted, free of charge, to any person
      * obtaining a copy of this software and associated documentation
      * files (the "Software"), to deal in the Software without
      * restriction, including without limitation the rights to use,
      * copy, modify, merge, publish, distribute, sublicense, and/or sell
      * copies of the Software, and to permit persons to whom the
      * Software is furnished to do so, subject to the following
      * conditions:
+     *
      * The above copyright notice and this permission notice shall be
      * included in all copies or substantial portions of the Software.
+     *
      * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
      * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
      * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
      * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
      * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
      * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
      * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
      * OTHER DEALINGS IN THE SOFTWARE.
      **/
     /**
+     *
      * @file math.h
      * @brief Contains math function(s)
+     *
      * @author Colony Project, CMU Robotics Club, James Kong
      */
     #ifndef _MATH_H_
     #define _MATH_H_
     /**
      * @addtogroup math
      * @{
      **/
     /** @brief absolute value **/
     int abs_int(int x);
     /**@}**/ //end group
     #endif

trunk/code/projects/libdragonfly/dragonfly_lib.h
87	87	#include <bom.h>
88	88	#include <move.h>
89	89	#include <reset.h>
90		#include <math.h>
91	90
92	91	#endif
93	92

Project

General

Profile

Colony

Revision 924