active_damping_stepped.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00035 #include <acado_toolkit.hpp>
00036 #include <acado_gnuplot.hpp>
00037 
00038 
00039 int main( )
00040 {
00041     USING_NAMESPACE_ACADO
00042 
00043 
00044     // INTRODUCE THE VARIABLES:
00045     // -------------------------
00046         DifferentialState xB; //Body Position
00047         DifferentialState xW; //Wheel Position
00048         DifferentialState vB; //Body Velocity
00049         DifferentialState vW; //Wheel Velocity
00050 
00051         Disturbance R;
00052         Control F;
00053 
00054         double mB = 350.0;
00055         double mW = 50.0;
00056         double kS = 20000.0;
00057         double kT = 200000.0;
00058 
00059 
00060     // DEFINE A DIFFERENTIAL EQUATION:
00061     // -------------------------------
00062     DifferentialEquation f;
00063 
00064         f << dot(xB) == vB;
00065         f << dot(xW) == vW;
00066         f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
00067         f << dot(vW) == (  kS*xB - (kT+kS)*xW + kT*R - F ) / mW;
00068 
00069 
00070     // DEFINE LEAST SQUARE FUNCTION:
00071     // -----------------------------
00072     Function h;
00073 
00074     h << xB;
00075     h << xW;
00076         h << vB;
00077     h << vW;
00078         h << F;
00079 
00080     DMatrix Q = zeros<double>(5,5); // LSQ coefficient matrix
00081         Q(0,0) = 10.0;
00082         Q(1,1) = 10.0;
00083         Q(2,2) = 1.0;
00084         Q(3,3) = 1.0;
00085         Q(4,4) = 1.0e-8;
00086 
00087     DVector r(5); // Reference
00088     r.setAll( 0.0 );
00089 
00090 
00091     // DEFINE AN OPTIMAL CONTROL PROBLEM:
00092     // ----------------------------------
00093     const double t_start = 0.0;
00094     const double t_end   = 1.0;
00095 
00096     OCP ocp( t_start, t_end, 20 );
00097 
00098     ocp.minimizeLSQ( Q, h, r );
00099 
00100         ocp.subjectTo( f );
00101 
00102         ocp.subjectTo( -200.0 <= F <= 200.0 );
00103         ocp.subjectTo( R == 0.0 );
00104 
00105 
00106 
00107     // SETTING UP THE (SIMULATED) PROCESS:
00108     // -----------------------------------
00109         OutputFcn identity;
00110         DynamicSystem dynamicSystem( f,identity );
00111 
00112         Process process( dynamicSystem,INT_RK45 );
00113 
00114         VariablesGrid disturbance; disturbance.read( "road.txt" );
00115         if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
00116                 return EXIT_FAILURE;
00117 
00118     // SETTING UP THE MPC CONTROLLER:
00119     // ------------------------------
00120 
00121         double samplingTime = 0.025;
00122         RealTimeAlgorithm alg( ocp,samplingTime );
00123         alg.set( INTEGRATOR_TYPE, INT_RK78 );
00124         //alg.set( MAX_NUM_ITERATIONS, 2 );
00125         alg.set( USE_IMMEDIATE_FEEDBACK,YES );
00126 
00127         StaticReferenceTrajectory zeroReference;
00128 
00129         Controller controller( alg,zeroReference );
00130 
00131 
00132         double startTime = 0.0;
00133         double endTime   = 2.5;
00134         
00135         DVector x0(4);
00136         x0.setZero();
00137 
00138         //      hand-coding call to 
00139         //      sim.init( x0 )
00140 
00141         DVector uCon;
00142         VariablesGrid ySim;
00143         
00144         if (controller.init( startTime,x0 ) != SUCCESSFUL_RETURN)
00145                 exit( EXIT_FAILURE );
00146         controller.getU( uCon );
00147         
00148         if (process.init( startTime,x0,uCon ) != SUCCESSFUL_RETURN)
00149                 exit( EXIT_FAILURE );
00150         process.getY( ySim );
00151 
00152 
00153         //      hand-coding call to 
00154         //      sim.run( )
00155 
00156         double currentTime = startTime;
00157         int nSteps = 0;
00158 
00159         while ( currentTime <= endTime )
00160         {
00161                 printf( "\n*** Simulation Loop No. %d (starting at time %.3f) ***\n",nSteps,currentTime );
00162 
00163                 double t = acadoGetTime();
00164                 if (controller.feedbackStep( currentTime,ySim.getLastVector() ) != SUCCESSFUL_RETURN)
00165                         exit( EXIT_FAILURE );
00166                 printf( "t = %e\n", acadoGetTime()-t );
00167                 controller.getU( uCon );
00168                 if (controller.preparationStep( ) != SUCCESSFUL_RETURN)
00169                         exit( EXIT_FAILURE );
00170                 
00171                 if (process.step( currentTime,currentTime+samplingTime,uCon ) != SUCCESSFUL_RETURN)
00172                         exit( EXIT_FAILURE );
00173                 process.getY( ySim );
00174                 
00175                 currentTime += samplingTime;
00176                 ++nSteps;
00177         }
00178 
00179 //     // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
00180 //     // ----------------------------------------------------------
00181 //      SimulationEnvironment sim( 0.0,3.0,process,controller );
00182 // 
00183 //      DVector x0(4);
00184 //      x0.setZero();
00185 // 
00186 //      sim.init( x0 );
00187 //      sim.run( );
00188 // 
00189 // 
00190 //     // ...AND PLOT THE RESULTS
00191 //     // ----------------------------------------------------------
00192 //      VariablesGrid diffStates;
00193 //      sim.getProcessDifferentialStates( diffStates );
00194 // 
00195 //      VariablesGrid feedbackControl;
00196 //      sim.getFeedbackControl( feedbackControl );
00197 // 
00198 //      GnuplotWindow window;
00199 //      window.addSubplot( diffStates(0), "Body Position [m]" );
00200 //      window.addSubplot( diffStates(1), "Wheel Position [m]" );
00201 //      window.addSubplot( diffStates(2), "Body Velocity [m/s]" );
00202 //      window.addSubplot( diffStates(3), "Wheel Velocity [m/s]" );
00203 //      window.addSubplot( feedbackControl, "Damping Force [N]" );
00204 //      window.addSubplot( disturbance,     "Road Excitation [m]" );
00205 //      window.plot( );
00206 
00207     return EXIT_SUCCESS;
00208 }
00209 
00210 
00211 


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:41