model_brake.h
Go to the documentation of this file.00001 /* -*- mode: C++ -*- 00002 * 00003 * Copyright (C) 2005, 2007, 2009 Austin Robot Technology 00004 * 00005 * License: Modified BSD Software License Agreement 00006 * 00007 * $Id: model_brake.h 2 2010-01-17 01:54:03Z jack.oquin $ 00008 */ 00009 00018 #ifndef _MODEL_BRAKE_H_ 00019 #define _MODEL_BRAKE_H_ 1 00020 00021 // ROS interfaces 00022 #include <ros/ros.h> 00023 00024 #include "animatics.h" 00025 00031 class ActuatorPlan 00032 { 00033 public: 00034 00036 ActuatorPlan() {reset();} 00037 00039 void append(double t, double a) 00040 { 00041 if (nsteps_ < MAX_STEPS) 00042 { 00043 steps_[nsteps_].until = t; 00044 steps_[nsteps_].accel = a; 00045 ++nsteps_; 00046 } 00047 } 00048 00050 bool finished() {return nsteps_ == 0;} 00051 00056 double interval(double t, double finish) 00057 { 00058 // (1) interval to finish 00059 double dt = finish - t; 00060 if (nsteps_ > 0) 00061 // (2) use interval in step (if smaller) 00062 dt = std::min(dt, steps_[0].until-t); 00063 return dt; 00064 } 00065 00067 void log(void) 00068 { 00069 if (nsteps_ > 0) 00070 ROS_DEBUG("next step: accelerate %.f until %.6f", 00071 steps_[0].accel, steps_[0].until); 00072 } 00073 00078 double next(double now) 00079 { 00080 // advance to next plan step, if time 00081 while ((nsteps_ > 0) && (steps_[0].until <= now)) 00082 { 00083 steps_[0] = steps_[1]; // remove the first step 00084 --nsteps_; 00085 } 00086 00087 // return acceleration to use 00088 if (nsteps_ > 0) 00089 return steps_[0].accel; 00090 else 00091 return 0.0; 00092 } 00093 00095 void reset(void) {nsteps_ = 0;} 00096 00097 private: 00098 int nsteps_; 00099 static const int MAX_STEPS = 2; 00100 00101 struct { 00102 double accel; 00103 double until; 00104 } steps_[MAX_STEPS]; 00105 }; 00106 00107 class ArtBrakeModel 00108 { 00109 public: 00110 00111 ArtBrakeModel(float init_pos); 00112 ~ArtBrakeModel(); 00113 00115 int interpret(const char *string, char *status, int nbytes); 00116 00117 private: 00118 00119 // ROS interfaces 00120 ros::NodeHandle node_; // simulation node handle 00121 00122 double last_update_time_; 00123 00124 Animatics *am_; // Animatics Smart Motor data 00125 00126 float brake_position_; // simulated brake position 00127 int sim_status_; // simulated status word 00128 int sim_encoder_; // simulated encoder value 00129 int encoder_goal_; // encoder set point 00130 int digital_pressure_; // simulated pressure A/D value 00131 int digital_potentiometer_;// simulated pot A/D value 00132 00133 // configured actuator parameters 00134 double actuator_accel_; // configured acceleration (ticks/s^2) 00135 double actuator_max_vel_; // configured maximum velocity (ticks/s) 00136 00137 // actuator state variables 00138 double x_; // current actuator position (ticks) 00139 double v_; // current actuator velocity (ticks/s) 00140 double a_; // current actuator acceleration (ticks/s^2) 00141 00142 ActuatorPlan plan_; // actuator movement plan 00143 00144 00146 inline double getAccel(double direction) 00147 { 00148 return (direction == 0.0? 0.0 : 00149 (direction > 0.0? actuator_accel_ : -actuator_accel_)); 00150 } 00151 00152 void move(double start, double finish); 00153 double plan(double dx); 00154 void update(void); 00155 void update_sensors(float position); 00156 00157 }; 00158 00159 #endif // _MODEL_BRAKE_H_