Project

General

Profile

Revision 1345

Added by Rich Hong almost 10 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

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= (((d3-d1) + (d4-d5))*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 couter-clockwise from front.
124
	// -180 <= omega <= 180
125
	//velocity: -255 <= velocity <= 255
157
  //omega: angle measure, positive couter-clockwise 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