Revision 1345
Final spline code for master/slave
updated outdated libdragonfly and libwireless
move.c  

1 
/** 

2 
* Copyright (c) 2007 Colony Project 

3 
* 

4 
* Permission is hereby granted, free of charge, to any person 

5 
* obtaining a copy of this software and associated documentation 

6 
* files (the "Software"), to deal in the Software without 

7 
* restriction, including without limitation the rights to use, 

8 
* copy, modify, merge, publish, distribute, sublicense, and/or sell 

9 
* copies of the Software, and to permit persons to whom the 

10 
* Software is furnished to do so, subject to the following 

11 
* conditions: 

12 
* 

13 
* The above copyright notice and this permission notice shall be 

14 
* included in all copies or substantial portions of the Software. 

15 
* 

16 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 

17 
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 

18 
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 

19 
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 

20 
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 

21 
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 

22 
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 

23 
* OTHER DEALINGS IN THE SOFTWARE. 

24 
**/ 

25  
26  
27 
/** 

28 
* @file move.c 

29 
* @brief Functions for moving 

30 
* 

31 
* Implementation of functions for moving the robot. 

32 
* 

33 
* @author Colony Project, CMU Robotics Club 

34 
**/ 

35  
1  36 
#include "dragonfly_lib.h" 
2  37  
3  38 
#include "move.h" 
...  ...  
8  43 
void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr); 
9  44  
10  45 
// global varaibles for move_avoid 
11 
int d1, d2, d3, d4, d5; 

12  
46 
int d1, d2, d3, d4, d5; 

13  47 
/** 
14  48 
* @defgroup move Movement 
15  49 
* @brief Functions fo controlling robot motion 
...  ...  
70  104 
* 
71  105 
* @see motors_init, range_init, move 
72  106 
**/ 
73 
void move_avoid(int velocity, int omega, int strength){ 

107 
void move_avoid(int velocity, int omega, int strength) {


74  108 
int pControl; 
75  109 
int vl = 0; 
76  110 
int vr = 0; 
...  ...  
92  126 
d5=(temp == 1) ? d5 : temp; 
93  127 

94  128 
/* Avoid obstacles ahead 
95 
if(d2>170) 

96 
v*=1; 

129 
if(d2>170)


130 
v*=1;


97  131 

98 
Naturally slow down if there is something in the way. 

99 
if(d2>150  d1>180  d3>180){ 

100 
v>>=1; 

132 
Naturally slow down if there is something in the way.


133 
if(d2>150  d1>180  d3>180){


134 
v>>=1;


101  135 
*/ 
102  136 

103  137 
//pControl= (((d3d1) + (d4d5))*strength)/100; 
...  ...  
120  154 
/**@}**///end the motion group 
121  155  
122  156 
void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) { 
123 
//omega: angle measure, positive couterclockwise from front.


124 
// 180 <= omega <= 180


125 
//velocity: 255 <= velocity <= 255


157 
//omega: angle measure, positive couterclockwise from front.


158 
// 180 <= omega <= 180


159 
//velocity: 255 <= velocity <= 255


126  160 

127  161 
long int vltemp, vrtemp; 
128  162 

...  ...  
130  164 
if (velocity < 255  velocity >255  omega < 255  omega > 255) return; 
131  165 

132  166 
//compute 
133 
vrtemp = velocity + omega * 3;


134 
vltemp = velocity  omega * 3;


167 
vrtemp = velocity + omega * 3;


168 
vltemp = velocity  omega * 3;


135  169 

136 
//check to see if max linear velocities have been exceeded.


170 
//check to see if max linear velocities have been exceeded.


137  171 
if (vrtemp > 255) { 
138  172 
vltemp = 255 * vltemp / vrtemp; 
139  173 
vrtemp = 255; 
140  174 
} 
141  175 
if (vltemp > 255) { 
142 
vrtemp = 255 * vrtemp / vltemp;


143 
vltemp = 255;


176 
vrtemp = 255 * vrtemp / vltemp;


177 
vltemp = 255;


144  178 
} 
145  179 
if (vrtemp < 255) { 
146  180 
vltemp = 255 * vltemp / vrtemp; 
...  ...  
153  187 

154  188 
*vr = (int)vrtemp; 
155  189 
*vl = (int)vltemp; 
156 


157  
158  190 
} 
159  
160 
Also available in: Unified diff