Revision 380
fixed some style stuff
| move.c | ||
|---|---|---|
| 43 | 43 |
void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr); |
| 44 | 44 |
|
| 45 | 45 |
// global varaibles for move_avoid |
| 46 |
int d1, d2, d3, d4, d5; /** |
|
| 46 |
int d1, d2, d3, d4, d5; |
|
| 47 |
/** |
|
| 47 | 48 |
* @defgroup move Movement |
| 48 | 49 |
* @brief Functions fo controlling robot motion |
| 49 | 50 |
* Higher level functions to control the movement of robots. |
| ... | ... | |
| 103 | 104 |
* |
| 104 | 105 |
* @see motors_init, range_init, move |
| 105 | 106 |
**/ |
| 106 |
void move_avoid(int velocity, int omega, int strength){
|
|
| 107 |
void move_avoid(int velocity, int omega, int strength) {
|
|
| 107 | 108 |
int pControl; |
| 108 | 109 |
int vl = 0; |
| 109 | 110 |
int vr = 0; |
| ... | ... | |
| 125 | 126 |
d5=(temp == -1) ? d5 : temp; |
| 126 | 127 |
|
| 127 | 128 |
/* Avoid obstacles ahead |
| 128 |
if(d2>170) |
|
| 129 |
v*=-1; |
|
| 129 |
if(d2>170)
|
|
| 130 |
v*=-1;
|
|
| 130 | 131 |
|
| 131 |
Naturally slow down if there is something in the way. |
|
| 132 |
if(d2>150 || d1>180 || d3>180){
|
|
| 133 |
v>>=1; |
|
| 132 |
Naturally slow down if there is something in the way.
|
|
| 133 |
if(d2>150 || d1>180 || d3>180){
|
|
| 134 |
v>>=1;
|
|
| 134 | 135 |
*/ |
| 135 | 136 |
|
| 136 | 137 |
//pControl= (((d3-d1) + (d4-d5))*strength)/100; |
| ... | ... | |
| 153 | 154 |
/**@}**///end the motion group |
| 154 | 155 |
|
| 155 | 156 |
void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) {
|
| 156 |
//omega: angle measure, positive couter-clockwise from front.
|
|
| 157 |
// -180 <= omega <= 180
|
|
| 158 |
//velocity: -255 <= velocity <= 255
|
|
| 157 |
//omega: angle measure, positive couter-clockwise from front.
|
|
| 158 |
// -180 <= omega <= 180
|
|
| 159 |
//velocity: -255 <= velocity <= 255
|
|
| 159 | 160 |
|
| 160 | 161 |
long int vltemp, vrtemp; |
| 161 | 162 |
|
| ... | ... | |
| 163 | 164 |
if (velocity < -255 || velocity >255 || omega < -255 || omega > 255) return; |
| 164 | 165 |
|
| 165 | 166 |
//compute |
| 166 |
vrtemp = velocity + omega * 3;
|
|
| 167 |
vltemp = velocity - omega * 3;
|
|
| 167 |
vrtemp = velocity + omega * 3;
|
|
| 168 |
vltemp = velocity - omega * 3;
|
|
| 168 | 169 |
|
| 169 |
//check to see if max linear velocities have been exceeded.
|
|
| 170 |
//check to see if max linear velocities have been exceeded.
|
|
| 170 | 171 |
if (vrtemp > 255) {
|
| 171 | 172 |
vltemp = 255 * vltemp / vrtemp; |
| 172 | 173 |
vrtemp = 255; |
| 173 | 174 |
} |
| 174 | 175 |
if (vltemp > 255) {
|
| 175 |
vrtemp = 255 * vrtemp / vltemp;
|
|
| 176 |
vltemp = 255;
|
|
| 176 |
vrtemp = 255 * vrtemp / vltemp;
|
|
| 177 |
vltemp = 255;
|
|
| 177 | 178 |
} |
| 178 | 179 |
if (vrtemp < -255) {
|
| 179 | 180 |
vltemp = -255 * vltemp / vrtemp; |
| ... | ... | |
| 186 | 187 |
|
| 187 | 188 |
*vr = (int)vrtemp; |
| 188 | 189 |
*vl = (int)vltemp; |
| 189 |
|
|
| 190 |
|
|
| 191 | 190 |
} |
| 192 |
|
|
| 193 |
|
|
Also available in: Unified diff