本文整理汇总了C++中PID::getCommand方法的典型用法代码示例。如果您正苦于以下问题：C++ PID::getCommand方法的具体用法？C++ PID::getCommand怎么用？C++ PID::getCommand使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类PID的用法示例。
在下文中一共展示了PID::getCommand方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: nav_callback
void nav_callback(const projeto::QuadStatus status)
{
struct timeval stop, start;
gettimeofday(&start, NULL);
//do stuff
double timestamp = status.header.stamp.toSec();
theta = Vector3d(status.theta.x, status.theta.y, status.theta.z);
Vector3d x_new(status.position.x, status.position.y, status.position.z);
Vector3d vel(status.vel.x, status.vel.y, status.vel.z);
//theta(0) = degree_to_rad(msg_in.rotX);
//theta(1) = degree_to_rad(msg_in.rotY);
//theta(2) = degree_to_rad(msg_in.rotZ);
//ROS_INFO("I heard ax: [%f] ay: [%f] az: [%f]", vx_, vy_, vz_);
//Vector3d velV (vx_, vy_, vz_);
//Quaternion<double> rotQ = rotation(theta);
//Matrix3d R = rotQ.matrix();
//Vector3d vel = R * velV;
//pthread_mutex_lock(&mutex_1);
//float dt = timestamp - previous_tm; //geting dt in secs
if (x_new(0) < -10 || x_new(0) > 10 || x_new(1) < -10 || x_new(1) > 10) {
f_vector_print("theta", theta);
f_vector_print("x", x);
f_vector_print("x_new", x_new);
return;
}
x = x_new;
previous_tm = timestamp;
// pthread_mutex_unlock(&mutex_1);
Vector3d future_position;
vector<Vector3d> trajectory = predict_trajectory2(vel, x, timestamp, timestamp + TIME_AHEAD, TRAJECTORY_DT, future_position);
float short_dist = MAX_DIST;
nav_msgs::GridCells gcells;
vector<geometry_msgs::Point> obstaclerepo;
if (!production_mode) {
sensor_msgs::PointCloud pc;
pc.header.frame_id = "/nav";
pc.header.stamp = ros::Time();
pc.channels.resize(1);
pc.channels[0].name="trajectory";
pc.channels[0].values.resize(trajectory.size());
pc.points.resize(trajectory.size());
int i = 0;
for (vector<Vector3d>::iterator it=trajectory.begin(); it!=trajectory.end(); ++it) {
Vector3d pos = *it;
pc.channels[0].values[i] = 0;
pc.points[i].x = pos(0);
pc.points[i].y = pos(1);
pc.points[i].z = pos(2);
i++;
}
pub_pc2.publish(pc);
gcells.header.frame_id = "/nav";
gcells.header.stamp = ros::Time();
gcells.cell_width = OCTREE_RESOLUTION;
gcells.cell_height = OCTREE_RESOLUTION;
}
if (control_mode) {
// cout << "TIMESTAMP " << timestamp << endl;
// cout << "CONTADOR " << contador << endl;
// cout << "LIMIT " << CONTROL_LIMIT << endl;
if (timestamp < contador + CONTROL_LIMIT) {
double u_x = pid_x.getCommand(pos_obj(0) - x(0), timestamp);
double u_y = pid_y.getCommand(pos_obj(1) - x(1), timestamp);
double u_z = pid_z.getCommand(pos_obj(2) - x(2), timestamp);
double u_yaw = pid_yaw.getCommand(yaw_obj - theta(2), timestamp);
//.........这里部分代码省略.........