Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <labust/control/HLControl.hpp>
00038 #include <labust/control/EnablePolicy.hpp>
00039 #include <labust/control/WindupPolicy.hpp>
00040 #include <labust/control/PIFFController.h>
00041 #include <labust/control/IPFFController.h>
00042 #include <labust/math/NumberManipulation.hpp>
00043 #include <labust/tools/MatrixLoader.hpp>
00044 #include <labust/tools/conversions.hpp>
00045
00046 #include <Eigen/Dense>
00047 #include <auv_msgs/BodyForceReq.h>
00048 #include <ros/ros.h>
00049
00050 namespace labust
00051 {
00052 namespace control{
00054 struct HDGControl : DisableAxis
00055 {
00056 enum {x=0,y};
00057
00058 HDGControl():Ts(0.1),useIP(false){};
00059
00060 void init()
00061 {
00062 ros::NodeHandle nh;
00063 initialize_controller();
00064 }
00065
00066 void windup(const auv_msgs::BodyForceReq& tauAch)
00067 {
00068
00069 con.windup = tauAch.disable_axis.yaw;
00070 };
00071
00072 void reset(const auv_msgs::NavSts& ref, const auv_msgs::NavSts& state)
00073 {
00074 con.internalState = 0;
00075 con.lastState = useIP?unwrap(state.orientation.yaw):state.orientation.yaw;
00076 };
00077
00078 auv_msgs::BodyVelocityReqPtr step(const auv_msgs::NavSts& ref,
00079 const auv_msgs::NavSts& state)
00080 {
00081 con.desired = ref.orientation.yaw;
00082 con.state = (useIP?unwrap(state.orientation.yaw):state.orientation.yaw);
00083
00084 float errorWrap = labust::math::wrapRad(
00085 con.desired - con.state);
00086
00087 if (useIP)
00088 {
00089 IPFF_wffStep(&con,Ts, errorWrap, 0);
00090 }
00091 else
00092 {
00093 PIFF_wffStep(&con,Ts, errorWrap, 0);
00094 }
00095
00096 auv_msgs::BodyVelocityReqPtr nu(new auv_msgs::BodyVelocityReq());
00097 nu->header.stamp = ros::Time::now();
00098 nu->goal.requester = "hdg_controller";
00099 labust::tools::vectorToDisableAxis(disable_axis, nu->disable_axis);
00100
00101 nu->twist.angular.z = con.output;
00102
00103 return nu;
00104 }
00105
00106 void initialize_controller()
00107 {
00108 ROS_INFO("Initializing heading controller...");
00109
00110 ros::NodeHandle nh;
00111 double closedLoopFreq(1);
00112 nh.param("hdg_controller/closed_loop_freq", closedLoopFreq, closedLoopFreq);
00113 nh.param("hdg_controller/sampling",Ts,Ts);
00114 nh.param("hdg_controller/use_ip",useIP,useIP);
00115
00116 disable_axis[5] = 0;
00117
00118 PIDBase_init(&con);
00119 PIFF_tune(&con, float(closedLoopFreq));
00120
00121 ROS_INFO("Heading controller initialized.");
00122 }
00123
00124 private:
00125 ros::Subscriber alt_sub;
00126 PIDBase con;
00127 double Ts;
00128 bool useIP;
00129 labust::math::unwrap unwrap;
00130 };
00131 }}
00132
00133 int main(int argc, char* argv[])
00134 {
00135 ros::init(argc,argv,"hdg_control");
00136
00137 labust::control::HLControl<labust::control::HDGControl,
00138 labust::control::EnableServicePolicy,
00139 labust::control::WindupPolicy<auv_msgs::BodyForceReq> > controller;
00140 ros::spin();
00141
00142 return 0;
00143 }
00144
00145
00146