plant_sim.cpp

Go to the documentation of this file.

/***************************************************************************/
// This file simulates a 1st or 2nd-order dynamic system, publishes its state,
// and subscribes to a 'control_effort' topic. The control effort is used
// to stabilize the servo.

#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace plant_sim
{
  // Global so it can be passed from the callback fxn to main
  static double control_effort = 0.0;
  static bool reverse_acting = false;
}
using namespace plant_sim;

// Callback when something is published on 'control_effort'
void ControlEffortCallback(const std_msgs::Float64& control_effort_input)
{
  // the stabilizing control effort
  if (reverse_acting)
  {
    control_effort = -control_effort_input.data;
  }
  else
  {
    control_effort = control_effort_input.data;
  }
}

int main(int argc, char **argv)
{
  int plant_order = 1;
  double temp = 4.7;          // initial condition for first-order plant
  double displacement = 3.3;  // initial condition for second-order plant

  ros::init(argc, argv, "plant");
  ros::NodeHandle sim_node;

  while (ros::Time(0) == ros::Time::now())
  {
    ROS_INFO("Plant_sim spinning waiting for time to become non-zero");
    sleep(1);
  }

  ros::NodeHandle node_priv("~");
  node_priv.param<int>("plant_order", plant_order, 1);
  node_priv.param<bool>("reverse_acting", reverse_acting, false);

  if (plant_order == 1)
  {
    ROS_INFO("Starting simulation of a first-order plant.");
  }
  else if (plant_order == 2)
  {
    ROS_INFO("Starting simulation of a second-order plant.");
  }
  else
  {
    ROS_ERROR("Error: Invalid plant type parameter, must be 1 or 2: %s", argv[1]);
    return -1;
  }

  // Advertise a plant state msg
  std_msgs::Float64 plant_state;
  ros::Publisher servo_state_pub = sim_node.advertise<std_msgs::Float64>("state", 1);

  // Subscribe to "control_effort" topic to get a controller_msg.msg
  ros::Subscriber sub = sim_node.subscribe("control_effort", 1, ControlEffortCallback );
  
  int loop_counter = 0;
  double delta_t = 0.01;
  ros::Rate loop_rate(1/delta_t); // Control rate in Hz

  // Initialize 1st-order (e.g temp controller) process variables
  double temp_rate = 0;     // rate of temp change
  
  // Initialize 2nd-order (e.g. servo-motor with load) process variables
  double speed = 0;         // meters/sec
  double acceleration = 0;  // meters/sec^2
  double mass = 0.1;          // in kg
  double friction = 1.0;    // a decelerating force factor
  double stiction = 1;      // control_effort must exceed this before stationary servo moves
  double Kv = 1;            // motor constant: force (newtons) / volt
  double Kbackemf = 0;      // Volts of back-emf per meter/sec of speed
  double decel_force;       // decelerating force

  while (ros::ok())
  {
    ros::spinOnce();

    switch (plant_order)
    {
    case 1:       // First order plant
      temp_rate = (0.1 * temp) + control_effort;
      temp = temp + temp_rate * delta_t;

      plant_state.data = temp;
      break;

    case 2:       // Second order plant
      if (fabs(speed) < 0.001)
      {
        // if nearly stopped, stop it & require overcoming stiction to restart
        speed = 0;
        if (fabs(control_effort) < stiction)
        {
          control_effort = 0;
        }
      }

      // Update the servo.
      // control_effort: the voltage applied to the servo. Output from PID controller. It is
      //   opposed by back emf (expressed as speed) to produce a net force. Range: -1 to +1
      // displacement: the actual value of the servo output position. Input to PID controller

      decel_force = -(speed * friction);  // can be +ve or -ve. Linear with speed
      acceleration = ((Kv * (control_effort - (Kbackemf * speed)) + decel_force) / mass);   // a = F/m
      speed = speed + (acceleration * delta_t);
      displacement = displacement + speed * delta_t;

      plant_state.data = displacement;
      break;
    
    default:
      ROS_ERROR("Invalid plant_order: %d", plant_order);
      return(-1);
    }

    servo_state_pub.publish(plant_state);
    loop_rate.sleep();
  }

  return 0;
}