YANE-Framework Tutorial 1.1.0

examples/mpctcp/src/controllermain.cpp

00001 /***************************************************************************
00002  *                                                                         *
00003  * Copyright (C) 2011 by www.nonlinearmpc.com                              *
00004  *                                                                         *
00005  * Authors:                                                                *
00006  *  Thomas Jahn <thomas.jahn@uni-bayreuth.de>                              *
00007  *  Juergen Pannek <juergen.pannek@googlemail.com>                         *
00008  *                                                                         *
00009  ***************************************************************************/
00010 
00011 #include <yane.h>
00012 #include <sqpfortran.h>
00013 #include <examplemodels2.h>
00014 
00015 #include <iostream>
00016 #include <csignal>
00017 #include <string>
00018 #include <sstream>
00019 #include <fstream>
00020 #include <cstdlib>
00021 
00022 #define HNEW 0.1
00023 #define HORIZON 17
00024 #define PREDICTION 6
00025 
00026 using namespace std;
00027 
00028 bool abort_mpc = false;
00029 
00030 void abortsig ( int signalnummer )
00031 {
00032         abort_mpc = true;
00033 }
00034 
00035 int main ( int argc, char *argv [ ] )
00036 {
00037         signal ( SIGINT, abortsig );
00038 
00039         stringstream basename;
00040         basename << "protocol_controller_";
00041 
00042         time_t currenttime = time ( 0 );
00043         char tbuf [ 256 ];
00044         strftime ( tbuf, 256, "%Y%m%d_%H%M%S", localtime ( & currenttime ) );
00045 
00046         basename << tbuf << "_horiz" << HORIZON << "_predict" << PREDICTION;
00047 
00048         string debugfilename = basename.str ( ) + ".debug.txt";
00049         string statfilename = basename.str ( ) + ".stat.txt";
00050 
00051         yane::Utils::Exception::enableDebugMessage ( true );
00052 
00053         yane::Model::Model * model = new yane::ExampleModels2::InvertedPendulum ( );
00054         yane::ExampleModels2::InvertedPendulumShootingData * shooting =
00055                 new yane::ExampleModels2::InvertedPendulumShootingData ( model );
00056 
00057         yane::Utils::RTClock * clock = new yane::Utils::RTClock ( );
00058         yane::Utils::DebugMasterFile * dm = new yane::Utils::DebugMasterFile (
00059                 clock, debugfilename.c_str ( ), 120 );
00060         yane::Utils::StatisticsMaster * sm = new yane::Utils::StatisticsMaster ( );
00061 
00062         yane::SqpFortran::SqpFortran * minimizer =
00063                 new yane::SqpFortran::SqpFortran ( );
00064         yane::MPC::CacheOdeManager * odemanager = new yane::MPC::CacheOdeManager ( );
00065 
00066         dm->setLevel ( 10 );
00067 
00068         yane::NetworkedControl::IOInterfaceTCP * interface =
00069                 new yane::NetworkedControl::IOInterfaceTCP ( HORIZON, dm, sm );
00070         cout << "Connecting to model...";
00071         cout.flush ( );
00072         interface->connectToModel ( model, "localhost" );
00073         cout << "done" << endl;
00074 
00075         yane::NetworkedControl::Controller
00076                 * ctrl = new yane::NetworkedControl::Controller ( model, shooting,
00077                         HORIZON, PREDICTION, minimizer, odemanager, clock,
00078                         interface, dm, sm );
00079 
00080         minimizer->setAccuracy ( 1E-3 );
00081 
00082         if ( interface->isConnected ( ) )
00083         {
00084                 cout << "Starting controller...";
00085                 cout.flush ( );
00086                 ctrl->start ( );
00087                 cout << "done" << endl;
00088 
00089                 while ( ! abort_mpc && interface->isConnected ( ) )
00090                 {
00091                         usleep ( 100000 );
00092                 }
00093                 string stat = sm->evaluateStatistics ( );
00094 
00095                 cout << "Stopping controller...";
00096                 cout.flush ( );
00097                 ctrl->stop ( );
00098                 cout << "done" << endl;
00099                 interface->closeConnection ( );
00100 
00101                 ofstream statfile ( statfilename.c_str ( ) );
00102                 statfile << stat;
00103                 statfile.close ( );
00104 
00105                 cout << stat << endl;
00106         }
00107         else
00108         {
00109                 cout << "could not connect to model... check tcp-network and modelid."
00110                         << endl;
00111         }
00112 
00113         delete model;
00114         delete dm;
00115         delete clock;
00116 }