Controlling Arduino+PWM_Controlled_Cylinder with Action Client
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}