YANE-Framework Tutorial 1.1.0

examples/mpctcp/src/simplesimulator.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 "simplesimulator.h"
00012 
00013 #include <yane.h>
00014 #include <examplemodels2.h>
00015 
00016 #include <cstring>
00017 #include <unistd.h>
00018 #include <iostream>
00019 #include <iomanip>
00020 
00021 using namespace std;
00022 
00023 SimpleSimulator::SimpleSimulator ( yane::NetworkedControl::Cycle * cycle,
00024         yane::ExampleModels2::InvertedPendulum * model,
00025         yane::Utils::DebugMaster * dm ) :
00026         yane::Utils::Thread ( "SimpleSimulator", dm )
00027 {
00028         _cycle = cycle;
00029         oldt = cycle->elapsedSeconds ( );
00030         _simulator = new yane::Model::Simulator ( model );
00031         _handler = 0;
00032         x = new double [ 4 ];
00033         x [ 0 ] = 0.0;
00034         x [ 1 ] = 0.0;
00035         x [ 2 ] = 0.0;
00036         x [ 3 ] = 0.0;
00037 
00038         u = 0.0;
00039 
00040         cout << "  Time          u        phi      omega          x          v "
00041                 << endl;
00042         cout << "=============================================================="
00043                 << endl;
00044 }
00045 
00046 SimpleSimulator::~SimpleSimulator ( )
00047 {
00048         delete _simulator;
00049         delete [ ] x;
00050 }
00051 
00052 void SimpleSimulator::run ( )
00053 {
00054         cout << "+++++++++++++++++++++ simulation started +++++++++++++++++++++"
00055                 << endl;
00056         int oldcycle = - 1;
00057         oldt = _cycle->elapsedSeconds ( );
00058         double t = oldt;
00059 
00060         yane::Utils::RTClock sensorclock;
00061 
00062         while ( ! _abort )
00063         {
00064                 lock ( );
00065                 t = _cycle->elapsedSeconds ( );
00066                 int newcycle = ( int ) ( t / _cycle->discrTime ( ) );
00067                 if ( oldcycle != newcycle )
00068                 {
00069                         if ( _handler )
00070                         {
00071                                 _handler->getCurrentControl ( & u );
00072                                 //                              _handler->sendSensorValues(oldt, x);
00073                         }
00074                         oldcycle = newcycle;
00075                         cout << fixed << setprecision ( 3 ) << setw ( 6 ) << t << " "
00076                                 << setprecision ( 5 ) << setw ( 10 ) << u << " ";
00077                         for ( int i = 0; i < 4; i++ )
00078                         {
00079                                 cout << setprecision ( 5 ) << setw ( 10 ) << x [ i ] << " ";
00080                         }
00081                         cout << endl;
00082                         /*                      if (newcycle%10==0)
00083                          {*/
00084                         /*                              if (_handler)
00085                          {
00086                          _handler->sendSensorValues(oldt, x);
00087                          }*/
00088                         /*                      }*/
00089                 }
00090 
00091                 if ( _handler && sensorclock.elapsedSeconds ( ) > 0.03 )
00092                 {
00093                         _handler->sendSensorValues ( oldt, x );
00094                         sensorclock.reset ( );
00095                 }
00096 
00097                 _simulator->initialize ( oldt, x, & u );
00098                 _simulator->run ( t );
00099                 oldt = t;
00100                 unlock ( );
00101 
00102                 _cycle->waitUntil ( _cycle->elapsedSeconds ( ) + 0.001 );
00103         }
00104         cout << "+++++++++++++++++++++ simulation stopped +++++++++++++++++++++"
00105                 << endl;
00106 }
00107 
00108 void SimpleSimulator::readState ( double & t, double * state )
00109 {
00110         lock ( );
00111         t = oldt;
00112         memcpy ( state, x, sizeof(double) * 4 );
00113         unlock ( );
00114 }
00115 
00116 void SimpleSimulator::setModelHandler (
00117         yane::NetworkedControl::ModelHandlerTCP * handler )
00118 {
00119         _handler = handler;
00120 }
00121 
00122 bool SimpleSimulator::isOptimal ( )
00123 {
00124         lock ( );
00125         double si = cos ( 0.5 * x [ 0 ] );
00126         double result = si * si + x [ 1 ] * x [ 1 ] + 30.0 * x [ 2 ] * x [ 2 ]
00127                 + 10.0 * x [ 3 ] * x [ 3 ];
00128         unlock ( );
00129 
00130         return result < 1E-3;
00131 }