Permalink
Browse files

Modules reorganization and logic governor

  • Loading branch information...
1 parent 965a459 commit 343766844465e0dcc2a74cb69ad4117e1ddb6937 @jonatanolofsson committed Feb 1, 2012
View
@@ -20,6 +20,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
add_subdirectory(lib)
+include_directories(${PROJECT_SOURCE_DIR})
include_directories(${PROJECT_SOURCE_DIR}/include)
include_directories(${PROJECT_SOURCE_DIR}/lib)
include_directories(${YAML_CPP_SOURCE_DIR}/include)
View
@@ -6,10 +6,10 @@ modules:
- name: sender
file: comm_sender.so
-
- - name: receiver
- file: comm_receiver.so
- configuration: test
+#~
+ #~ - name: receiver
+ #~ file: comm_receiver.so
+ #~ configuration: test
- name: observer
file: observer.so
@@ -27,5 +27,11 @@ modules:
#~ use_cmcu: No
#~ cmcu_port: /dev/serial/by-id/
-
-
+ - name: logic
+ file: logic.so
+ configuration:
+ frequency: 50
+ initial_mode: hover
+ modes:
+ directory: flightmodes/
+ extension: .so
@@ -21,4 +21,8 @@ add_library(slicot_control
SB02OW.f
SB02OY.f
)
-target_link_libraries(slicot_control ${LAPACK_LIBRARIES})
+
+option(LINK_LAPACK On)
+if(LINK_LAPACK)
+ target_link_libraries(slicot_control ${LAPACK_LIBRARIES})
+endif(LINK_LAPACK)
@@ -0,0 +1,9 @@
+set(module controller)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -lgfortran")
+
+add_library(${module} SHARED ${module}.cpp)
+set_target_properties(${module} PROPERTIES PREFIX "")
+target_link_libraries(${module}
+ ${Boost_LIBRARIES}
+ slicot_control
+)
@@ -0,0 +1,77 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "crap/module.hpp"
+#include "modules/controller/model.hpp"
+#include "modules/observer/model.hpp"
+#include "math/control/LQ.hpp"
+
+namespace CRAP {
+ namespace controller {
+ control_vector u;
+ reference_vector r;
+ YAML::Node config;
+
+ typedef control::LQ<model::number_of_states, model::number_of_controls> controller_type;
+ controller_type reg;
+
+
+ void calculate_control_signal(const observer::model::state_vector& state) {
+ u = reg.control_signal(state, r);
+ //~ cpplot::figure("Control signal") << u(0);
+ }
+
+ void init_controller(YAML::Node& c) {
+ r.setZero(); u.setZero();
+ config = c;
+ control::linear_model<model::number_of_states, model::number_of_controls> lmodel;
+ lmodel.A << 0, 1,
+ 0, 0;
+ lmodel.B << 0,
+ 1;
+ lmodel.M << 1,
+ 0;
+ lmodel.Q.setIdentity();
+ lmodel.R.setIdentity();
+
+ reg.set_model(lmodel);
+ }
+ }
+}
+
+// Module interface
+extern "C" {
+ using namespace CRAP::controller;
+
+ control_vector control_signal() {
+ return u;
+ }
+}
+
+// CRAP interface
+extern "C" {
+ using namespace CRAP;
+ void configure(YAML::Node& c) {
+ controller::init_controller(c);
+ }
+
+ void run() {
+ comm::listen("/state_estimate", controller::calculate_control_signal);
+ }
+}
@@ -0,0 +1,39 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CRAP_CONTROLLER_MODEL_HPP_
+#define CRAP_CONTROLLER_MODEL_HPP_
+#include <Eigen/Core>
+
+namespace CRAP {
+ namespace controller {
+ using namespace Eigen;
+ namespace model {
+ const int number_of_states = 2;
+ const int number_of_controls = 1;
+ }
+
+ typedef Matrix<double, model::number_of_controls, 1> control_vector;
+ typedef Matrix<double, model::number_of_controls, 1> reference_vector;
+ typedef Matrix<double, model::number_of_states, 1> state_vector;
+ typedef control_vector(*control_signal_fn)();
+ }
+}
+
+#endif
View
@@ -0,0 +1,62 @@
+#include <map>
+#include <boost/thread/mutex.hpp>
+#include <assert.h>
+#include "modules/logic/governor.hpp"
+#include <dlfcn.h>
+#include "crap/state_engine.hpp"
+
+namespace CRAP {
+ namespace logic {
+ namespace governor {
+ typedef std::map<std::string, void*> modemap_t;
+ modemap_t modes;
+ modemap_t mode_handles;
+ state_engine_t state_machine;
+
+ std::string current_mode;
+ YAML::Node config;
+
+
+ void configure(YAML::Node& c) {
+ config = c;
+ }
+
+ bool load_mode(const std::string mode) {
+ if(modes.find(mode) != modes.end()) return true;
+ void* m;
+ m = dlopen((config["modes"]["directory"].as<std::string>("") + mode + config["modes"]["extension"].as<std::string>(".so")).c_str(), RTLD_LAZY);
+ if(m) {
+ void* f;
+ f = dlsym(m, mode.c_str());
+ if(f) {
+ modes[mode] = f;
+ mode_handles[mode] = m;
+ std::cout << "Loaded flightmode: " << mode << std::endl;
+ return true;
+ }
+ else {
+ std::cout << "Function not found in loaded file: " << mode << std::endl;
+ dlclose(m);
+ }
+ }
+ std::cout << "Flightmode loading failed: " << mode << std::endl;
+ return false;
+ }
+
+
+ void switch_mode(const std::string& mode) {
+ if(mode == current_mode) return;
+ if(load_mode(mode)) {
+ state_machine.next(modes[mode]);
+ current_mode = mode;
+ }
+ }
+
+ void tick() {
+ //if(battery level < battery threshold) switch_mode("landing");
+
+ state_machine();
+ }
+ }
+ }
+}
View
@@ -0,0 +1,36 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CRAP_LOGIC_GOVERNOR_HPP_
+#define CRAP_LOGIC_GOVERNOR_HPP_
+
+#include <string>
+#include "yaml-cpp/yaml.h"
+
+namespace CRAP {
+ namespace logic {
+ namespace governor {
+ void configure(YAML::Node& c);
+ void switch_mode(const std::string& mode);
+ void tick();
+ }
+ }
+}
+
+#endif
View
@@ -0,0 +1,56 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include "crap/module.hpp"
+#include "crap/state_engine.hpp"
+#include "modules/logic/governor.hpp"
+#include <iostream>
+
+namespace CRAP {
+ namespace logic {
+ CRAP::time::frequency_t frequency;
+
+ bool on = true;
+
+ void state_engine() {
+ while(on && time::ticktock(frequency)) {
+ governor::tick();
+ }
+ }
+ }
+}
+
+extern "C" {
+ using namespace CRAP;
+ void configure(YAML::Node& c) {
+ logic::governor::configure(c);
+ logic::frequency = time::frequency_t(c["frequency"].as<double>(50));
+ logic::governor::switch_mode(c["initial_mode"].as<std::string>());
+ }
+
+ void run() {
+ comm::listen("/logic/mode", logic::governor::switch_mode);
+ logic::state_engine();
+ }
+
+ void shutdown() {
+ logic::on = false;
+ }
+}
@@ -0,0 +1,10 @@
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/flightmodes)
+
+set(MODES
+ hover
+)
+
+
+foreach(mode ${MODES})
+ add_subdirectory(${mode})
+endforeach(mode)
@@ -0,0 +1,5 @@
+set(mode hover)
+add_library(${mode} SHARED ${mode}.cpp)
+set_target_properties(${mode} PROPERTIES PREFIX "")
+target_link_libraries(${mode}
+ ${Boost_LIBRARIES})
@@ -0,0 +1,48 @@
+/**
+ * Copyright 2011, 2012 Jonatan Olofsson
+ *
+ * This file is part of C++ Robot Automation Platform (CRAP).
+ *
+ * CRAP is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * CRAP is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with CRAP. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "hover.hpp"
+#include <iostream>
+#include <Eigen/Core>
+#include "modules/observer/model.hpp"
+#include "crap/communication.hpp"
+
+extern "C" {
+ using namespace CRAP;
+ using namespace Eigen;
+ using namespace observer::model;
+
+ Vector3d target_state;
+
+ typedef state_vector(*state_fn)();
+ state_fn get_state = comm::bind<state_fn>("observer", "get_state");
+
+ void* hover() {
+ std::cout << "Hovering state 1" << std::endl;
+ state_vector current_state = get_state();
+ target_state.x() = current_state(state::position);
+ return (void*)&hover2;
+ }
+
+ void* hover2() {
+ std::cout << "Hovering state 2" << std::endl;
+
+ return (void*)&hover2;
+ }
+}
Oops, something went wrong.

0 comments on commit 3437668

Please sign in to comment.