00001 /* -*- mode: C++ -*- 00002 * 00003 * Description: common steering constants and calculations 00004 * 00005 * Copyright (C) 2005, 2007, 2009 Austin Robot Technology 00006 * License: Modified BSD Software License Agreement 00007 * 00008 * $Id: steering.h 630 2010-09-25 16:20:42Z jack.oquin $ 00009 */ 00010 00011 #ifndef _STEERING_H 00012 #define _STEERING_H 00013 00014 #include <angles/angles.h> 00015 #include <art_msgs/ArtVehicle.h> 00016 00022 namespace Steering 00023 { 00024 00026 const float steer_speed_min = 3.8; /* minimum speed for calculation */ 00027 /* (experimentally verified) */ 00028 00053 static inline float steering_angle(float v, float y) 00054 { 00055 float steer_radians = atan2f(art_msgs::ArtVehicle::wheelbase * y,v); 00056 float steer_degrees = angles::to_degrees(steer_radians); 00057 00058 steer_degrees = fminf(steer_degrees, 00059 art_msgs::ArtVehicle::max_steer_degrees); 00060 steer_degrees = fmaxf(steer_degrees, 00061 -art_msgs::ArtVehicle::max_steer_degrees); 00062 00063 return steer_degrees; 00064 } 00065 00069 static inline double angle_to_yaw(double v, float angle) 00070 { 00071 return v * (tanf(angles::from_degrees(angle)) 00072 / art_msgs::ArtVehicle::wheelbase); 00073 } 00074 00075 const double maximum_yaw = 00076 angle_to_yaw(steer_speed_min, art_msgs::ArtVehicle::max_steer_degrees); 00077 } 00078 00079 #endif // _STEERING_H