plant_sim.cpp
Go to the documentation of this file.
1 /***************************************************************************/
37 // This file simulates a 1st or 2nd-order dynamic system, publishes its state,
38 // and subscribes to a 'control_effort' topic. The control effort is used
39 // to stabilize the servo.
40 
41 #include <ros/ros.h>
42 #include <std_msgs/Float64.h>
43 
44 namespace plant_sim
45 {
46 // Global so it can be passed from the callback fxn to main
47 static double control_effort = 0.0;
48 static bool reverse_acting = false;
49 }
50 using namespace plant_sim;
51 
52 // Callback when something is published on 'control_effort'
53 void controlEffortCallback(const std_msgs::Float64& control_effort_input)
54 {
55  // the stabilizing control effort
56  if (reverse_acting)
57  {
58  control_effort = -control_effort_input.data;
59  }
60  else
61  {
62  control_effort = control_effort_input.data;
63  }
64 }
65 
66 int main(int argc, char** argv)
67 {
68  int plant_order = 1;
69  double temp = 4.7; // initial condition for first-order plant
70  double displacement = 3.3; // initial condition for second-order plant
71 
72  ros::init(argc, argv, "plant");
73  ros::NodeHandle sim_node;
74 
75  while (ros::ok() && ros::Time(0) == ros::Time::now())
76  {
77  ROS_INFO("Plant_sim spinning waiting for time to become non-zero");
78  sleep(1);
79  }
80 
81  ros::NodeHandle node_priv("~");
82  node_priv.param<int>("plant_order", plant_order, 1);
83  node_priv.param<bool>("reverse_acting", reverse_acting, false);
84 
85  if (plant_order == 1)
86  {
87  ROS_INFO("Starting simulation of a first-order plant.");
88  }
89  else if (plant_order == 2)
90  {
91  ROS_INFO("Starting simulation of a second-order plant.");
92  }
93  else
94  {
95  ROS_ERROR("Error: Invalid plant type parameter, must be 1 or 2: %s", argv[1]);
96  return -1;
97  }
98 
99  // Advertise a plant state msg
100  std_msgs::Float64 plant_state;
101  ros::Publisher servo_state_pub = sim_node.advertise<std_msgs::Float64>("state", 1);
102 
103  // Subscribe to "control_effort" topic to get a controller_msg.msg
104  ros::Subscriber sub = sim_node.subscribe("control_effort", 1, controlEffortCallback);
105 
106  int loop_counter = 0;
107  double delta_t = 0.01;
108  ros::Rate loop_rate(1 / delta_t); // Control rate in Hz
109 
110  // Initialize 1st-order (e.g temp controller) process variables
111  double temp_rate = 0; // rate of temp change
112 
113  // Initialize 2nd-order (e.g. servo-motor with load) process variables
114  double speed = 0; // meters/sec
115  double acceleration = 0; // meters/sec^2
116  double mass = 0.1; // in kg
117  double friction = 1.0; // a decelerating force factor
118  double stiction = 1; // control_effort must exceed this before stationary servo moves
119  double Kv = 1; // motor constant: force (newtons) / volt
120  double Kbackemf = 0; // Volts of back-emf per meter/sec of speed
121  double decel_force; // decelerating force
122 
123  while (ros::ok())
124  {
125  ros::spinOnce();
126 
127  switch (plant_order)
128  {
129  case 1: // First order plant
130  temp_rate = (0.1 * temp) + control_effort;
131  temp = temp + temp_rate * delta_t;
132 
133  plant_state.data = temp;
134  break;
135 
136  case 2: // Second order plant
137  if (fabs(speed) < 0.001)
138  {
139  // if nearly stopped, stop it & require overcoming stiction to restart
140  speed = 0;
141  if (fabs(control_effort) < stiction)
142  {
143  control_effort = 0;
144  }
145  }
146 
147  // Update the servo.
148  // control_effort: the voltage applied to the servo. Output from PID
149  // controller. It is
150  // opposed by back emf (expressed as speed) to produce a net force.
151  // Range: -1 to +1
152  // displacement: the actual value of the servo output position. Input to
153  // PID controller
154 
155  decel_force = -(speed * friction); // can be +ve or -ve. Linear with speed
156  acceleration = ((Kv * (control_effort - (Kbackemf * speed)) + decel_force) / mass); // a = F/m
157  speed = speed + (acceleration * delta_t);
158  displacement = displacement + speed * delta_t;
159 
160  plant_state.data = displacement;
161  break;
162 
163  default:
164  ROS_ERROR("Invalid plant_order: %d", plant_order);
165  return (-1);
166  }
167 
168  servo_state_pub.publish(plant_state);
169  loop_rate.sleep();
170  }
171 
172  return 0;
173 }
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
Definition: plant_sim.cpp:66
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void controlEffortCallback(const std_msgs::Float64 &control_effort_input)
Definition: plant_sim.cpp:53
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
static bool reverse_acting
Definition: plant_sim.cpp:48
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
static double control_effort
Definition: plant_sim.cpp:47
static Time now()
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)


pid
Author(s): Andy Zelenak , Paul Bouchier
autogenerated on Mon Apr 15 2019 02:41:32