1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 #include "FirstOrderLinearTIDS.hpp"
20 #include "SiconosAlgebraProd.hpp"
21 #include "TimeStepping.hpp"
22 #include "Relay.hpp"
23 #include "EventsManager.hpp"
24
25 #include "LinearSMC.hpp"
26
27 #include "SiconosVector.hpp"
28 #include "ControlSensor.hpp"
29 #include "ZeroOrderHoldOSI.hpp"
30 #include "TimeDiscretisation.hpp"
31 #include "ActuatorFactory.hpp"
32
33 //#define DEBUG_WHERE_MESSAGES
34 // #define DEBUG_NOCOLOR
35 // #define DEBUG_STDOUT
36 // #define DEBUG_MESSAGES
37 #include "siconos_debug.h"
38
39
40
LinearSMC(SP::ControlSensor sensor,unsigned int type)41 LinearSMC::LinearSMC(SP::ControlSensor sensor, unsigned int type):
42 CommonSMC(type, sensor)
43 {
44 }
45
LinearSMC(SP::ControlSensor sensor,SP::SimpleMatrix B,SP::SimpleMatrix D,unsigned int type)46 LinearSMC::LinearSMC(SP::ControlSensor sensor, SP::SimpleMatrix B, SP::SimpleMatrix D, unsigned int type):
47 CommonSMC(type, sensor, B, D)
48 {
49 }
50
~LinearSMC()51 LinearSMC::~LinearSMC()
52 {
53 }
54
actuate()55 void LinearSMC::actuate()
56 {
57 DEBUG_BEGIN("void LinearSMC::actuate()\n")
58
59 if(!_noUeq)
60 {
61 computeUeq();
62 FirstOrderLinearDS& LinearDS_SMC = *std::static_pointer_cast<FirstOrderLinearDS>(_DS_SMC);
63 prod(*_B, *_ueq, *(LinearDS_SMC.b()));
64 }
65
66 DEBUG_EXPR(_DS_SMC->xMemory().display(););
67
68 *(_DS_SMC->x()) = _sensor->y();
69
70 // SS: Really need to modify stored xMemory?
71 _DS_SMC->xMemory().getSiconosVectorMutable(0) = _sensor->y();
72
73 Type::Siconos dsType = Type::value(*_DS_SMC);
74 if(dsType == Type::FirstOrderNonLinearDS)
75 {
76 _DS_SMC->computef(_simulationSMC->startingTime(), _DS_SMC->x());
77 _DS_SMC->swapInMemory();
78 // _DS_SMC->computef(_simulationSMC->startingTime());
79 // *_DS_SMC->fold() = *_DS_SMC->f();
80 }
81
82 _simulationSMC->computeOneStep();
83 // if (_indx > 0)
84 {
85 _simulationSMC->nextStep();
86 }
87
88
89 // discontinous part
90 *_us = *_lambda;
91 *_u = *_us;
92 *_u += *_ueq;
93 DEBUG_EXPR(_u->display(););
94 _indx++;
95 DEBUG_END("void LinearSMC::actuate()\n")
96 }
97
98 AUTO_REGISTER_ACTUATOR(LINEAR_SMC, LinearSMC)
99