Controlling Arduino+PWM_Controlled_Cylinder with Action Client

edit

Hi all,

the goal is to control a cylinder which is connected to the Arduino and takes as input PWM values and outputs the encoder position using the Actionlib. To read out the encoder values I also implemented hardware_interface which saves the encoder position under the topic /joint_states. In my_server.cpp I subscribe to these values with :

sub_ = nh_.subscribe("/joint_states", 1, &myAction::analysisCB, this);

The corresponding callback function:

void analysisCB(const sensor_msgs::JointState::ConstPtr&  msg)
  {
 // make sure that the action hasn't been canceled
if (!as_.isActive())
{
  ROS_INFO_STREAM("action_server status " << as_.isActive() <<"."); 
  return;
}

ROS_INFO("In da AnalysisCB");

for(int id = 0; id <5; id++){ // Fulfill the arrays whith motor readings
  pos[id] = 0;
} 

pos[0] = msg->position[0];
pos[1] = msg->position[1];
pos[2] = msg->position[2];
pos[3] = msg->position[3];
pos[4] = msg->position[4];

for(int id = 0; id <5; id++){ // Fulfill the arrays whith motor readings
  ROS_INFO_STREAM(" current pos 1  " << pos[id] <<".");
} 

if ( goal_.points.size() > 0)
{
feedback_.diff.clear();
feedback_.diff.push_back(goal_.points[interpolation_counter_].positions[0]- pos[0]) ;
feedback_.diff.push_back(goal_.points[interpolation_counter_].positions[1]- pos[1]) ;
feedback_.diff.push_back(goal_.points[interpolation_counter_].positions[2]- pos[2]) ;
feedback_.diff.push_back(goal_.points[interpolation_counter_].positions[3]- pos[3]) ;
feedback_.diff.push_back(goal_.points[interpolation_counter_].positions[4]- pos[4]) ;


as_.publishFeedback(feedback_);
ROS_INFO("Feedback published back");
auto feedback_norm =0.0f;
for (int i=0; i < 5; i++)
{
    feedback_norm = sqrt(feedback_.diff[i] * feedback_.diff[i]) + feedback_norm;
}

if( feedback_norm< 0.1) 
{

  ROS_INFO("%s: Succeeded", action_name_.c_str());

  if (interpolation_counter_ < goal_.points.size())
  {
    interpolation_counter_++;
  }

  else{
        // set the action state to succeeded
         as_.setSucceeded(result_);
  }
}

else{
  ROS_INFO("%s: Executing", action_name_.c_str());
}}}

The callback function for goal:

void goalCB()
  {
    // reset helper variables

    sum_ = 0;
    sum_sq_ = 0;
    // accept the new goal
    goal_ = as_.acceptNewGoal()->des_trajectory;
    ROS_INFO("goal_arrived, size: %d", goal_.points.size() );
  }

The action file:

#goal         
trajectory_msgs/JointTrajectory des_trajectory
---
#result
trajectory_msgs/JointTrajectory real_trajectory
---
#feedback
float32[] diff

My next move would be to convert the feedback into PWM values and publish to the arduino to control the cylinders according to the difference values. However, to realize this, I need another controller of type PID/ PD to efficiently move the cylinder. feedback: diff: [6.322370040834357e-07, 0.07905552536249161, -0.08287492394447327, -0.07032601535320282, 0.07414892315864563]

This sounds not correct, since I already have PID controller configured in my trajectory_control.yaml. Am I on the right track or did I miss something?

my_arm:
    joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 50
    my_arm_controller:
        type: "position_controllers/JointTrajectoryController"
        joints:
            - my_jnt0
            - my_jnt1
            - my_jnt2 
            - my_jnt3
            - my_jnt4
        gains:
            my_jnt0: {p: 1000.0, i: 0.0, d: 0.1}
            my_jnt1: {p: 1000.0, i: 0.0, d: 0.1}
            my_jnt2: {p: 1000.0, i: 0.0, d: 0.1}
            my_jnt3: {p: 1000.0, i: 0.0, d: 0.1}
            my_jnt4: {p: 1000.0, i: 0.0, d: 0.1}