velocity_commands.cpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Includes
12 *****************************************************************************/
13 
14 #include <iostream>
15 #include <cstdio>
16 #include <cmath>
17 
18 double bias, radius, speed;
19 
20 void velocityCommand(const double& vx, const double &wz) {
21  const double epsilon = 0.0001;
22  if ( std::abs(wz) < epsilon ) {
23  radius = 0;
24  } else if ( (std::abs(vx) < epsilon ) && ( wz > epsilon ) ) {
25  radius = 1;
26  } else if ((std::abs(vx) < epsilon ) && ( wz < -1*epsilon ) ) {
27  radius = -1;
28  } else {
29  radius = (short)(vx * 1000.0f / wz);
30  }
31  if ( vx < 0.0 ) {
32  speed = (short)(1000.0f * std::min(vx + bias * wz / 2.0f, vx - bias * wz / 2.0f));
33  } else {
34  speed = (short)(1000.0f * std::max(vx + bias * wz / 2.0f, vx - bias * wz / 2.0f));
35  }
36 }
37 
38 class Rb2Vw
39 {
40 public:
42  Rb2Vw( const int wheelToWheelInMM = 230 ) : half_wheel_2_wheel(wheelToWheelInMM/2) {}
43 
44  void getVW( const int leftWheelVelocity, const int rightWheelVelocity, int & linearSpeed, int & rotationalSpeedInDegree )
45  {
46  if( std::abs( static_cast<float>(leftWheelVelocity ))
47  > std::abs( static_cast<float>(rightWheelVelocity) ) )
48  linearSpeed = leftWheelVelocity;
49  else
50  linearSpeed = rightWheelVelocity;
51 
52  rotationalSpeedInDegree = ( rightWheelVelocity - leftWheelVelocity ) * 180 / (half_wheel_2_wheel*2) / 3.14152;
53  }
54 
55  void getSR( const int leftWheelVelocity, const int rightWheelVelocity, int & desiredSpeed, int & desiredRadius )
56  {
57  if( leftWheelVelocity == rightWheelVelocity )
58  {
59  desiredSpeed = leftWheelVelocity;
60  desiredRadius = 0;
61  }
62  else if( std::abs(static_cast<float>(leftWheelVelocity)) == std::abs(static_cast<float>(rightWheelVelocity)) )
63  {
64  desiredSpeed = std::abs(static_cast<float>(leftWheelVelocity));
65  if( leftWheelVelocity < rightWheelVelocity ) desiredRadius = +1;
66  else desiredRadius = -1;
67  }
68  else
69  {
70  desiredRadius = -(leftWheelVelocity+rightWheelVelocity)*half_wheel_2_wheel/(leftWheelVelocity-rightWheelVelocity);
71  if( std::abs(static_cast<float>(leftWheelVelocity)) < std::abs(static_cast<float>(rightWheelVelocity)) )
72  {
73  desiredSpeed = rightWheelVelocity;
74  }
75  else
76  {
77  desiredSpeed = leftWheelVelocity;
78  //desiredRadius = -desiredRadius;
79  }
80  }
81  }
82 
83  void getWheelVelocity( const double & linearVelInMeter, const double & rotationalVelInRadian, double & leftWheelVel, double & rightWheelVel )
84  {
85  leftWheelVel = linearVelInMeter - rotationalVelInRadian * static_cast<double>( half_wheel_2_wheel ) * 0.001;
86  rightWheelVel = linearVelInMeter + rotationalVelInRadian * static_cast<double>( half_wheel_2_wheel ) * 0.001;
87  }
88 
89  void update( const int desiredSpeed, const int desiredRadius, int & leftWheelVelocity, int & rightWheelVelocity )
90  {
91  bool change_direction(false);
92 
93  if( desiredSpeed == 0 && desiredRadius == 0 )
94  {
95  leftWheelVelocity = 0;
96  rightWheelVelocity = 0;
97  return;
98  }
99 
100  if( desiredRadius == 0 )
101  {
102  leftWheelVelocity = desiredSpeed;
103  rightWheelVelocity = desiredSpeed;
104  }
105  else if( desiredRadius == -1 || desiredRadius == +1 )
106  {
107  leftWheelVelocity = -desiredRadius * desiredSpeed;
108  rightWheelVelocity = desiredRadius * desiredSpeed;
109  }
110  else
111  {
112  int radius( desiredRadius );
113  if( radius < 0 )
114  {
115  radius = -radius;
116  change_direction = true;
117  }
118 
119  int left( radius - half_wheel_2_wheel );
120  int right( radius + half_wheel_2_wheel );
121 
122  if( change_direction )
123  {
124  leftWheelVelocity = (desiredSpeed*right)/right;
125  rightWheelVelocity = (desiredSpeed*left)/right;
126  }
127  else
128  {
129  leftWheelVelocity = (desiredSpeed*left)/right;
130  rightWheelVelocity = (desiredSpeed*right)/right;
131  }
132  }
133  }
134 };
135 
136 
137 int main(int argc, char **argv) {
138  std::cout << "Hello Dude" << std::endl;
139 
140  bias = 0.23;
141  double vx = -0.10;
142  double wz = 0.15;
143 
144  Rb2Vw fw;
145 // const unsigned int steps = 30;
146 // for (unsigned int i = 0; i < steps; ++i ) {
147 // velocityCommand(vx,wz);
148 // std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
149 // vx += 0.2/static_cast<double>(steps);
150 // }
151 // vx = -0.10;
152 // wz = -0.15;
153 // for ( i = 0; i < steps; ++i ) {
154 // velocityCommand(vx,wz);
155 // std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
156 // vx += (0.2/static_cast<double>(steps));
157 // }
158 
159  std::cout << "Reproduce the issue with slow motion command" << std::endl;
160  vx = -0.03;
161  wz = 0.4;
162  velocityCommand(vx,wz);
163  std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
164 
165  int left_wheel_speed;
166  int right_wheel_speed;
167  fw.update( speed, radius, left_wheel_speed, right_wheel_speed );
168  printf("fw will generate left right wheel speed as [%d mm/sec | %d mm/sec] \n", left_wheel_speed, right_wheel_speed );
169 
170  double left_wheel_speed_in_meter;
171  double right_wheel_speed_in_meter;
172  int desire_speed, desired_radius;
173  fw.getWheelVelocity( vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
174  printf("When you have vx(%f), wz(%f), robot have to generate left right wheel speed as %f, %f \n", vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
175  fw.getSR( left_wheel_speed_in_meter*1000, right_wheel_speed_in_meter*1000, desire_speed, desired_radius );
176  printf("My quick code result for speed and radius are %d, %d \n", desire_speed, desired_radius );
177 
178  fw.update( desire_speed, desired_radius, left_wheel_speed, right_wheel_speed );
179  printf("fw will generate left right wheel speed as [%d mm/sec | %d mm/sec] \n", left_wheel_speed, right_wheel_speed );
180 
181  double err_l, err_r;
182  for( vx=-0.3;vx<0.3; vx+=0.001 )
183  {
184  for( wz=-0.5;wz<0.5; wz += 0.1 )
185  {
186  velocityCommand(vx,wz);
187  fw.update( speed, radius, left_wheel_speed, right_wheel_speed );
188  fw.getWheelVelocity( vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
189 
190  err_l = left_wheel_speed - left_wheel_speed_in_meter*1000.0;
191  err_r = right_wheel_speed - right_wheel_speed_in_meter*1000.0;
192 
193  if( fabs(err_l) > 2.5 || fabs(err_r) > 2.5 )
194  {
195  printf("something wrong [%f,%f] --> [%f,%f] [%d,%d][%f,%f]\n", vx, wz, speed, radius,
196  left_wheel_speed, right_wheel_speed,
197  left_wheel_speed_in_meter*1000.0, right_wheel_speed_in_meter*1000.0 );
198  }
199  }
200  }
201 
202  for( vx=-0.3;vx<0.3; vx+=0.001 )
203  {
204  for( wz=-0.5;wz<0.5; wz += 0.1 )
205  {
206  fw.getWheelVelocity( vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
207  fw.getSR( left_wheel_speed_in_meter*1000, right_wheel_speed_in_meter*1000, desire_speed, desired_radius );
208  fw.update( desire_speed, desired_radius, left_wheel_speed, right_wheel_speed );
209 
210 
211  err_l = left_wheel_speed - left_wheel_speed_in_meter*1000.0;
212  err_r = right_wheel_speed - right_wheel_speed_in_meter*1000.0;
213 
214  if( fabs(err_l) > 2.5 || fabs(err_r) > 2.5 )
215  {
216  printf("something wrong [%f,%f] --> [%f,%f] [%d,%d][%f,%f]\n", vx, wz, speed, radius,
217  left_wheel_speed, right_wheel_speed,
218  left_wheel_speed_in_meter*1000.0, right_wheel_speed_in_meter*1000.0 );
219  }
220  }
221  }
222 
223  return 0;
224 }
225 
226 
227 
229 //** Main
230 //*****************************************************************************/
231 //
232 //int main(int argc, char **argv) {
233 //
234 // std::cout << "Hello Dude" << std::endl;
235 //
236 // bias = 0.23;
237 // double vx = -0.10;
238 // double wz = 0.4;
239 // const unsigned int steps = 20;
240 // for (unsigned int i = 0; i < steps; ++i ) {
241 // velocityCommand(vx,wz);
242 // std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
243 // vx += 0.2/static_cast<double>(steps);
244 // }
245 // vx = -0.10;
246 // wz = 0.4;
247 // for (unsigned int i = 0; i < steps; ++i ) {
248 // velocityCommand(vx,wz);
249 // std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
250 // vx += 0.2/static_cast<double>(steps);
251 // }
252 // return 0;
253 //}
void update(const int desiredSpeed, const int desiredRadius, int &leftWheelVelocity, int &rightWheelVelocity)
void getVW(const int leftWheelVelocity, const int rightWheelVelocity, int &linearSpeed, int &rotationalSpeedInDegree)
double radius
int half_wheel_2_wheel
double speed
void velocityCommand(const double &vx, const double &wz)
void getSR(const int leftWheelVelocity, const int rightWheelVelocity, int &desiredSpeed, int &desiredRadius)
Rb2Vw(const int wheelToWheelInMM=230)
double bias
void getWheelVelocity(const double &linearVelInMeter, const double &rotationalVelInRadian, double &leftWheelVel, double &rightWheelVel)
void f(int i) ecl_debug_throw_decl(StandardException)
int main(int argc, char **argv)


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Fri Sep 18 2020 03:22:02