identificationNode.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, LABUST, UNIZG-FER
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the LABUST nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <labust/control/SOIdentification.hpp>
00035 #include <labust/vehicles/vehiclesfwd.hpp>
00036 #include <labust/xml/GyrosReader.hpp>
00037 #include <labust/xml/GyrosWriter.hpp>
00038 #include <labust/tools/TimingTools.hpp>
00039 #include <labust/control/PIDController.hpp>
00040 
00041 #include <std_msgs/String.h>
00042 #include <ros/ros.h>
00043 
00044 #include <map>
00045 
00046 double yaw(0);
00047 double lastTime(labust::tools::unix_time());
00048 
00052 struct TuningParameters
00053 {
00057         double alpha,beta,betaa;
00061         double w;
00065         double max;
00066 };
00067 
00068 template <class PID>
00069 void tuneController(const TuningParameters& param, PID* pid)
00070 {
00071   double a3 = 1/(param.w*param.w*param.w), a2 = 3/(param.w*param.w), a1 = 3/(param.w);
00072 
00073   double K_Ipsi = param.alpha/a3;
00074   double K_Ppsi = param.alpha*a1/a3;
00075   double K_Dpsi = (param.alpha*a2/a3 - param.beta);
00076 
00077   /*K_Ipsi = 1.55139;
00078   K_Ppsi = 6.64881;
00079   K_Dpsi = 8.0083;*/
00080 
00081   //pid->setGains(a1*param.alpha/a3,param.alpha*a3,a2/a3*param.alpha - param.beta,0);
00082   pid->setGains(K_Ppsi,K_Ipsi,K_Dpsi,K_Dpsi/10);
00083 
00084   std::cout<<"Param:"<<param.alpha<<","<<param.beta<<","<<param.w<<std::endl;
00085   std::cout<<"K_Ppsi:"<<K_Ppsi<<",K_Ipsi:"<<K_Ipsi<<",K_Dpsi:"<<K_Dpsi<<std::endl;
00086 
00087   labust::math::Limit<double> limit(-param.max,param.max);
00088   pid->setLimits(limit);
00089 
00090   //pid->setGains(10,0,0,0);
00091 }
00092 
00093 void onState(const std_msgs::String::ConstPtr& msg)
00094 {
00095         labust::xml::GyrosReader reader(msg->data);
00096         labust::vehicles::stateMap state;
00097         reader.dictionary(state);
00098 
00099         yaw = state[labust::vehicles::state::yaw];
00100         lastTime = reader.GetTimeStamp();
00101 
00102         std::cout<<"Received."<<std::endl;
00103 }
00104 
00105 void sendTau(double torque,
00106                 const ros::Publisher& tau,  double lat = 0)
00107 {
00108   //send all zeros at the end.
00109   std::map<int,double> tauM;
00110   for (int i=labust::vehicles::tau::X; i<=labust::vehicles::tau::N; ++i)
00111   {
00112         tauM[i] = 0;
00113   }
00114 
00115   tauM[labust::vehicles::tau::N] = torque;
00116   tauM[labust::vehicles::tau::Y] = lat;
00117   tauM[labust::vehicles::tau::Z] = 0;
00118 
00119   labust::xml::GyrosWriter writer(tauM.begin(),tauM.end());
00120   writer.SetTimeStamp(true);
00121   std_msgs::String str;
00122   str.data = writer.GyrosXML();
00123   tau.publish(str);
00124 }
00125 
00126 int main(int argc, char* argv[])
00127 try
00128 {
00129         ros::init(argc, argv, "vehicleNode");
00130 
00131         ros::NodeHandle n;
00132 
00133 
00134         ros::Publisher tau = n.advertise<std_msgs::String>("tau", 10);
00135         ros::Subscriber state = n.subscribe("state",10,&onState);
00136 
00137   ros::Rate loop_rate(10);
00138   labust::control::SOIdentification ident;
00139 
00140  ident.setRelay(50,15*M_PI/180);
00141   ident.reset();
00142 
00143         
00144  labust::control::IPD ipd;
00145  //ipd.setStructure(0,1,0);
00146 
00147   double yaw_ref = 0;
00148  double ditter = 0*2.5;
00149   double lat = 0*80;
00150 
00151   //sendTau(0,tau);
00152   int i=0;
00153 
00154   while (ros::ok())
00155   {
00156         ++i;
00157         if (!ident.isFinished())
00158         {
00159                 sendTau(ident.step(yaw_ref-yaw,0.1),tau);
00160                 //sendTau(60,tau);
00161 
00162                 std::cout<<yaw<<std::endl;
00163         }
00164         else
00165         {
00166                 labust::control::SOIdentification::ParameterContainer param;
00167                 ident.parameters(&param);
00168 
00169                 std::cout<<"Alpha:"<<param[ident.alpha];
00170                 std::cout<<", beta:"<<param[ident.kx];
00171                 std::cout<<", betaa:"<<param[ident.kxx];
00172                 std::cout<<", delta:"<<param[ident.delta]<<std::endl;
00173 
00174                 TuningParameters tparam;
00175 
00176                 tparam.w = 0.4;
00177                 tparam.alpha = param[ident.alpha];//128;
00178                 tparam.beta = param[ident.kx];//41.8;
00179                 tparam.betaa = param[ident.kxx];
00180                 tparam.max = 80;
00181 
00182                 tuneController(tparam,&ipd);
00183 
00184           //send all zeros at the end.
00185                 //sendTau(0,tau);
00186                 //Do control.
00187                 if ((i%100) == 0) lat = -lat;
00188           sendTau(ipd.step(yaw_ref, yaw) + ditter,tau, lat);
00189         }
00190 
00191     ros::spinOnce();
00192     //std::cout<<"Timing:"<<labust::tools::unix_time() - lastTime<<std::endl;
00193     loop_rate.sleep();
00194 
00195   }
00196 
00197   //send all zeros at the end.
00198   sendTau(0,tau);
00199 
00200         return 0;
00201 }
00202 catch (std::exception& e)
00203 {
00204         std::cerr<<e.what()<<std::endl;
00205 }
00206 
00207 
00208 


labust_uvapp
Author(s): Dula Nad
autogenerated on Sun Mar 1 2015 12:12:22