diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000000000000000000000000000000000000..6a7d9021d7737266fff887bfc0e5f5974ddb7689 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "3rd/NatNetLinux"] + path = 3rd/NatNetLinux + url = git@github.com:rocketman768/NatNetLinux.git diff --git a/3rd/.gitignore b/3rd/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..a007feab071f496a016f150e1acd8fae57669cce --- /dev/null +++ b/3rd/.gitignore @@ -0,0 +1 @@ +build/* diff --git a/3rd/NatNetLinux b/3rd/NatNetLinux new file mode 160000 index 0000000000000000000000000000000000000000..f87e8ca39b7a77ec1692b78e96c58a901f455fa7 --- /dev/null +++ b/3rd/NatNetLinux @@ -0,0 +1 @@ +Subproject commit f87e8ca39b7a77ec1692b78e96c58a901f455fa7 diff --git a/README.md b/README.md new file mode 100644 index 0000000000000000000000000000000000000000..8245eac508c09714261680c158b8cc54e16c9746 --- /dev/null +++ b/README.md @@ -0,0 +1,7 @@ +# JugglerOptiTrackMbedEsconSolution + +# SoftTrunk +code for 3D version of soft arm using rigid body model approach + +# 3rd +3rd party libraries as submodules diff --git a/SoftTrunk/.gitignore b/SoftTrunk/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..78c27dfcdc320b794cf035d42a1dc203b1405734 --- /dev/null +++ b/SoftTrunk/.gitignore @@ -0,0 +1,14 @@ +# Ignore all +* + +# Unignore all with extensions +!*.* + +# Unignore all dirs +!*/ + +*CMakeCache.txt +*cmake_install.cmake +CMakeFiles/* +graph.png +*.h.gch diff --git a/SoftTrunk/CMakeLists.txt b/SoftTrunk/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..f21d98136ca8c0647b139c1f1b6f2ffa14cc03a2 --- /dev/null +++ b/SoftTrunk/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required (VERSION 3.0) +set (CMAKE_CXX_STANDARD 11) +project(SoftTrunk) +include_directories(include src/matplotlib-cpp src/MiniPID include/mpa include/modbus) + +find_package(PythonLibs 2.7) +FIND_PACKAGE(PkgConfig) +# https://github.com/AgileManufacturing/Avans/blob/master/cmake/FindModbus.cmake +pkg_check_modules(PC_LIBMODBUS REQUIRED libmodbus) + + +add_executable(test_valve src/test_valve.cpp src/mpa/mpa.cpp src/MiniPID/MiniPID.cpp) + + +add_executable(example_forceController src/example_forceController.cpp src/forceController.cpp src/mpa/mpa.cpp src/MiniPID/MiniPID.cpp) +target_include_directories(example_forceController PRIVATE ${PYTHON_INCLUDE_DIRS}) +target_link_libraries(example_forceController ${PYTHON_LIBRARIES}) +target_link_libraries(example_forceController -pthread) +# http://www.kaizou.org/2014/11/typical-cmake-project/ +target_include_directories(example_forceController PRIVATE ${PC_LIBMODBUS_DIRS}) +target_link_libraries(example_forceController ${PC_LIBMODBUS_LIBRARIES} ) + +add_executable(example_sinusoidal src/example_sinusoidal.cpp src/forceController.cpp src/mpa/mpa.cpp src/MiniPID/MiniPID.cpp) +target_include_directories(example_sinusoidal PRIVATE ${PYTHON_INCLUDE_DIRS}) +target_link_libraries(example_sinusoidal ${PYTHON_LIBRARIES}) +target_link_libraries(example_sinusoidal -pthread) +# http://www.kaizou.org/2014/11/typical-cmake-project/ +target_include_directories(example_sinusoidal PRIVATE ${PC_LIBMODBUS_DIRS}) +target_link_libraries(example_sinusoidal ${PC_LIBMODBUS_LIBRARIES} ) + +FIND_PACKAGE( Boost COMPONENTS program_options system thread REQUIRED) +INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIRS} ) +ADD_EXECUTABLE( example_optitrack "src/example_optitrack.cpp" ) +TARGET_LINK_LIBRARIES( example_optitrack ${Boost_LIBRARIES} ) diff --git a/SoftTrunk/README.md b/SoftTrunk/README.md new file mode 100644 index 0000000000000000000000000000000000000000..74d9dcecfbcb485631ec599482cc4e676b8e2eae --- /dev/null +++ b/SoftTrunk/README.md @@ -0,0 +1,15 @@ +# Fabrication of soft robot +View **Soft Robot Fabrication.md** +# Installing libmodbus +`sudo apt install libmodbus-dev` +# Installing RBDL +This code uses the [Rigid Body Dynamics Library](https://rbdl.bitbucket.io/index.html). +Download the most recent stable version as zip file, then follow its README to install, but make sure to also compile the URDF reader addon; +``` +mkdir build +cd build/ +cmake -D RBDL_BUILD_ADDON_URDFREADER=ON CMAKE_BUILD_TYPE=Release ../ +make +sudo make install +``` +May also need to install Eigen3, as described. diff --git a/SoftTrunk/Soft Robot Fabrication.md b/SoftTrunk/Soft Robot Fabrication.md new file mode 100644 index 0000000000000000000000000000000000000000..3af9198923b0b642784e9805c02b45c8d37e5b45 --- /dev/null +++ b/SoftTrunk/Soft Robot Fabrication.md @@ -0,0 +1,89 @@ +Steps to fabricate the soft trunk +# silicon casting +1. Assemble mold, if casting the final arm piece + 1. make sure the wax mold doesn't have rough edges + ![](img/IMG_1774.JPG) + a not-so-clean wax mold + 1. assemble them to make final structure (will probably need to shave off fraction of mm into the holes of wax to make them fit) + ![](img/IMG_1635.JPG) + assembled mold + ![](img/IMG_1775.JPG) + make sure wax pieces form a circle +1. make mix + 1. stir ingredients well before mixing (obviously, use separate stirrers!) + 1. **degas** + 1. put in vacuum chamber + 1. turn on pump + 1. twist valve to vacuum air out + 1. and keep turning them on and off to prevent overflowing + ![](img/IMG_1644.JPG) + 1. do until bubbles subside + 1. turn off pump, slowly pressurize + 1. weigh equal amounts of ingredients(use zero button on scale) + 1. mix well (instructions say to mix for 2-3 minutes) + 1. degas the mixed silicone +1. Spray Ease-Release on mold +![](img/IMG_1642.JPG) +1. pour silicon onto mold + 1. pour slowly, a little below top surface (so it won't overflow in vacuum chamber) + 1. degas + 1. if making final arm piece, degas for ~30 minutes to ensure bubbles are gone + 1. place on desk, pour again and go slightly over the top + 1. slide over with ruler or something to make top flush +1. let it sleep for as long as silicon wants +1. take it out (some technique required) +![](img/IMG_1637.JPG) + * for the cast for the wax mold, (what good technique is there to release from mold??) + * for final arm piece, twist it around to loosen it + ![](img/IMG_1638.JPG) + +# wax casting +1. warm stuff up + 1. heat oven to 92 degrees Celcius + 1. generate molten wax from block of wax (break up with hammer if necessary)(place so it won't melt to outside cup) + 1. Put silicon mold in oven as well (~15 minutes) +1. align silicon mold with something to make sure it's straight +1. inject wax + 1. take out jar with wax and syringe(use gloves!) + 1. inject to mold, slightly overfilling it + ![image of mold with wax injected in it](img/IMG_1766.JPG) + 1. syringe will clog if out too long (put back in jar if that happens, to warm it up) +1. make flush + 1. after 10 minutes, use blade to cut wax flush with top surface of mold + ![image of blade cutting wax](img/IMG_1773.JPG) +1. should be cooled down in about 30 minutes +1. carefully take them out(once it's not warm anymore, which is about 40 minutes? **Don't wait till they're too cold**- they'll become too brittle and break easily) and handle with care +1. If there's some deforming (twists, bends), try to correct it... + +# vanishing wax and cleaning +1. Put arm in oven supported by aluminum shafts, with aluminum plate below to catch molten wax +1. put in boiling water +1. clean with water- run water through holes +1. clean holes for tubing with wax remover, then wipe it off with q-tips(use gloves!) +1. clean holes for tubing with alcohol + +# tubing +1. make sure holes are cleaned with wax remover and alcohol +1. put silicon glue on both the hole(use q-tips) and tube(on all the surface that bonds to main piece)- use kimwipes +1. insert + 1. make sure the plugs are flush + 1. insert about 1 cm + 1. 12 inches (30cm) of slack for air tubing + +# Using 3D printer +## Solidworks +Export STL +* The mold for wax mold is in ConfigurationManager -> Quad Mold Mold + +## Insight +insert STL -> orient STL + +In Modeler setup, +* part interior style: solid +* visible surface style: enhanced +* support style: SMART + +then "slice", then check that it looks fine. Finally, "finish" to write out the sliced files. + +## Control Center +select printer, insert CMB, build job diff --git a/SoftTrunk/img/IMG_1635.JPG b/SoftTrunk/img/IMG_1635.JPG new file mode 100644 index 0000000000000000000000000000000000000000..f1b07f33cd2989ffe3bffbc93561637348632a08 Binary files /dev/null and b/SoftTrunk/img/IMG_1635.JPG differ diff --git a/SoftTrunk/img/IMG_1637.JPG b/SoftTrunk/img/IMG_1637.JPG new file mode 100644 index 0000000000000000000000000000000000000000..479851e2059db86f433017235108ab1f001acbdc Binary files /dev/null and b/SoftTrunk/img/IMG_1637.JPG differ diff --git a/SoftTrunk/img/IMG_1638.JPG b/SoftTrunk/img/IMG_1638.JPG new file mode 100644 index 0000000000000000000000000000000000000000..c346e57d948c36943e88b64c2af74cee24bb434b Binary files /dev/null and b/SoftTrunk/img/IMG_1638.JPG differ diff --git a/SoftTrunk/img/IMG_1642.JPG b/SoftTrunk/img/IMG_1642.JPG new file mode 100644 index 0000000000000000000000000000000000000000..753b46f5ecc2131ad9f9abb94bba196f28b06e9d Binary files /dev/null and b/SoftTrunk/img/IMG_1642.JPG differ diff --git a/SoftTrunk/img/IMG_1644.JPG b/SoftTrunk/img/IMG_1644.JPG new file mode 100644 index 0000000000000000000000000000000000000000..fcf455d18db02a894fdfd16bc8866221e53ba61b Binary files /dev/null and b/SoftTrunk/img/IMG_1644.JPG differ diff --git a/SoftTrunk/img/IMG_1766.JPG b/SoftTrunk/img/IMG_1766.JPG new file mode 100644 index 0000000000000000000000000000000000000000..19fe9ad6037b6af17b186cbdf28191d2bf5bc8e7 Binary files /dev/null and b/SoftTrunk/img/IMG_1766.JPG differ diff --git a/SoftTrunk/img/IMG_1773.JPG b/SoftTrunk/img/IMG_1773.JPG new file mode 100644 index 0000000000000000000000000000000000000000..dba90578cefa5b8270214d2af020c7c0d198f863 Binary files /dev/null and b/SoftTrunk/img/IMG_1773.JPG differ diff --git a/SoftTrunk/img/IMG_1774.JPG b/SoftTrunk/img/IMG_1774.JPG new file mode 100644 index 0000000000000000000000000000000000000000..fa2213890b512ec482f0c6c96ece269e2794e0c6 Binary files /dev/null and b/SoftTrunk/img/IMG_1774.JPG differ diff --git a/SoftTrunk/img/IMG_1775.JPG b/SoftTrunk/img/IMG_1775.JPG new file mode 100644 index 0000000000000000000000000000000000000000..339f59daf69bec3c124feb31e6d556574d743496 Binary files /dev/null and b/SoftTrunk/img/IMG_1775.JPG differ diff --git a/SoftTrunk/include/arm.h b/SoftTrunk/include/arm.h new file mode 100644 index 0000000000000000000000000000000000000000..1dbf1a262855c64266d4f51d0cb4927cedf9247d --- /dev/null +++ b/SoftTrunk/include/arm.h @@ -0,0 +1,31 @@ +#ifndef ARMPCC_H +#define ARMPCC_H + +#define ARM_ELEMENTS 3 + +#ifndef RBDL_BUILD_ADDON_URDFREADER + #error "Error: RBDL addon URDFReader not enabled." +#endif + +#include <rbdl/addons/urdfreader/urdfreader.h> +using RigidBodyDynamics; +using RigidBodyDynamics::Math; + + +class ArmElement{ + // one PCC element in the arm. +private: + double length; + Model rbdl_model; +public: + ArmElement(double length); +}; + +class Arm{ + // the PCC Arm itself. +private: + ArmElement* armElements[ARM_ELEMENTS]; +public: + Arm(); +}; +#endif diff --git a/SoftTrunk/include/forceController.h b/SoftTrunk/include/forceController.h new file mode 100644 index 0000000000000000000000000000000000000000..45ea12c2b4be8411e23b9776a7d9e7be9c7d2fc9 --- /dev/null +++ b/SoftTrunk/include/forceController.h @@ -0,0 +1,28 @@ +#ifndef FORCECONTROLLER_H +#define FORCECONTROLLER_H + +#include "MiniPID.h" +#include "matplotlibcpp.h" +#include "mpa.h" +#include <thread> +#include <vector> + +class ForceController { +private: + void controllerThread(); + bool run; + std::vector<int> commanded_pressures; + int DoF; + std::vector<MiniPID> pid; + MPA mpa{"192.168.1.101", "502"}; + std::thread controller_thread; + std::vector<double> x; + std::vector<double> pressure_log; + std::vector<double> commandpressure_log; + +public: + void setSinglePressure(int, int); + explicit ForceController(int); + void disconnect(); +}; +#endif diff --git a/SoftTrunk/include/modbus/modbus-rtu.h b/SoftTrunk/include/modbus/modbus-rtu.h new file mode 100644 index 0000000000000000000000000000000000000000..214a888b7e692fdc40f904b2573c4b93580e25ef --- /dev/null +++ b/SoftTrunk/include/modbus/modbus-rtu.h @@ -0,0 +1,42 @@ +/* + * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com> + * + * SPDX-License-Identifier: LGPL-2.1+ + */ + +#ifndef MODBUS_RTU_H +#define MODBUS_RTU_H + +#include "modbus.h" + +MODBUS_BEGIN_DECLS + +/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5 + * RS232 / RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes + */ +#define MODBUS_RTU_MAX_ADU_LENGTH 256 + +MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity, + int data_bit, int stop_bit); + +#define MODBUS_RTU_RS232 0 +#define MODBUS_RTU_RS485 1 + +MODBUS_API int modbus_rtu_set_serial_mode(modbus_t *ctx, int mode); +MODBUS_API int modbus_rtu_get_serial_mode(modbus_t *ctx); + +#define MODBUS_RTU_RTS_NONE 0 +#define MODBUS_RTU_RTS_UP 1 +#define MODBUS_RTU_RTS_DOWN 2 + +MODBUS_API int modbus_rtu_set_rts(modbus_t *ctx, int mode); +MODBUS_API int modbus_rtu_get_rts(modbus_t *ctx); + +MODBUS_API int modbus_rtu_set_custom_rts(modbus_t *ctx, void (*set_rts) (modbus_t *ctx, int on)); + +MODBUS_API int modbus_rtu_set_rts_delay(modbus_t *ctx, int us); +MODBUS_API int modbus_rtu_get_rts_delay(modbus_t *ctx); + +MODBUS_END_DECLS + +#endif /* MODBUS_RTU_H */ diff --git a/SoftTrunk/include/modbus/modbus-tcp.h b/SoftTrunk/include/modbus/modbus-tcp.h new file mode 100644 index 0000000000000000000000000000000000000000..abaef2762c7ebc6dd55c5c7d90e6421710e08d33 --- /dev/null +++ b/SoftTrunk/include/modbus/modbus-tcp.h @@ -0,0 +1,52 @@ +/* + * Copyright © 2001-2010 Stéphane Raimbault <stephane.raimbault@gmail.com> + * + * SPDX-License-Identifier: LGPL-2.1+ + */ + +#ifndef MODBUS_TCP_H +#define MODBUS_TCP_H + +#include "modbus.h" + +MODBUS_BEGIN_DECLS + +#if defined(_WIN32) && !defined(__CYGWIN__) +/* Win32 with MinGW, supplement to <errno.h> */ +#include <winsock2.h> +#if !defined(ECONNRESET) +#define ECONNRESET WSAECONNRESET +#endif +#if !defined(ECONNREFUSED) +#define ECONNREFUSED WSAECONNREFUSED +#endif +#if !defined(ETIMEDOUT) +#define ETIMEDOUT WSAETIMEDOUT +#endif +#if !defined(ENOPROTOOPT) +#define ENOPROTOOPT WSAENOPROTOOPT +#endif +#if !defined(EINPROGRESS) +#define EINPROGRESS WSAEINPROGRESS +#endif +#endif + +#define MODBUS_TCP_DEFAULT_PORT 502 +#define MODBUS_TCP_SLAVE 0xFF + +/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5 + * TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes + */ +#define MODBUS_TCP_MAX_ADU_LENGTH 260 + +MODBUS_API modbus_t* modbus_new_tcp(const char *ip_address, int port); +MODBUS_API int modbus_tcp_listen(modbus_t *ctx, int nb_connection); +MODBUS_API int modbus_tcp_accept(modbus_t *ctx, int *s); + +MODBUS_API modbus_t* modbus_new_tcp_pi(const char *node, const char *service); +MODBUS_API int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection); +MODBUS_API int modbus_tcp_pi_accept(modbus_t *ctx, int *s); + +MODBUS_END_DECLS + +#endif /* MODBUS_TCP_H */ diff --git a/SoftTrunk/include/modbus/modbus-version.h b/SoftTrunk/include/modbus/modbus-version.h new file mode 100644 index 0000000000000000000000000000000000000000..743efff11babcb214d383dcd720c7bcf55cd41eb --- /dev/null +++ b/SoftTrunk/include/modbus/modbus-version.h @@ -0,0 +1,53 @@ +/* + * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com> + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#ifndef MODBUS_VERSION_H +#define MODBUS_VERSION_H + +/* The major version, (1, if %LIBMODBUS_VERSION is 1.2.3) */ +#define LIBMODBUS_VERSION_MAJOR (3) + +/* The minor version (2, if %LIBMODBUS_VERSION is 1.2.3) */ +#define LIBMODBUS_VERSION_MINOR (1) + +/* The micro version (3, if %LIBMODBUS_VERSION is 1.2.3) */ +#define LIBMODBUS_VERSION_MICRO (4) + +/* The full version, like 1.2.3 */ +#define LIBMODBUS_VERSION 3.1.4 + +/* The full version, in string form (suited for string concatenation) + */ +#define LIBMODBUS_VERSION_STRING "3.1.4" + +/* Numerically encoded version, eg. v1.2.3 is 0x010203 */ +#define LIBMODBUS_VERSION_HEX ((LIBMODBUS_VERSION_MAJOR << 16) | \ + (LIBMODBUS_VERSION_MINOR << 8) | \ + (LIBMODBUS_VERSION_MICRO << 0)) + +/* Evaluates to True if the version is greater than @major, @minor and @micro + */ +#define LIBMODBUS_VERSION_CHECK(major,minor,micro) \ + (LIBMODBUS_VERSION_MAJOR > (major) || \ + (LIBMODBUS_VERSION_MAJOR == (major) && \ + LIBMODBUS_VERSION_MINOR > (minor)) || \ + (LIBMODBUS_VERSION_MAJOR == (major) && \ + LIBMODBUS_VERSION_MINOR == (minor) && \ + LIBMODBUS_VERSION_MICRO >= (micro))) + +#endif /* MODBUS_VERSION_H */ diff --git a/SoftTrunk/include/modbus/modbus.h b/SoftTrunk/include/modbus/modbus.h new file mode 100644 index 0000000000000000000000000000000000000000..fda3f02b70ba4ef306c530f679837e36999e95d2 --- /dev/null +++ b/SoftTrunk/include/modbus/modbus.h @@ -0,0 +1,293 @@ +/* + * Copyright © 2001-2013 Stéphane Raimbault <stephane.raimbault@gmail.com> + * + * SPDX-License-Identifier: LGPL-2.1+ + */ + +#ifndef MODBUS_H +#define MODBUS_H + +/* Add this for macros that defined unix flavor */ +#if (defined(__unix__) || defined(unix)) && !defined(USG) +#include <sys/param.h> +#endif + +#ifndef _MSC_VER +#include <stdint.h> +#else +#include "stdint.h" +#endif + +#include "modbus-version.h" + +#if defined(_MSC_VER) +# if defined(DLLBUILD) +/* define DLLBUILD when building the DLL */ +# define MODBUS_API __declspec(dllexport) +# else +# define MODBUS_API __declspec(dllimport) +# endif +#else +# define MODBUS_API +#endif + +#ifdef __cplusplus +# define MODBUS_BEGIN_DECLS extern "C" { +# define MODBUS_END_DECLS } +#else +# define MODBUS_BEGIN_DECLS +# define MODBUS_END_DECLS +#endif + +MODBUS_BEGIN_DECLS + +#ifndef FALSE +#define FALSE 0 +#endif + +#ifndef TRUE +#define TRUE 1 +#endif + +#ifndef OFF +#define OFF 0 +#endif + +#ifndef ON +#define ON 1 +#endif + +/* Modbus function codes */ +#define MODBUS_FC_READ_COILS 0x01 +#define MODBUS_FC_READ_DISCRETE_INPUTS 0x02 +#define MODBUS_FC_READ_HOLDING_REGISTERS 0x03 +#define MODBUS_FC_READ_INPUT_REGISTERS 0x04 +#define MODBUS_FC_WRITE_SINGLE_COIL 0x05 +#define MODBUS_FC_WRITE_SINGLE_REGISTER 0x06 +#define MODBUS_FC_READ_EXCEPTION_STATUS 0x07 +#define MODBUS_FC_WRITE_MULTIPLE_COILS 0x0F +#define MODBUS_FC_WRITE_MULTIPLE_REGISTERS 0x10 +#define MODBUS_FC_REPORT_SLAVE_ID 0x11 +#define MODBUS_FC_MASK_WRITE_REGISTER 0x16 +#define MODBUS_FC_WRITE_AND_READ_REGISTERS 0x17 + +#define MODBUS_BROADCAST_ADDRESS 0 + +/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 1 page 12) + * Quantity of Coils to read (2 bytes): 1 to 2000 (0x7D0) + * (chapter 6 section 11 page 29) + * Quantity of Coils to write (2 bytes): 1 to 1968 (0x7B0) + */ +#define MODBUS_MAX_READ_BITS 2000 +#define MODBUS_MAX_WRITE_BITS 1968 + +/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 3 page 15) + * Quantity of Registers to read (2 bytes): 1 to 125 (0x7D) + * (chapter 6 section 12 page 31) + * Quantity of Registers to write (2 bytes) 1 to 123 (0x7B) + * (chapter 6 section 17 page 38) + * Quantity of Registers to write in R/W registers (2 bytes) 1 to 121 (0x79) + */ +#define MODBUS_MAX_READ_REGISTERS 125 +#define MODBUS_MAX_WRITE_REGISTERS 123 +#define MODBUS_MAX_WR_WRITE_REGISTERS 121 +#define MODBUS_MAX_WR_READ_REGISTERS 125 + +/* The size of the MODBUS PDU is limited by the size constraint inherited from + * the first MODBUS implementation on Serial Line network (max. RS485 ADU = 256 + * bytes). Therefore, MODBUS PDU for serial line communication = 256 - Server + * address (1 byte) - CRC (2 bytes) = 253 bytes. + */ +#define MODBUS_MAX_PDU_LENGTH 253 + +/* Consequently: + * - RTU MODBUS ADU = 253 bytes + Server address (1 byte) + CRC (2 bytes) = 256 + * bytes. + * - TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes. + * so the maximum of both backend in 260 bytes. This size can used to allocate + * an array of bytes to store responses and it will be compatible with the two + * backends. + */ +#define MODBUS_MAX_ADU_LENGTH 260 + +/* Random number to avoid errno conflicts */ +#define MODBUS_ENOBASE 112345678 + +/* Protocol exceptions */ +enum { + MODBUS_EXCEPTION_ILLEGAL_FUNCTION = 0x01, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, + MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE, + MODBUS_EXCEPTION_ACKNOWLEDGE, + MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY, + MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE, + MODBUS_EXCEPTION_MEMORY_PARITY, + MODBUS_EXCEPTION_NOT_DEFINED, + MODBUS_EXCEPTION_GATEWAY_PATH, + MODBUS_EXCEPTION_GATEWAY_TARGET, + MODBUS_EXCEPTION_MAX +}; + +#define EMBXILFUN (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_FUNCTION) +#define EMBXILADD (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS) +#define EMBXILVAL (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE) +#define EMBXSFAIL (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE) +#define EMBXACK (MODBUS_ENOBASE + MODBUS_EXCEPTION_ACKNOWLEDGE) +#define EMBXSBUSY (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY) +#define EMBXNACK (MODBUS_ENOBASE + MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE) +#define EMBXMEMPAR (MODBUS_ENOBASE + MODBUS_EXCEPTION_MEMORY_PARITY) +#define EMBXGPATH (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_PATH) +#define EMBXGTAR (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_TARGET) + +/* Native libmodbus error codes */ +#define EMBBADCRC (EMBXGTAR + 1) +#define EMBBADDATA (EMBXGTAR + 2) +#define EMBBADEXC (EMBXGTAR + 3) +#define EMBUNKEXC (EMBXGTAR + 4) +#define EMBMDATA (EMBXGTAR + 5) +#define EMBBADSLAVE (EMBXGTAR + 6) + +extern const unsigned int libmodbus_version_major; +extern const unsigned int libmodbus_version_minor; +extern const unsigned int libmodbus_version_micro; + +typedef struct _modbus modbus_t; + +typedef struct { + int nb_bits; + int start_bits; + int nb_input_bits; + int start_input_bits; + int nb_input_registers; + int start_input_registers; + int nb_registers; + int start_registers; + uint8_t *tab_bits; + uint8_t *tab_input_bits; + uint16_t *tab_input_registers; + uint16_t *tab_registers; +} modbus_mapping_t; + +typedef enum +{ + MODBUS_ERROR_RECOVERY_NONE = 0, + MODBUS_ERROR_RECOVERY_LINK = (1<<1), + MODBUS_ERROR_RECOVERY_PROTOCOL = (1<<2) +} modbus_error_recovery_mode; + +MODBUS_API int modbus_set_slave(modbus_t* ctx, int slave); +MODBUS_API int modbus_get_slave(modbus_t* ctx); +MODBUS_API int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_mode error_recovery); +MODBUS_API int modbus_set_socket(modbus_t *ctx, int s); +MODBUS_API int modbus_get_socket(modbus_t *ctx); + +MODBUS_API int modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec); +MODBUS_API int modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec); + +MODBUS_API int modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec); +MODBUS_API int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec); + +MODBUS_API int modbus_get_indication_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec); +MODBUS_API int modbus_set_indication_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec); + +MODBUS_API int modbus_get_header_length(modbus_t *ctx); + +MODBUS_API int modbus_connect(modbus_t *ctx); +MODBUS_API void modbus_close(modbus_t *ctx); + +MODBUS_API void modbus_free(modbus_t *ctx); + +MODBUS_API int modbus_flush(modbus_t *ctx); +MODBUS_API int modbus_set_debug(modbus_t *ctx, int flag); + +MODBUS_API const char *modbus_strerror(int errnum); + +MODBUS_API int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest); +MODBUS_API int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest); +MODBUS_API int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest); +MODBUS_API int modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest); +MODBUS_API int modbus_write_bit(modbus_t *ctx, int coil_addr, int status); +MODBUS_API int modbus_write_register(modbus_t *ctx, int reg_addr, int value); +MODBUS_API int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data); +MODBUS_API int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data); +MODBUS_API int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask); +MODBUS_API int modbus_write_and_read_registers(modbus_t *ctx, int write_addr, int write_nb, + const uint16_t *src, int read_addr, int read_nb, + uint16_t *dest); +MODBUS_API int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest); + +MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address( + unsigned int start_bits, unsigned int nb_bits, + unsigned int start_input_bits, unsigned int nb_input_bits, + unsigned int start_registers, unsigned int nb_registers, + unsigned int start_input_registers, unsigned int nb_input_registers); + +MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits, + int nb_registers, int nb_input_registers); +MODBUS_API void modbus_mapping_free(modbus_mapping_t *mb_mapping); + +MODBUS_API int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length); + +MODBUS_API int modbus_receive(modbus_t *ctx, uint8_t *req); + +MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp); + +MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, + int req_length, modbus_mapping_t *mb_mapping); +MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, + unsigned int exception_code); + +/** + * UTILS FUNCTIONS + **/ + +#define MODBUS_GET_HIGH_BYTE(data) (((data) >> 8) & 0xFF) +#define MODBUS_GET_LOW_BYTE(data) ((data) & 0xFF) +#define MODBUS_GET_INT64_FROM_INT16(tab_int16, index) \ + (((int64_t)tab_int16[(index) ] << 48) + \ + ((int64_t)tab_int16[(index) + 1] << 32) + \ + ((int64_t)tab_int16[(index) + 2] << 16) + \ + (int64_t)tab_int16[(index) + 3]) +#define MODBUS_GET_INT32_FROM_INT16(tab_int16, index) ((tab_int16[(index)] << 16) + tab_int16[(index) + 1]) +#define MODBUS_GET_INT16_FROM_INT8(tab_int8, index) ((tab_int8[(index)] << 8) + tab_int8[(index) + 1]) +#define MODBUS_SET_INT16_TO_INT8(tab_int8, index, value) \ + do { \ + tab_int8[(index)] = (value) >> 8; \ + tab_int8[(index) + 1] = (value) & 0xFF; \ + } while (0) +#define MODBUS_SET_INT32_TO_INT16(tab_int16, index, value) \ + do { \ + tab_int16[(index) ] = (value) >> 16; \ + tab_int16[(index) + 1] = (value); \ + } while (0) +#define MODBUS_SET_INT64_TO_INT16(tab_int16, index, value) \ + do { \ + tab_int16[(index) ] = (value) >> 48; \ + tab_int16[(index) + 1] = (value) >> 32; \ + tab_int16[(index) + 2] = (value) >> 16; \ + tab_int16[(index) + 3] = (value); \ + } while (0) + +MODBUS_API void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value); +MODBUS_API void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits, + const uint8_t *tab_byte); +MODBUS_API uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx, unsigned int nb_bits); +MODBUS_API float modbus_get_float(const uint16_t *src); +MODBUS_API float modbus_get_float_abcd(const uint16_t *src); +MODBUS_API float modbus_get_float_dcba(const uint16_t *src); +MODBUS_API float modbus_get_float_badc(const uint16_t *src); +MODBUS_API float modbus_get_float_cdab(const uint16_t *src); + +MODBUS_API void modbus_set_float(float f, uint16_t *dest); +MODBUS_API void modbus_set_float_abcd(float f, uint16_t *dest); +MODBUS_API void modbus_set_float_dcba(float f, uint16_t *dest); +MODBUS_API void modbus_set_float_badc(float f, uint16_t *dest); +MODBUS_API void modbus_set_float_cdab(float f, uint16_t *dest); + +#include "modbus-tcp.h" +#include "modbus-rtu.h" + +MODBUS_END_DECLS + +#endif /* MODBUS_H */ diff --git a/SoftTrunk/include/mpa/mpa.h b/SoftTrunk/include/mpa/mpa.h new file mode 100644 index 0000000000000000000000000000000000000000..ee7a9f70ab3bfb0c385463a040f6215e8f617d1b --- /dev/null +++ b/SoftTrunk/include/mpa/mpa.h @@ -0,0 +1,34 @@ +#pragma once + +#include <cstdint> +#include <vector> + +#include "modbus.h" + +class MPA { +public: + explicit MPA(const char *node, const char *service); + ~MPA(); + + MPA &operator=(const MPA &) = delete; + MPA(const MPA &) = delete; + MPA() = default; + + bool connect(); + bool disconnect(); + + int get_single_pressure(int index); + void set_single_pressure(int index, int pressure); + + void get_all_pressures(std::vector<int> *output); + void set_all_pressures(const std::vector<int> &pressures); + +private: + void ensure_connection() const; + + std::vector<uint16_t> input_buffer_; + std::vector<uint16_t> output_buffer_; + + bool connected_; + modbus_t *ctx_; +}; diff --git a/SoftTrunk/src/MiniPID/LICENSE.txt b/SoftTrunk/src/MiniPID/LICENSE.txt new file mode 100644 index 0000000000000000000000000000000000000000..e587591e143165eb560c2385f0652ac369fc6595 --- /dev/null +++ b/SoftTrunk/src/MiniPID/LICENSE.txt @@ -0,0 +1,621 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/> + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS \ No newline at end of file diff --git a/SoftTrunk/src/MiniPID/MiniPID.cpp b/SoftTrunk/src/MiniPID/MiniPID.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2c5dc15e5e9d86d3faf2931ffbe734445143d45a --- /dev/null +++ b/SoftTrunk/src/MiniPID/MiniPID.cpp @@ -0,0 +1,408 @@ +/** +* Small, easy to use PID implementation with advanced controller capability.<br> +* Minimal usage:<br> +* setPID(p,i,d); <br> +* ...looping code...{ <br> +* output=getOutput(sensorvalue,target); <br> +* } +* +* @see +* http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-direction/improving-the-beginners-pid-introduction +*/ + +#include "MiniPID.h" + +//********************************** +// Constructor functions +//********************************** +MiniPID::MiniPID(double p, double i, double d) { + init(); + P = p; + I = i; + D = d; +} +MiniPID::MiniPID(double p, double i, double d, double f) { + init(); + P = p; + I = i; + D = d; + F = f; +} +void MiniPID::init() { + P = 0; + I = 0; + D = 0; + F = 0; + + maxIOutput = 0; + maxError = 0; + errorSum = 0; + maxOutput = 0; + minOutput = 0; + setpoint = 0; + lastActual = 0; + firstRun = true; + reversed = false; + outputRampRate = 0; + lastOutput = 0; + outputFilter = 0; + setpointRange = 0; +} + +//********************************** +// Configuration functions +//********************************** +/** + * Configure the Proportional gain parameter. <br> + * this->responds quicly to changes in setpoint, and provides most of the + * initial driving force + * to make corrections. <br> + * Some systems can be used with only a P gain, and many can be operated with + * only PI.<br> + * For position based controllers, this->is the first parameter to tune, with I + * second. <br> + * For rate controlled systems, this->is often the second after F. + * + * @param p Proportional gain. Affects output according to + * <b>output+=P*(setpoint-current_value)</b> + */ +void MiniPID::setP(double p) { + P = p; + checkSigns(); +} + +/** + * Changes the I parameter <br> + * this->is used for overcoming disturbances, and ensuring that the controller + * always gets to the control mode. + * Typically tuned second for "Position" based modes, and third for "Rate" or + * continuous based modes. <br> + * Affects output through <b>output+=previous_errors*Igain + * ;previous_errors+=current_error</b> + * + * @see {@link #setMaxIOutput(double) setMaxIOutput} for how to restrict + * + * @param i New gain value for the Integral term + */ +void MiniPID::setI(double i) { + if (I != 0) { + errorSum = errorSum * I / i; + } + if (maxIOutput != 0) { + maxError = maxIOutput / i; + } + I = i; + checkSigns(); + /* Implementation note: + * this->Scales the accumulated error to avoid output errors. + * As an example doubling the I term cuts the accumulated error in half, which + * results in the + * output change due to the I term constant during the transition. + * + */ +} + +void MiniPID::setD(double d) { + D = d; + checkSigns(); +} + +/**Configure the FeedForward parameter. <br> + * this->is excellent for Velocity, rate, and other continuous control modes + * where you can + * expect a rough output value based solely on the setpoint.<br> + * Should not be used in "position" based control modes. + * + * @param f Feed forward gain. Affects output according to + * <b>output+=F*Setpoint</b>; + */ +void MiniPID::setF(double f) { + F = f; + checkSigns(); +} + +/** Create a new PID object. + * @param p Proportional gain. Large if large difference between setpoint and + * target. + * @param i Integral gain. Becomes large if setpoint cannot reach target + * quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small + * values prevents P and I terms from causing overshoot. + */ +void MiniPID::setPID(double p, double i, double d) { + P = p; + I = i; + D = d; + checkSigns(); +} + +void MiniPID::setPID(double p, double i, double d, double f) { + P = p; + I = i; + D = d; + F = f; + checkSigns(); +} + +/**Set the maximum output value contributed by the I component of the system + * this->can be used to prevent large windup issues and make tuning simpler + * @param maximum. Units are the same as the expected output value + */ +void MiniPID::setMaxIOutput(double maximum) { + /* Internally maxError and Izone are similar, but scaled for different + * purposes. + * The maxError is generated for simplifying math, since calculations against + * the max error are far more common than changing the I term or Izone. + */ + maxIOutput = maximum; + if (I != 0) { + maxError = maxIOutput / I; + } +} + +/**Specify a maximum output. If a single parameter is specified, the minimum is + * set to (-maximum). + * @param output + */ +void MiniPID::setOutputLimits(double output) { + setOutputLimits(-output, output); +} + +/** + * Specify a maximum output. + * @param minimum possible output value + * @param maximum possible output value + */ +void MiniPID::setOutputLimits(double minimum, double maximum) { + if (maximum < minimum) + return; + maxOutput = maximum; + minOutput = minimum; + + // Ensure the bounds of the I term are within the bounds of the allowable + // output swing + if (maxIOutput == 0 || maxIOutput > (maximum - minimum)) { + setMaxIOutput(maximum - minimum); + } +} + +/** Set the operating direction of the PID controller + * @param reversed Set true to reverse PID output + */ +void MiniPID::setDirection(bool reversed) { this->reversed = reversed; } + +//********************************** +// Primary operating functions +//********************************** + +/**Set the target for the PID calculations + * @param setpoint + */ +void MiniPID::setSetpoint(double setpoint) { this->setpoint = setpoint; } + +/** Calculate the PID value needed to hit the target setpoint. +* Automatically re-calculates the output at each call. +* @param actual The monitored value +* @param target The target value +* @return calculated output value for driving the actual to the target +*/ +double MiniPID::getOutput(double actual, double setpoint) { + double output; + double Poutput; + double Ioutput; + double Doutput; + double Foutput; + + this->setpoint = setpoint; + + // Ramp the setpoint used for calculations if user has opted to do so + if (setpointRange != 0) { + setpoint = clamp(setpoint, actual - setpointRange, actual + setpointRange); + } + + // Do the simple parts of the calculations + double error = setpoint - actual; + + // Calculate F output. Notice, this->depends only on the setpoint, and not the + // error. + Foutput = F * setpoint; + + // Calculate P term + Poutput = P * error; + + // If this->is our first time running this-> we don't actually _have_ a + // previous input or output. + // For sensor, sanely assume it was exactly where it is now. + // For last output, we can assume it's the current time-independent outputs. + if (firstRun) { + lastActual = actual; + lastOutput = Poutput + Foutput; + firstRun = false; + } + + // Calculate D Term + // Note, this->is negative. this->actually "slows" the system if it's doing + // the correct thing, and small values helps prevent output spikes and + // overshoot + + Doutput = -D * (actual - lastActual); + lastActual = actual; + + // The Iterm is more complex. There's several things to factor in to make it + // easier to deal with. + // 1. maxIoutput restricts the amount of output contributed by the Iterm. + // 2. prevent windup by not increasing errorSum if we're already running + // against our max Ioutput + // 3. prevent windup by not increasing errorSum if output is output=maxOutput + Ioutput = I * errorSum; + if (maxIOutput != 0) { + Ioutput = clamp(Ioutput, -maxIOutput, maxIOutput); + } + + // And, finally, we can just add the terms up + output = Foutput + Poutput + Ioutput + Doutput; + + // Figure out what we're doing with the error. + if (minOutput != maxOutput && !bounded(output, minOutput, maxOutput)) { + errorSum = error; + // reset the error sum to a sane level + // Setting to current error ensures a smooth transition when the P term + // decreases enough for the I term to start acting upon the controller + // From that point the I term will build up as would be expected + } else if (outputRampRate != 0 && + !bounded(output, lastOutput - outputRampRate, + lastOutput + outputRampRate)) { + errorSum = error; + } else if (maxIOutput != 0) { + errorSum = clamp(errorSum + error, -maxError, maxError); + // In addition to output limiting directly, we also want to prevent I term + // buildup, so restrict the error directly + } else { + errorSum += error; + } + + // Restrict output to our specified output and ramp limits + if (outputRampRate != 0) { + output = + clamp(output, lastOutput - outputRampRate, lastOutput + outputRampRate); + } + if (minOutput != maxOutput) { + output = clamp(output, minOutput, maxOutput); + } + if (outputFilter != 0) { + output = lastOutput * outputFilter + output * (1 - outputFilter); + } + + lastOutput = output; + return output; +} + +/** + * Calculates the PID value using the last provided setpoint and actual valuess + * @return calculated output value for driving the actual to the target + */ +double MiniPID::getOutput() { return getOutput(lastActual, setpoint); } + +/** + * + * @param actual + * @return calculated output value for driving the actual to the target + */ +double MiniPID::getOutput(double actual) { return getOutput(actual, setpoint); } + +/** + * Resets the controller. this->erases the I term buildup, and removes D gain on + * the next loop. + */ +void MiniPID::reset() { + firstRun = true; + errorSum = 0; +} + +/**Set the maximum rate the output can increase per cycle. + * @param rate + */ +void MiniPID::setOutputRampRate(double rate) { outputRampRate = rate; } + +/** Set a limit on how far the setpoint can be from the current position + * <br>Can simplify tuning by helping tuning over a small range applies to a + * much larger range. + * <br>this->limits the reactivity of P term, and restricts impact of large D + * term + * during large setpoint adjustments. Increases lag and I term if range is too + * small. + * @param range + */ +void MiniPID::setSetpointRange(double range) { setpointRange = range; } + +/**Set a filter on the output to reduce sharp oscillations. <br> + * 0.1 is likely a sane starting value. Larger values P and D oscillations, but + * force larger I values. + * Uses an exponential rolling sum filter, according to a simple <br> + * <pre>output*(1-strength)*sum(0..n){output*strength^n}</pre> + * @param output valid between [0..1), meaning [current output only.. historical + * output only) + */ +void MiniPID::setOutputFilter(double strength) { + if (strength == 0 || bounded(strength, 0, 1)) { + outputFilter = strength; + } +} + +//************************************** +// Helper functions +//************************************** + +/** + * Forces a value into a specific range + * @param value input value + * @param min maximum returned value + * @param max minimum value in range + * @return Value if it's within provided range, min or max otherwise + */ +double MiniPID::clamp(double value, double min, double max) { + if (value > max) { + return max; + } + if (value < min) { + return min; + } + return value; +} + +/** + * Test if the value is within the min and max, inclusive + * @param value to test + * @param min Minimum value of range + * @param max Maximum value of range + * @return + */ +bool MiniPID::bounded(double value, double min, double max) { + return (min < value) && (value < max); +} + +/** + * To operate correctly, all PID parameters require the same sign, + * with that sign depending on the {@literal}reversed value + */ +void MiniPID::checkSigns() { + if (reversed) { // all values should be below zero + if (P > 0) + P *= -1; + if (I > 0) + I *= -1; + if (D > 0) + D *= -1; + if (F > 0) + F *= -1; + } else { // all values should be above zero + if (P < 0) + P *= -1; + if (I < 0) + I *= -1; + if (D < 0) + D *= -1; + if (F < 0) + F *= -1; + } +} diff --git a/SoftTrunk/src/MiniPID/MiniPID.h b/SoftTrunk/src/MiniPID/MiniPID.h new file mode 100644 index 0000000000000000000000000000000000000000..b6298a26f7791552791227ce163c1113df03adbb --- /dev/null +++ b/SoftTrunk/src/MiniPID/MiniPID.h @@ -0,0 +1,58 @@ +#ifndef MINIPID_H +#define MINIPID_H + +class MiniPID { +public: + MiniPID(double, double, double); + MiniPID(double, double, double, double); + void setP(double); + void setI(double); + void setD(double); + void setF(double); + void setPID(double, double, double); + void setPID(double, double, double, double); + void setMaxIOutput(double); + void setOutputLimits(double); + void setOutputLimits(double, double); + void setDirection(bool); + void setSetpoint(double); + void reset(); + void setOutputRampRate(double); + void setSetpointRange(double); + void setOutputFilter(double); + double getOutput(); + double getOutput(double); + double getOutput(double, double); + +private: + double clamp(double, double, double); + bool bounded(double, double, double); + void checkSigns(); + void init(); + double P; + double I; + double D; + double F; + + double maxIOutput; + double maxError; + double errorSum; + + double maxOutput; + double minOutput; + + double setpoint; + + double lastActual; + + bool firstRun; + bool reversed; + + double outputRampRate; + double lastOutput; + + double outputFilter; + + double setpointRange; +}; +#endif diff --git a/SoftTrunk/src/MiniPID/README.md b/SoftTrunk/src/MiniPID/README.md new file mode 100644 index 0000000000000000000000000000000000000000..9a36aa31248f3c19cee70d97248f35d8a0e52ef3 --- /dev/null +++ b/SoftTrunk/src/MiniPID/README.md @@ -0,0 +1,108 @@ +# miniPID +This is a small, fully self contained PID class designed to help provide simple, efficient tuning, and simple integration wherever any level of PID control might be needed. + +All code is contained in a single C++ class, and doesn't depend on any specific compiler or toolchain. However, example Arduino sketches are included. + +## Design Goals +- Provide all expected features of a quality PID loop. +- Allow for stability without extensive tuning. +- Be simple to integrate into projects without code restructuring +- Be easy to "chain", sending the output of one PID to the input of another. +- Be flexible enough that any provided functions can be used in isolation. +- Be simple enough to be used in "transient" or one-off control sequences + +## Features +##### PID Functions +True to the name, the main purpose of this class is PID control. + +##### Feed Forward rate setting. +Provides an "Expected output", helpful when doing velocity control systems. + +##### Setpoint Range +Force the PID system to cap the setpoint to a range near the current input values. This allows you to tune the PID in a smaller range, and have it perform sanely during large setpoint changes. + +##### Output ramping +Allows a maximum rate of change on the output, preventing jumps in output during setpoint changes + +##### Output Range +Adjustable min and maximum range, so the output can directly drive a variety of systems. + +##### Output Filtering +Helps smooth the output, preventing high-frequency oscillations. + +##### I term restriction +Allows you to specify a maximum output value the I term will generate. This allows active error correction, with minimal risk of I-term windup. + +##### Smart I term buildup +The I term and summed error will never increase if the system is already doing everything permitted to correct the system error. + +##### Simple API. +No need for lots of convoluted calculation functions, or asyncronous calculation modes. After configuration, `getOutput()` is probably the only function you need. + +## Usage +A bare bones PID system could look like this. + +``` cpp +MiniPID pid=MiniPID(1,0,0); +//set any other PID configuration options here. + +while(true){ + //get some sort of sensor value + //set some sort of target value + double output=pid.getOutput(sensor,target); + //do something with the output + delay(50); +} +``` +That's it. No fuss, no muss. A few lines of code and some basic tuning, and your PID system is in place. + +### Getting and setting outputs +There's several ways. In simple systems, the fastest is `output=pid.getOutput(sensor,target);`. This does all the calculations with the current values, applying any configuration, and returns the output. + +For more event-driven systems, you can split up the setting and getting, using `output=pid.getOutput(sensor);` and `pid.setSetpoint(target)`. + +If your outputs are to be disabled or driven by a different system, then you may want to use the `pid.reset()` method. That will set the PID controller to re-initialize the next time `getOutput()` is used. Because of this, `reset()` can be used when disabling PID control, when reenabling it, or at any point between. + +### Configuring the PID. +The most complex part of PID systems is the configuration. Tuning a PID process properly typically requires either significant calculation, significant trial and error, or both. + +This library is designed to produce "decent" PID results with minimal effort and time investment, by providing more extensive configuration options than most controllers. + +Note, PID systems work best when the calculations are performed at constant time intervals. This PID implimentation does not handle this, and assumes the primary loop or framework handles the precise timing details. + +#### `MiniPID(p,i,d)` +#### `MiniPID(p,i,d,f)` + +These create the basic PID, allowing for further configuration. Generally, you initialize the PID values here, but you can re-configure on the fly using the `setP(double P)`,`setI(double I)`,`setD(double D)`, and `setF(double F)` methods. + +Tuning PID systems is out of scope of this readme, but a good examples can be found all over the internet. + +#### `SetF(double F)` +Feed-Forward is a 4th system variable that is very helpful on systems with a target velocity, or other systems where an on-target system results in continous motion. Feed forward is not helpful on positional control systems, or other systems where being on target results in halted (or small cyclic) motion. + +Conceptually, Feed-forward defines a "best guess" as to what the system output should be for a given setpoint value. Feed forward does not consider what the system is _actually_ doing, and a system driven solely by feed-forward is actually an open-loop system. Mathematically, a feed-forward only system is equivilent to `output=setpoint*F`. + +In most cases, the F term can be calculated very simply by running an output at full speed, and measuring your sensor rate. The F term is then `max_output_value/max_sensor_rate`. + +For this class of systems, it's helpful to consider the F term the primary variable, and configured first. Using F in this way will result in a shorter time-to-target since you don't wait for error buildup (to aquire the I term). It's also simpler to tune and more stable since you don't have large P and D terms which will cause oscillation. The P, I, and D values then will then add minor corrections, operating on a much smaller system error. In this setup, P and I will generally correct for non-linearities in the system such as such as drag, inertia, and friction. D is helpful for providing recovery on sudden loading of the system, or quickly switching to a new setpoint. + +#### `setOutputLimits(double minimum,double maximum)` +#### `setOutputLimits(double output)` +Optional, but highly recommended to set. The set the output limits, and ensure the controller behaves when reaching the maximum output capabilities of your physical system. + +#### `setMaxIOutput(double maximum)` +Sets the maximum output generated by the I term inside the controller. This is independent of the `setOutputLimits` values. This can assist in reducing windup over large setpoint changes or stall conditions. + +#### `setDirection(boolean reversed)` +Reverses the output. Use this if the controller attempts to go the wrong way during operation. + +#### `reset()` +Resets the PID controller. This primarily clears the I and D terms clears the previous sensor state, and sets the target setpoint the the current position. + +This is useful if the output system's state may have changed since the last time the PID system output was applied. This is generally the case when the output system is in a manual control mode (such as a joystick), or was disabled and may have been physically moved. + +#### `setOutputRampRate(double rate)` +Set the maximum rate of change in a single calculation cycle. This is particularly useful for adding "inertial" to the system, preventing jerks during setpoint changes. + +#### `setOutputFilter(double strength)` +The output filter prevents sharp changes in the output, adding inertia and minimizing the effect of high frequency oscillations. This adds significant stability to systems with poor tunings, but at the cost of slower setpoint changes, disturbance rejection, and increased overshoot. diff --git a/SoftTrunk/src/MiniPID/examples/Basic/Basic.ino b/SoftTrunk/src/MiniPID/examples/Basic/Basic.ino new file mode 100644 index 0000000000000000000000000000000000000000..890d0ce333411bacf4c038e4991b300cf23a87b2 --- /dev/null +++ b/SoftTrunk/src/MiniPID/examples/Basic/Basic.ino @@ -0,0 +1,52 @@ +#include <Arduino.h> +#include <MiniPID.h> + +// Create a PID system. +// It's not very well tuned, but some sample values will get things started. +MiniPID pid=MiniPID(.1,.01,0); + +void setup(){ + pid.setOutputLimits(-100,100); + pid.setOutputRampRate(10); + // ... add any other configuration options for the PID here. + + // Plug a potentiometer into these Analog pins + // Rotate it to adjust the "target" your system will try to go to. + pinMode(A5,OUTPUT); + pinMode(A4,INPUT); + pinMode(A3,OUTPUT); +} + +double setpoint=0; +double sensor=0; + +void loop(){ + // This is simply an analogRead, but weighted against the last few readings. + // It helps prevent the potentiometer readings from jumping around + // due to "noise" on the circuit. + setpoint=setpoint*0.99+analogRead(A4)*.01; + + //Run through the PID calculations, and get the desired output values + double out=pid.getOutput(sensor,setpoint); + + // "Do something" with the output. + // A real world system would do something more interesting, such as turn a motor or servo. + // The sensor would then report back the "effect" of our output. + // For this example, we don't have a physical system, so we'll instead just add them together. + sensor=sensor+out; + + //Send some information about what's happening to our serial print. + Serial.print("Setpoint: "); + Serial.print((int)setpoint); + Serial.print("\tInput? "); + Serial.print((int)sensor); + Serial.print("\tOutput: "); + Serial.print((int)out); + Serial.print("\tError: "); + Serial.print(setpoint-sensor); + Serial.println(""); + + // PIDs like a nice, consistent loop time. In general, the Blink Without Delay sketch + // is a much better way to keep time. Another good approached is ElapsedMillis or Metro. + delay(50); +} \ No newline at end of file diff --git a/SoftTrunk/src/MiniPID/keywords.txt b/SoftTrunk/src/MiniPID/keywords.txt new file mode 100644 index 0000000000000000000000000000000000000000..7e74a37c820d174140127837abf17d18658bb0cb --- /dev/null +++ b/SoftTrunk/src/MiniPID/keywords.txt @@ -0,0 +1,25 @@ +# Data Types KEYWORD1 +MiniPID KEYWORD2 + +#Methods KEYWORD2 + +setP KEYWORD2 +setI KEYWORD2 +setD KEYWORD2 +setF KEYWORD2 +setPID KEYWORD2 +setPID KEYWORD2 +setMaxIOutput KEYWORD2 +setOutputLimits KEYWORD2 +setOutputLimits KEYWORD2 +setDirection KEYWORD2 +setSetpoint KEYWORD2 +reset KEYWORD2 +setOutputRampRate KEYWORD2 +setSetpointRange KEYWORD2 +setOutputFilter KEYWORD2 +getOutput KEYWORD2 +getOutput KEYWORD2 +getOutput KEYWORD2 + +#Constants KEYWORD1 diff --git a/SoftTrunk/src/README.md b/SoftTrunk/src/README.md new file mode 100644 index 0000000000000000000000000000000000000000..91b3be835cd63e6ac3acf0d98c797d9bf9dedd12 --- /dev/null +++ b/SoftTrunk/src/README.md @@ -0,0 +1,5 @@ +# external libraries +## [MiniPID](https://github.com/tekdemo/MiniPID) +PID control library +## [matplotlib-cpp](https://github.com/lava/matplotlib-cpp) +plotting library diff --git a/SoftTrunk/src/arm.cpp b/SoftTrunk/src/arm.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7e8b079cf5357ce3c8089a107b00f3d6c42bf330 --- /dev/null +++ b/SoftTrunk/src/arm.cpp @@ -0,0 +1,44 @@ +// Copyright 2018 ... +#include "./arm.h" +#include <iostream> +#include <rbdl/rbdl.h> + +#ifndef RBDL_BUILD_ADDON_URDFREADER + #error "Error: RBDL addon URDFReader not enabled." +#endif + +#include <rbdl/addons/urdfreader/urdfreader.h> +using RigidBodyDynamics; +using RigidBodyDynamics::Math; + +using std::cout; + +Arm::Arm() { + create_rbdl_model(); + create_actual_model(); +} + +Arm::create_rbdl_model() { + Model* rbdl_model = new Model(); + if (!Addons::URDFReadFromFile("../urdf/robot.urdf", rbdl_model, false)) { + std::cerr << "Error loading model ../urdf/robot.urdf" << std::endl; + abort(); + } + rbdl_model->gravity = Vector3d(0., 0., -9.81); + // DoF is model->dof_count. If I can do ARM_ELEMENTS=rbdl_model->dof_count/6, it would be a much better solution. + // but since I still am not sure how to make variable length arrays in C++, will just define it separately as a constant. +} + +Arm::create_actual_model() { + for (int i=0; i < ARM_ELEMENTS; i++) { + double length = 0.2; + armElements[i]= new ArmElement(length); + } +} +Arm::setTargetForces() { + +} +Arm::actuate() + +ArmElement::ArmElement(double length):length(length) { +} diff --git a/SoftTrunk/src/example_forceController.cpp b/SoftTrunk/src/example_forceController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..50f8b3afa355083310c7ef151064ed2982dfb480 --- /dev/null +++ b/SoftTrunk/src/example_forceController.cpp @@ -0,0 +1,27 @@ +#include "forceController.h" +#include <chrono> +#include <iostream> +#include <thread> +/* +This program shows an example of using the forceController. +forceController is basically an interface to the Festo valves that also +implements a PID loop for each valve. +*/ +void wait() { std::this_thread::sleep_for(std::chrono::milliseconds(500)); } +int main() { + ForceController forceController(4); + /* + for (int i = 0; i < 20; i++) { + forceController.setSinglePressure(i % 4, 1200); + forceController.setSinglePressure((i + 2) % 4, 200); + wait(); + forceController.setSinglePressure(i % 4, 0); + forceController.setSinglePressure((i + 2) % 4, 0); + } + */ + forceController.setSinglePressure(2, 1500); + wait(); + forceController.disconnect(); + + return 1; +} diff --git a/SoftTrunk/src/example_optitrack.cpp b/SoftTrunk/src/example_optitrack.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1fc3553a9b081bb14172d01355419952f73c6b8b --- /dev/null +++ b/SoftTrunk/src/example_optitrack.cpp @@ -0,0 +1,211 @@ +/* + * Based on SimpleExample.cpp provided as example in NatNetLinux. + */ + +#include <signal.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include <arpa/inet.h> +#include <errno.h> +#include <netdb.h> +#include <netinet/in.h> +#include <sys/socket.h> +#include <sys/types.h> +#include <unistd.h> + +#include <NatNetLinux/CommandListener.h> +#include <NatNetLinux/FrameListener.h> +#include <NatNetLinux/NatNet.h> +#include <boost/program_options.hpp> +#include <iostream> +#include <time.h> + +class Globals { +public: + // Parameters read from the command line + static uint32_t localAddress; + static uint32_t serverAddress; + + // State of the main() thread. + static bool run; +}; +uint32_t Globals::localAddress = 0; +uint32_t Globals::serverAddress = 0; +bool Globals::run = false; + +// End the program gracefully. +void terminate(int) { + // Tell the main() thread to close. + Globals::run = false; +} + +// Set the global addresses from the command line. +void readOpts(int argc, char *argv[]) { + namespace po = boost::program_options; + + po::options_description desc( + "simple-example: demonstrates using NatNetLinux\nOptions"); + desc.add_options()("help", "Display help message")( + "local-addr,l", po::value<std::string>(), "Local IPv4 address")( + "server-addr,s", po::value<std::string>(), "Server IPv4 address"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (argc < 5 || vm.count("help") || !vm.count("local-addr") || + !vm.count("server-addr")) { + std::cout << desc << std::endl; + exit(1); + } + + Globals::localAddress = inet_addr(vm["local-addr"].as<std::string>().c_str()); + Globals::serverAddress = + inet_addr(vm["server-addr"].as<std::string>().c_str()); +} + +// This thread loop just prints frames as they arrive. +void printFrames(FrameListener &frameListener) { + bool valid; + MocapFrame frame; + Globals::run = true; + while (Globals::run) { + while (true) { + // Try to get a new frame from the listener. + MocapFrame frame(frameListener.pop(&valid).first); + // Quit if the listener has no more frames. + if (!valid) + break; + std::cout << frame << std::endl; + /* + MocapFrame is described in NatNet.h + frame.frameNum(): frame ID + frame.rigidBodies(): vector of "RigidBody"s + + RigidBody is defined in NatNet.h + rigidbody.id: integer ID + rigidbody.location: Point3f + rigidbody.orientation: Quaternion4f + */ + } + + // Sleep for a little while to simulate work :) + usleep(1000); + } +} + +// This thread loop collects inter-frame arrival statistics and prints a +// histogram at the end. You can plot the data by copying it to a file +// (say time.txt), and running gnuplot with the command: +// gnuplot> plot 'time.txt' using 1:2 title 'Time Stats' with bars +void timeStats(FrameListener &frameListener, const float diffMin_ms = 0.5, + const float diffMax_ms = 7.0, const int bins = 100) { + size_t hist[bins]; + float diff_ms; + int bin; + struct timespec current; + struct timespec prev; + struct timespec tmp; + + std::cout + << std::endl + << "Collecting inter-frame arrival statistics...press ctrl-c to finish." + << std::endl; + + memset(hist, 0x00, sizeof(hist)); + bool valid; + Globals::run = true; + while (Globals::run) { + while (true) { + // Try to get a new frame from the listener. + prev = current; + tmp = frameListener.pop(&valid).second; + // Quit if the listener has no more frames. + if (!valid) + break; + + current = tmp; + + diff_ms = std::abs((static_cast<float>(current.tv_sec) - + static_cast<float>(prev.tv_sec)) * + 1000.f + + (static_cast<float>(current.tv_nsec) - + static_cast<float>(prev.tv_nsec)) / + 1000000.f); + + bin = (diff_ms - diffMin_ms) / (diffMax_ms - diffMin_ms) * (bins + 1); + if (bin < 0) + bin = 0; + else if (bin >= bins) + bin = bins - 1; + + hist[bin] += 1; + } + + // Sleep for a little while to simulate work :) + usleep(1000); + } + + // Print the stats + std::cout << std::endl << std::endl; + std::cout << "# Time diff (ms), Count" << std::endl; + for (bin = 0; bin < bins; ++bin) + std::cout << diffMin_ms + (diffMax_ms - diffMin_ms) * (0.5f + bin) / bins + << ", " << hist[bin] << std::endl; +} + +int main(int argc, char *argv[]) { + // Version number of the NatNet protocol, as reported by the server. + unsigned char natNetMajor; + unsigned char natNetMinor; + + // Sockets + int sdCommand; + int sdData; + + // Catch ctrl-c and terminate gracefully. + signal(SIGINT, terminate); + + // Set addresses + readOpts(argc, argv); + // Use this socket address to send commands to the server. + struct sockaddr_in serverCommands = + NatNet::createAddress(Globals::serverAddress, NatNet::commandPort); + + // Create sockets + sdCommand = NatNet::createCommandSocket(Globals::localAddress); + sdData = NatNet::createDataSocket(Globals::localAddress); + + // Start the CommandListener in a new thread. + CommandListener commandListener(sdCommand); + commandListener.start(); + + // Send a ping packet to the server so that it sends us the NatNet version + // in its response to commandListener. + NatNetPacket ping = NatNetPacket::pingPacket(); + ping.send(sdCommand, serverCommands); + + // Wait here for ping response to give us the NatNet version. + commandListener.getNatNetVersion(natNetMajor, natNetMinor); + + // Start up a FrameListener in a new thread. + FrameListener frameListener(sdData, natNetMajor, natNetMinor); + frameListener.start(); + + // This infinite loop simulates a "worker" thread that reads the frame + // buffer each time through, and exits when ctrl-c is pressed. + printFrames(frameListener); + // timeStats(frameListener); + + // Wait for threads to finish. + frameListener.stop(); + commandListener.stop(); + frameListener.join(); + commandListener.join(); + + // Epilogue + close(sdData); + close(sdCommand); + return 0; +} diff --git a/SoftTrunk/src/example_sinusoidal.cpp b/SoftTrunk/src/example_sinusoidal.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f1f3e44b00f9a6e13f971c08397f01384841025b --- /dev/null +++ b/SoftTrunk/src/example_sinusoidal.cpp @@ -0,0 +1,25 @@ +// Copyright 2018 ... +#include "forceController.h" +#include <cmath> +#include <iostream> +#include <thread> +#define PI 3.141592 +/* +sends a sinusoidal wave to 4 actuators, and each phase is offset by PI/2. +*/ +void wait() { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } + +int sinusoid(double t) { return 500 + 400 * sin(t); } +int main() { + ForceController forceController(4); + + for (int i = 0; i < 10000; i++) { + for (int j = 0; j < 4; j++) { + forceController.setSinglePressure( + j, sinusoid(static_cast<double>(i) / 40 + PI * j / 2)); + } + wait(); + } + forceController.disconnect(); + return 1; +} diff --git a/SoftTrunk/src/forceController.cpp b/SoftTrunk/src/forceController.cpp new file mode 100644 index 0000000000000000000000000000000000000000..09a584b391b2ef3c2b1dfe186622904bc820a5e0 --- /dev/null +++ b/SoftTrunk/src/forceController.cpp @@ -0,0 +1,97 @@ +// Copyright 2018 ... +#include "forceController.h" +#include <iostream> +#include <thread> + +#define USE_PID true +#define PLOT true +/* + This is a class that acts as a PID controller for the Festo valves. + It sends out target pressures to the Festo valves. + Runs as a separate thread from the main code, since it continuously does PID + control. + */ +ForceController::ForceController(int DoF) : DoF(DoF) { + run = true; + if (!mpa.connect()) { + std::cout << "Failed to connect to MPA." << '\n'; + return; + } + // Ziegler-Nichols method + double Ku = 2.6; + double Tu = 0.14; + double KP = 1.0; // 0.6 * Ku; + double KI = 0.0; // KP/ (Tu / 2.0) * 0.002; + double KD = 0.0; // KP*Tu/2.0 / 0.002; + + for (int i = 0; i < 16; i++) { + commanded_pressures.push_back(0); + pid.push_back(MiniPID(KP, KI, KD)); + pid[i].setOutputLimits(20); + // setting a good output limit is important so as not oscillate + } + controller_thread = std::thread(&ForceController::controllerThread, this); +} + +void ForceController::setSinglePressure(int index, int pressure) { + // set pressure for a single valve. + if (index < 0 || index >= DoF) { + // wrong index + return; + } + commanded_pressures[index] = pressure; + return; +} + +void ForceController::controllerThread() { + std::vector<int> sensor_pressures(16); + std::vector<int> output_pressures(16); + for (int i = 0; i < 16; i++) { + output_pressures[i] = 0; + } + int i = 0; + int plot_cycles = 500; + while (run) { + i++; + mpa.get_all_pressures(&sensor_pressures); + for (int i = 0; i < DoF; i++) { + output_pressures[i] = + commanded_pressures[i] + + pid[i].getOutput(sensor_pressures[i], commanded_pressures[i]); + if (output_pressures[i] < 0) { + // goes haywire when it tries to write negative value + output_pressures[i] = 0; + } + } + if (USE_PID) { + mpa.set_all_pressures(output_pressures); + } else { + mpa.set_all_pressures(commanded_pressures); + } + if (PLOT) { + x.push_back(i); + pressure_log.push_back(sensor_pressures[0]); + commandpressure_log.push_back(commanded_pressures[0]); + } + } + + // turn off all valves + for (int i = 0; i < 16; i++) { + output_pressures[i] = 0; + } + mpa.set_all_pressures(output_pressures); + mpa.disconnect(); +} +void ForceController::disconnect() { + run = false; + controller_thread.join(); + if (PLOT) { + namespace plt = matplotlibcpp; + plt::figure_size(1200, 780); + plt::named_plot("measured pressure", x, pressure_log); + plt::named_plot("commanded pressure", x, commandpressure_log); + plt::legend(); + plt::save("./graph.png"); + std::cout << "graph output to ./graph.png" << '\n'; + } +} diff --git a/SoftTrunk/src/matplotlib-cpp/.gitignore b/SoftTrunk/src/matplotlib-cpp/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..7622be7961a803891b007a91bdb5f9e7fa51e25e --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/.gitignore @@ -0,0 +1,35 @@ +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Logfiles +*.tlog +*.log + +# Build +/examples/build/* diff --git a/SoftTrunk/src/matplotlib-cpp/LICENSE b/SoftTrunk/src/matplotlib-cpp/LICENSE new file mode 100644 index 0000000000000000000000000000000000000000..a67f2b52b130d0fbc34078ea3d27a5befe388120 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/LICENSE @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2014 Benno Evers + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/SoftTrunk/src/matplotlib-cpp/LICENSE.matplotlib b/SoftTrunk/src/matplotlib-cpp/LICENSE.matplotlib new file mode 100644 index 0000000000000000000000000000000000000000..1c1a66b2f6dfd348ad7b952537d719402e982f02 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/LICENSE.matplotlib @@ -0,0 +1,47 @@ +This library does not contain any files from the matplotlib project, nor +does it make any changes to it. On the other hand, the code contained herein +is perfectly useless without a separate installation of matplotlib. +I don't know enough about US copyright law to decide whether this implies +that this library "uses" or is "based on" matplotlib. +In any case, matplotlib comes with the following license: + +License agreement for matplotlib 1.4.3 +1. This LICENSE AGREEMENT is between the Matplotlib Development Team (“MDTâ€), + and the Individual or Organization (“Licenseeâ€) accessing and otherwise + using matplotlib software in source or binary form and its associated documentation. + +2. Subject to the terms and conditions of this License Agreement, MDT hereby grants + Licensee a nonexclusive, royalty-free, world-wide license to reproduce, analyze, + test, perform and/or display publicly, prepare derivative works, distribute, and + otherwise use matplotlib 1.4.3 alone or in any derivative version, provided, however, + that MDT’s License Agreement and MDT’s notice of copyright, i.e., + “Copyright (c) 2012-2013 Matplotlib Development Team; All Rights Reserved†are retained + in matplotlib 1.4.3 alone or in any derivative version prepared by Licensee. + +3. In the event Licensee prepares a derivative work that is based on or incorporates + matplotlib 1.4.3 or any part thereof, and wants to make the derivative work available + to others as provided herein, then Licensee hereby agrees to include in any such work a + brief summary of the changes made to matplotlib 1.4.3. + +4. MDT is making matplotlib 1.4.3 available to Licensee on an “AS IS†basis. MDT MAKES NO + REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED. BY WAY OF EXAMPLE, BUT NOT LIMITATION, + MDT MAKES NO AND DISCLAIMS ANY REPRESENTATION OR WARRANTY OF MERCHANTABILITY OR FITNESS + FOR ANY PARTICULAR PURPOSE OR THAT THE USE OF MATPLOTLIB 1.4.3 WILL NOT INFRINGE ANY + THIRD PARTY RIGHTS. + +5. MDT SHALL NOT BE LIABLE TO LICENSEE OR ANY OTHER USERS OF MATPLOTLIB 1.4.3 FOR ANY + INCIDENTAL, SPECIAL, OR CONSEQUENTIAL DAMAGES OR LOSS AS A RESULT OF MODIFYING, + DISTRIBUTING, OR OTHERWISE USING MATPLOTLIB 1.4.3, OR ANY DERIVATIVE THEREOF, + EVEN IF ADVISED OF THE POSSIBILITY THEREOF. + +6. This License Agreement will automatically terminate upon a material breach of + its terms and conditions. + +7. Nothing in this License Agreement shall be deemed to create any relationship of + agency, partnership, or joint venture between MDT and Licensee. This License + Agreement does not grant permission to use MDT trademarks or trade name in a + trademark sense to endorse or promote products or services of Licensee, or any + third party. + +8. By copying, installing or otherwise using matplotlib 1.4.3, Licensee agrees to be + bound by the terms and conditions of this License Agreement. diff --git a/SoftTrunk/src/matplotlib-cpp/Makefile b/SoftTrunk/src/matplotlib-cpp/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..e75d1343ab2ef970bb45c4316957b48fb9cad623 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/Makefile @@ -0,0 +1,22 @@ +examples: minimal basic modern animation nonblock xkcd + +minimal: examples/minimal.cpp matplotlibcpp.h + cd examples && g++ -DWITHOUT_NUMPY minimal.cpp -I/usr/include/python2.7 -lpython2.7 -o minimal -std=c++11 + +basic: examples/basic.cpp matplotlibcpp.h + cd examples && g++ basic.cpp -I/usr/include/python2.7 -lpython2.7 -o basic + +modern: examples/modern.cpp matplotlibcpp.h + cd examples && g++ modern.cpp -I/usr/include/python2.7 -lpython2.7 -o modern -std=c++11 + +animation: examples/animation.cpp matplotlibcpp.h + cd examples && g++ animation.cpp -I/usr/include/python2.7 -lpython2.7 -o animation -std=c++11 + +nonblock: examples/nonblock.cpp matplotlibcpp.h + cd examples && g++ nonblock.cpp -I/usr/include/python2.7 -lpython2.7 -o nonblock -std=c++11 + +xkcd: examples/xkcd.cpp matplotlibcpp.h + cd examples && g++ xkcd.cpp -I/usr/include/python2.7 -lpython2.7 -o xkcd -std=c++11 + +clean: + rm -f examples/{minimal,basic,modern,animation,nonblock,xkcd} diff --git a/SoftTrunk/src/matplotlib-cpp/README.md b/SoftTrunk/src/matplotlib-cpp/README.md new file mode 100644 index 0000000000000000000000000000000000000000..d57e5f5fdd6f09642e909e5071444cec45b267a9 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/README.md @@ -0,0 +1,201 @@ +matplotlib-cpp +============== + +Welcome to matplotlib-cpp, possibly the simplest C++ plotting library. +It is built to resemble the plotting API used by Matlab and matplotlib. + + + +Usage +----- +Complete minimal example: +```cpp +#include "matplotlibcpp.h" +namespace plt = matplotlibcpp; +int main() { + plt::plot({1,3,2,4}); + plt::show(); +} +``` + g++ minimal.cpp -std=c++11 -I/usr/include/python2.7 -lpython2.7 + +**Result:** + +![Minimal example](./examples/minimal.png) + +A more comprehensive example: +```cpp +#include "matplotlibcpp.h" +#include <cmath> + +namespace plt = matplotlibcpp; + +int main() +{ + // Prepare data. + int n = 5000; + std::vector<double> x(n), y(n), z(n), w(n,2); + for(int i=0; i<n; ++i) { + x.at(i) = i*i; + y.at(i) = sin(2*M_PI*i/360.0); + z.at(i) = log(i); + } + + // Set the size of output image = 1200x780 pixels + plt::figure_size(1200, 780); + // Plot line from given x and y data. Color is selected automatically. + plt::plot(x, y); + // Plot a red dashed line from given x and y data. + plt::plot(x, w,"r--"); + // Plot a line whose name will show up as "log(x)" in the legend. + plt::named_plot("log(x)", x, z); + + // Set x-axis to interval [0,1000000] + plt::xlim(0, 1000*1000); + // Enable legend. + plt::legend(); + // Save the image (file format is determined by the extension) + plt::save("./basic.png"); +} +``` + g++ basic.cpp -I/usr/include/python2.7 -lpython2.7 + +Result: ![Basic example](./examples/basic.png) + +matplotlib-cpp doesn't require C++11, but will enable some additional syntactic sugar when available: +```cpp +#include <cmath> +#include "matplotlibcpp.h" + +using namespace std; +namespace plt = matplotlibcpp; + +int main() +{ + // Prepare data. + int n = 5000; // number of data points + vector<double> x(n),y(n); + for(int i=0; i<n; ++i) { + double t = 2*M_PI*i/n; + x.at(i) = 16*sin(t)*sin(t)*sin(t); + y.at(i) = 13*cos(t) - 5*cos(2*t) - 2*cos(3*t) - cos(4*t); + } + + // plot() takes an arbitrary number of (x,y,format)-triples. + // x must be iterable (that is, anything providing begin(x) and end(x)), + // y must either be callable (providing operator() const) or iterable. + plt::plot(x, y, "r-", x, [](double d) { return 12.5+abs(sin(d)); }, "k-"); + + + // show plots + plt::show(); +} +``` + g++ modern.cpp -std=c++11 -I/usr/include/python2.7 -lpython + +Result: ![Modern example](./examples/modern.png) + +Or some *funny-looking xkcd-styled* example: +```cpp +#include "matplotlibcpp.h" +#include <vector> +#include <cmath> + +namespace plt = matplotlibcpp; + +int main() { + std::vector<double> t(1000); + std::vector<double> x(t.size()); + + for(size_t i = 0; i < t.size(); i++) { + t[i] = i / 100.0; + x[i] = sin(2.0 * M_PI * 1.0 * t[i]); + } + + plt::xkcd(); + plt::plot(t, x); + plt::title("AN ORDINARY SIN WAVE"); + plt::save("xkcd.png"); +} + +``` + g++ xkcd.cpp -std=c++11 -I/usr/include/python2.7 -lpython2.7 + +**Result:** + +![Minimal example](./examples/xkcd.png) + +Installation +------------ + +matplotlib-cpp works by wrapping the popular python plotting library matplotlib. (matplotlib.org) +This means you have to have a working python installation, including development headers. +On Ubuntu: + + sudo apt-get install python-matplotlib python-numpy python2.7-dev + +If, for some reason, you're unable to get a working installation of numpy on your system, +you can add the define `WITHOUT_NUMPY` to erase this dependency. + +The C++-part of the library consists of the single header file `matplotlibcpp.h` which can be placed +anywhere. + +Since a python interpreter is opened internally, it is necessary to link against `libpython2.7` in order to use +matplotlib-cpp. + +# CMake + +If you prefer to use CMake as build system, you will want to add something like this to your +CMakeLists.txt: +```cmake +find_package(PythonLibs 2.7) +target_include_directories(myproject PRIVATE ${PYTHON_INCLUDE_DIRS}) +target_link_libraries(myproject ${PYTHON_LIBRARIES}) +``` +# Python 3 + +This library supports both python2 and python3 (although the python3 support is probably far less tested, +so it is recommended to prefer python2.7). To switch the used python version, simply change +the compiler flags accordingly. + + g++ example.cpp -I/usr/include/python3.6 -lpython3.6 + +The same technique can be used for linking against a custom build of python + + g++ example.cpp -I/usr/local/include/fancy-python4 -L/usr/local/lib -lfancy-python4 + + +Why? +---- +I initially started this library during my diploma thesis. The usual approach of +writing data from the c++ algorithm to a file and afterwards parsing and plotting +it in python using matplotlib proved insufficient: Keeping the algorithm +and plotting code in sync requires a lot of effort when the C++ code frequently and substantially +changes. Additionally, the python yaml parser was not able to cope with files that +exceed a few hundred megabytes in size. + +Therefore, I was looking for a C++ plotting library that was extremely easy to use +and to add into an existing codebase, preferably header-only. When I found +none, I decided to write one myself, which is basically a C++ wrapper around +matplotlib. As you can see from the above examples, plotting data and saving it +to an image file can be done as few as two lines of code. + +The general approach of providing a simple C++ API for utilizing python code +was later generalized and extracted into a separate, more powerful +library in another project of mine, [wrappy](http://www.github.com/lava/wrappy). + + +Todo/Issues/Wishlist +-------------------- +* This library is not thread safe. Protect all concurrent access with a mutex. + Sadly, this is not easy to fix since it is not caused by the library itself but + by the python interpreter, which is itself not thread-safe. + +* It would be nice to have a more object-oriented design with a Plot class which would allow + multiple independent plots per program. + +* Right now, only a small subset of matplotlibs functionality is exposed. Stuff like xlabel()/ylabel() etc. should + be easy to add. + +* If you use Anaconda on Windows, you might need to set PYTHONHOME to Anaconda home directory and QT_QPA_PLATFORM_PLUGIN_PATH to %PYTHONHOME%Library/plugins/platforms. The latter is for especially when you get the error which says 'This application failed to start because it could not find or load the Qt platform plugin "windows" +in "".' diff --git a/SoftTrunk/src/matplotlib-cpp/contrib/CMakeLists.txt b/SoftTrunk/src/matplotlib-cpp/contrib/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..ba14b86dc2ae598e31305149bd57f9cb7ea8128a --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/contrib/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.1) +project (MatplotlibCPP_Test) + +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +include_directories(${PYTHONHOME}/include) +link_directories(${PYTHONHOME}/libs) + +add_definitions(-DMATPLOTLIBCPP_PYTHON_HEADER=Python.h) + +# message(STATUS "*** dump start cmake variables ***") +# get_cmake_property(_variableNames VARIABLES) +# foreach(_variableName ${_variableNames}) + # message(STATUS "${_variableName}=${${_variableName}}") +# endforeach() +# message(STATUS "*** dump end ***") + +add_executable(minimal ${CMAKE_CURRENT_SOURCE_DIR}/../examples/minimal.cpp) +add_executable(basic ${CMAKE_CURRENT_SOURCE_DIR}/../examples/basic.cpp) +add_executable(modern ${CMAKE_CURRENT_SOURCE_DIR}/../examples/modern.cpp) diff --git a/SoftTrunk/src/matplotlib-cpp/contrib/README.md b/SoftTrunk/src/matplotlib-cpp/contrib/README.md new file mode 100644 index 0000000000000000000000000000000000000000..efc0a50065ab97d53129d0cb946ad3f91e14127c --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/contrib/README.md @@ -0,0 +1,19 @@ +# contrib/ + +This folder contains contributions that may be useful to users of this library, but +have a too specialized audience to become part of the main tree. + +In particular, things in here will have a higher rate of bit-rot, since +contributors are not required to and may be unable to check whether their +changes break any of them. + +## Windows support + +### Configuring and Building Samples + +```cmd +> cd contrib +> WinBuild.cmd +``` + +The `WinBuild.cmd` will set up temporal ENV variables and build binaries in (matplotlib root)/examples with the Release configuration. diff --git a/SoftTrunk/src/matplotlib-cpp/contrib/WinBuild.cmd b/SoftTrunk/src/matplotlib-cpp/contrib/WinBuild.cmd new file mode 100644 index 0000000000000000000000000000000000000000..4e3b450ba528121b46539962316e4ca354160d66 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/contrib/WinBuild.cmd @@ -0,0 +1,46 @@ +@echo off +@setlocal EnableDelayedExpansion + +if NOT DEFINED MSVC_VERSION set MSVC_VERSION=14 +if NOT DEFINED CMAKE_CONFIG set CMAKE_CONFIG=Release +if NOT DEFINED PYTHONHOME set PYTHONHOME=C:/Users/%username%/Anaconda3 + +if "%MSVC_VERSION%"=="14" ( + if "%processor_architecture%" == "AMD64" ( + set CMAKE_GENERATOR=Visual Studio 14 2015 Win64 + ) else ( + set CMAKE_GENERATOR=Visual Studio 14 2015 + ) +) else if "%MSVC_VERSION%"=="12" ( + if "%processor_architecture%" == "AMD64" ( + set CMAKE_GENERATOR=Visual Studio 12 2013 Win64 + + ) else ( + set CMAKE_GENERATOR=Visual Studio 12 2013 + ) +) + +set batch_file=!VS%MSVC_VERSION%0COMNTOOLS!..\..\VC\vcvarsall.bat +call "%batch_file%" %processor_architecture% + +pushd .. +pushd examples +if NOT EXIST build mkdir build +pushd build + +cmake -G"!CMAKE_GENERATOR!" ^ + -DPYTHONHOME:STRING=%PYTHONHOME%^ + -DCMAKE_BUILD_TYPE:STRING=%CMAKE_CONFIG% ^ + %~dp0 +cmake --build . --config %CMAKE_CONFIG% + +pushd %CMAKE_CONFIG% +if not EXIST platforms mkdir platforms +if EXIST %PYTHONHOME%/Library/plugins/platforms/qwindows.dll ^ +cp %PYTHONHOME%/Library/plugins/platforms/qwindows.dll ./platforms/ +popd +REM move ./%CMAKE_CONFIG% ../ +popd +popd +popd +@endlocal diff --git a/SoftTrunk/src/matplotlib-cpp/examples/animation.cpp b/SoftTrunk/src/matplotlib-cpp/examples/animation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d97943003a4c9d72d636ab7ebcd482f6a99fdf3a --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/animation.cpp @@ -0,0 +1,36 @@ +#define _USE_MATH_DEFINES +#include <cmath> +#include "../matplotlibcpp.h" + +namespace plt = matplotlibcpp; + +int main() +{ + int n = 1000; + std::vector<double> x, y, z; + + for(int i=0; i<n; i++) { + x.push_back(i*i); + y.push_back(sin(2*M_PI*i/360.0)); + z.push_back(log(i)); + + if (i % 10 == 0) { + // Clear previous plot + plt::clf(); + // Plot line from given x and y data. Color is selected automatically. + plt::plot(x, y); + // Plot a line whose name will show up as "log(x)" in the legend. + plt::named_plot("log(x)", x, z); + + // Set x-axis to interval [0,1000000] + plt::xlim(0, n*n); + + // Add graph title + plt::title("Sample figure"); + // Enable legend. + plt::legend(); + // Display plot continuously + plt::pause(0.01); + } + } +} diff --git a/SoftTrunk/src/matplotlib-cpp/examples/animation.gif b/SoftTrunk/src/matplotlib-cpp/examples/animation.gif new file mode 100644 index 0000000000000000000000000000000000000000..ef8eb5ad30980a5cfa69ff94e128198fb07e9025 Binary files /dev/null and b/SoftTrunk/src/matplotlib-cpp/examples/animation.gif differ diff --git a/SoftTrunk/src/matplotlib-cpp/examples/basic.cpp b/SoftTrunk/src/matplotlib-cpp/examples/basic.cpp new file mode 100644 index 0000000000000000000000000000000000000000..152c2742f777e990201cb95e790cbe4e1ef00495 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/basic.cpp @@ -0,0 +1,39 @@ +#define _USE_MATH_DEFINES +#include <iostream> +#include <cmath> +#include "../matplotlibcpp.h" + +namespace plt = matplotlibcpp; + +int main() +{ + // Prepare data. + int n = 5000; + std::vector<double> x(n), y(n), z(n), w(n,2); + for(int i=0; i<n; ++i) { + x.at(i) = i*i; + y.at(i) = sin(2*M_PI*i/360.0); + z.at(i) = log(i); + } + + // Set the size of output image = 1200x780 pixels + plt::figure_size(1200, 780); + // Plot line from given x and y data. Color is selected automatically. + plt::plot(x, y); + // Plot a red dashed line from given x and y data. + plt::plot(x, w,"r--"); + // Plot a line whose name will show up as "log(x)" in the legend. + plt::named_plot("log(x)", x, z); + + // Set x-axis to interval [0,1000000] + plt::xlim(0, 1000*1000); + + // Add graph title + plt::title("Sample figure"); + // Enable legend. + plt::legend(); + // save figure + const char* filename = "./basic.png"; + std::cout << "Saving result to " << filename << std::endl;; + plt::save(filename); +} diff --git a/SoftTrunk/src/matplotlib-cpp/examples/basic.png b/SoftTrunk/src/matplotlib-cpp/examples/basic.png new file mode 100644 index 0000000000000000000000000000000000000000..4d87ff01a24d2368ac80b73e14587e9bd58f7bf9 Binary files /dev/null and b/SoftTrunk/src/matplotlib-cpp/examples/basic.png differ diff --git a/SoftTrunk/src/matplotlib-cpp/examples/fill_between.png b/SoftTrunk/src/matplotlib-cpp/examples/fill_between.png new file mode 100644 index 0000000000000000000000000000000000000000..a199423bdf0c0cea0010ffdfd0bf995395893683 Binary files /dev/null and b/SoftTrunk/src/matplotlib-cpp/examples/fill_between.png differ diff --git a/SoftTrunk/src/matplotlib-cpp/examples/fill_inbetween.cpp b/SoftTrunk/src/matplotlib-cpp/examples/fill_inbetween.cpp new file mode 100644 index 0000000000000000000000000000000000000000..788d0086d02476c9b0be1d0d403bd943f48d59d7 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/fill_inbetween.cpp @@ -0,0 +1,28 @@ +#define _USE_MATH_DEFINES +#include "../matplotlibcpp.h" +#include <cmath> +#include <iostream> + +using namespace std; +namespace plt = matplotlibcpp; + +int main() { + // Prepare data. + int n = 5000; + std::vector<double> x(n), y(n), z(n), w(n, 2); + for (int i = 0; i < n; ++i) { + x.at(i) = i * i; + y.at(i) = sin(2 * M_PI * i / 360.0); + z.at(i) = log(i); + } + + // Prepare keywords to pass to PolyCollection. See + // https://matplotlib.org/api/_as_gen/matplotlib.axes.Axes.fill_between.html + std::map<string, string> keywords; + keywords["alpha"] = "0.4"; + keywords["color"] = "grey"; + keywords["hatch"] = "-"; + + plt::fill_between(x, y, z, keywords); + plt::show(); +} diff --git a/SoftTrunk/src/matplotlib-cpp/examples/minimal.cpp b/SoftTrunk/src/matplotlib-cpp/examples/minimal.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fbe1e1cd1fb07dd087ca39ee7b00209c22ead9ea --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/minimal.cpp @@ -0,0 +1,8 @@ +#include "../matplotlibcpp.h" + +namespace plt = matplotlibcpp; + +int main() { + plt::plot({1,3,2,4}); + plt::show(); +} diff --git a/SoftTrunk/src/matplotlib-cpp/examples/minimal.png b/SoftTrunk/src/matplotlib-cpp/examples/minimal.png new file mode 100644 index 0000000000000000000000000000000000000000..0f6cf373fd52a213bf5fb7d12fb2195a2f26e3cc Binary files /dev/null and b/SoftTrunk/src/matplotlib-cpp/examples/minimal.png differ diff --git a/SoftTrunk/src/matplotlib-cpp/examples/modern.cpp b/SoftTrunk/src/matplotlib-cpp/examples/modern.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a8aa0c75db624839cfed965bb0520739b339191a --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/modern.cpp @@ -0,0 +1,30 @@ +#define _USE_MATH_DEFINES +#include <cmath> +#include "../matplotlibcpp.h" + +using namespace std; +namespace plt = matplotlibcpp; + +int main() +{ + // plot(y) - the x-coordinates are implicitly set to [0,1,...,n) + //plt::plot({1,2,3,4}); + + // Prepare data for parametric plot. + int n = 5000; // number of data points + vector<double> x(n),y(n); + for(int i=0; i<n; ++i) { + double t = 2*M_PI*i/n; + x.at(i) = 16*sin(t)*sin(t)*sin(t); + y.at(i) = 13*cos(t) - 5*cos(2*t) - 2*cos(3*t) - cos(4*t); + } + + // plot() takes an arbitrary number of (x,y,format)-triples. + // x must be iterable (that is, anything providing begin(x) and end(x)), + // y must either be callable (providing operator() const) or iterable. + plt::plot(x, y, "r-", x, [](double d) { return 12.5+abs(sin(d)); }, "k-"); + + + // show plots + plt::show(); +} diff --git a/SoftTrunk/src/matplotlib-cpp/examples/modern.png b/SoftTrunk/src/matplotlib-cpp/examples/modern.png new file mode 100644 index 0000000000000000000000000000000000000000..0c51555f0979ac59744a276d4cf315eaf8d66d25 Binary files /dev/null and b/SoftTrunk/src/matplotlib-cpp/examples/modern.png differ diff --git a/SoftTrunk/src/matplotlib-cpp/examples/nonblock.cpp b/SoftTrunk/src/matplotlib-cpp/examples/nonblock.cpp new file mode 100644 index 0000000000000000000000000000000000000000..327d96c77080ecdda191221692ffebc7f0ec4857 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/nonblock.cpp @@ -0,0 +1,46 @@ +#define _USE_MATH_DEFINES +#include <cmath> +#include "../matplotlibcpp.h" + +namespace plt = matplotlibcpp; + + +using namespace matplotlibcpp; +using namespace std; + +int main() +{ + // Prepare data. + int n = 5000; + std::vector<double> x(n), y(n), z(n), w(n,2); + for(int i=0; i<n; ++i) { + x.at(i) = i*i; + y.at(i) = sin(2*M_PI*i/360.0); + z.at(i) = log(i); + } + + // Plot line from given x and y data. Color is selected automatically. + plt::subplot(2,2,1); + plt::plot(x, y); + + // Plot a red dashed line from given x and y data. + plt::subplot(2,2,2); + plt::plot(x, w,"r--"); + + // Plot a line whose name will show up as "log(x)" in the legend. + plt::subplot(2,2,3); + plt::named_plot("log(x)", x, z); + + // Set x-axis to interval [0,1000000] + plt::xlim(0, 1000*1000); + + // Add graph title + plt::title("Sample figure"); + // Enable legend. + plt::legend(); + + plt::show(false); + + cout << "matplotlibcpp::show() is working in an non-blocking mode" << endl; + getchar(); +} diff --git a/SoftTrunk/src/matplotlib-cpp/examples/xkcd.cpp b/SoftTrunk/src/matplotlib-cpp/examples/xkcd.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9bf64548d19e73c5572924183dda7d4f04deb02f --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/examples/xkcd.cpp @@ -0,0 +1,21 @@ +#include "../matplotlibcpp.h" +#include <vector> +#include <cmath> + +namespace plt = matplotlibcpp; + +int main() { + std::vector<double> t(1000); + std::vector<double> x(t.size()); + + for(size_t i = 0; i < t.size(); i++) { + t[i] = i / 100.0; + x[i] = sin(2.0 * M_PI * 1.0 * t[i]); + } + + plt::xkcd(); + plt::plot(t, x); + plt::title("AN ORDINARY SIN WAVE"); + plt::show(); +} + diff --git a/SoftTrunk/src/matplotlib-cpp/examples/xkcd.png b/SoftTrunk/src/matplotlib-cpp/examples/xkcd.png new file mode 100644 index 0000000000000000000000000000000000000000..c285e3d84464cc0b82425cbb2992caf24010dacc Binary files /dev/null and b/SoftTrunk/src/matplotlib-cpp/examples/xkcd.png differ diff --git a/SoftTrunk/src/matplotlib-cpp/matplotlibcpp.h b/SoftTrunk/src/matplotlib-cpp/matplotlibcpp.h new file mode 100644 index 0000000000000000000000000000000000000000..6e351e825e1b0e20084339579ad81f0c5a1cefe2 --- /dev/null +++ b/SoftTrunk/src/matplotlib-cpp/matplotlibcpp.h @@ -0,0 +1,1185 @@ +#pragma once + +#include <vector> +#include <map> +#include <numeric> +#include <algorithm> +#include <stdexcept> +#include <iostream> +#include <stdint.h> // <cstdint> requires c++11 support + +#if __cplusplus > 199711L || _MSC_VER > 1800 +# include <functional> +#endif + +#include <Python.h> + +#define WITHOUT_NUMPY +#ifndef WITHOUT_NUMPY +# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +# include <numpy/arrayobject.h> +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +# define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { +namespace detail { + +static std::string s_backend; + +struct _interpreter { + PyObject *s_python_function_show; + PyObject *s_python_function_close; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_plot; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_subplot; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ion; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_grid; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_empty_tuple; + PyObject *s_python_function_stem; + PyObject *s_python_function_xkcd; + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. + http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + */ + + static _interpreter& get() { + static _interpreter ctx; + return ctx; + } + +private: + +#ifndef WITHOUT_NUMPY +# if PY_MAJOR_VERSION >= 3 + + void *import_numpy() { + import_array(); // initialize C-API + return NULL; + } + +# else + + void import_numpy() { + import_array(); // initialize C-API + } + +# endif +#endif + + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + +#ifndef WITHOUT_NUMPY + import_numpy(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if (!matplotlib) { throw std::runtime_error("Error loading module matplotlib!"); } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast<char*>("use"), const_cast<char*>("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_show = PyObject_GetAttrString(pymod, "show"); + s_python_function_close = PyObject_GetAttrString(pymod, "close"); + s_python_function_draw = PyObject_GetAttrString(pymod, "draw"); + s_python_function_pause = PyObject_GetAttrString(pymod, "pause"); + s_python_function_figure = PyObject_GetAttrString(pymod, "figure"); + s_python_function_plot = PyObject_GetAttrString(pymod, "plot"); + s_python_function_semilogx = PyObject_GetAttrString(pymod, "semilogx"); + s_python_function_semilogy = PyObject_GetAttrString(pymod, "semilogy"); + s_python_function_loglog = PyObject_GetAttrString(pymod, "loglog"); + s_python_function_fill_between = PyObject_GetAttrString(pymod, "fill_between"); + s_python_function_hist = PyObject_GetAttrString(pymod,"hist"); + s_python_function_subplot = PyObject_GetAttrString(pymod, "subplot"); + s_python_function_legend = PyObject_GetAttrString(pymod, "legend"); + s_python_function_ylim = PyObject_GetAttrString(pymod, "ylim"); + s_python_function_title = PyObject_GetAttrString(pymod, "title"); + s_python_function_axis = PyObject_GetAttrString(pymod, "axis"); + s_python_function_xlabel = PyObject_GetAttrString(pymod, "xlabel"); + s_python_function_ylabel = PyObject_GetAttrString(pymod, "ylabel"); + s_python_function_grid = PyObject_GetAttrString(pymod, "grid"); + s_python_function_xlim = PyObject_GetAttrString(pymod, "xlim"); + s_python_function_ion = PyObject_GetAttrString(pymod, "ion"); + s_python_function_save = PyObject_GetAttrString(pylabmod, "savefig"); + s_python_function_annotate = PyObject_GetAttrString(pymod,"annotate"); + s_python_function_clf = PyObject_GetAttrString(pymod, "clf"); + s_python_function_errorbar = PyObject_GetAttrString(pymod, "errorbar"); + s_python_function_tight_layout = PyObject_GetAttrString(pymod, "tight_layout"); + s_python_function_stem = PyObject_GetAttrString(pymod, "stem"); + s_python_function_xkcd = PyObject_GetAttrString(pymod, "xkcd"); + + if( !s_python_function_show + || !s_python_function_close + || !s_python_function_draw + || !s_python_function_pause + || !s_python_function_figure + || !s_python_function_plot + || !s_python_function_semilogx + || !s_python_function_semilogy + || !s_python_function_loglog + || !s_python_function_fill_between + || !s_python_function_subplot + || !s_python_function_legend + || !s_python_function_ylim + || !s_python_function_title + || !s_python_function_axis + || !s_python_function_xlabel + || !s_python_function_ylabel + || !s_python_function_grid + || !s_python_function_xlim + || !s_python_function_ion + || !s_python_function_save + || !s_python_function_clf + || !s_python_function_annotate + || !s_python_function_errorbar + || !s_python_function_errorbar + || !s_python_function_tight_layout + || !s_python_function_stem + || !s_python_function_xkcd + ) { throw std::runtime_error("Couldn't find required function!"); } + + if ( !PyFunction_Check(s_python_function_show) + || !PyFunction_Check(s_python_function_close) + || !PyFunction_Check(s_python_function_draw) + || !PyFunction_Check(s_python_function_pause) + || !PyFunction_Check(s_python_function_figure) + || !PyFunction_Check(s_python_function_plot) + || !PyFunction_Check(s_python_function_semilogx) + || !PyFunction_Check(s_python_function_semilogy) + || !PyFunction_Check(s_python_function_loglog) + || !PyFunction_Check(s_python_function_fill_between) + || !PyFunction_Check(s_python_function_subplot) + || !PyFunction_Check(s_python_function_legend) + || !PyFunction_Check(s_python_function_annotate) + || !PyFunction_Check(s_python_function_ylim) + || !PyFunction_Check(s_python_function_title) + || !PyFunction_Check(s_python_function_axis) + || !PyFunction_Check(s_python_function_xlabel) + || !PyFunction_Check(s_python_function_ylabel) + || !PyFunction_Check(s_python_function_grid) + || !PyFunction_Check(s_python_function_xlim) + || !PyFunction_Check(s_python_function_ion) + || !PyFunction_Check(s_python_function_save) + || !PyFunction_Check(s_python_function_clf) + || !PyFunction_Check(s_python_function_tight_layout) + || !PyFunction_Check(s_python_function_errorbar) + || !PyFunction_Check(s_python_function_stem) + || !PyFunction_Check(s_python_function_xkcd) + ) { throw std::runtime_error("Python object is unexpectedly not a PyFunction."); } + + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } +}; + +} // end namespace detail + +// must be called before the first regular call to matplotlib to have any effect +inline void backend(const std::string& name) +{ + detail::s_backend = name; +} + +inline bool annotate(std::string annotation, double x, double y) +{ + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +#ifndef WITHOUT_NUMPY +// Type selector for numpy array conversion +template <typename T> struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default +template <> struct select_npy_type<double> { const static NPY_TYPES type = NPY_DOUBLE; }; +template <> struct select_npy_type<float> { const static NPY_TYPES type = NPY_FLOAT; }; +template <> struct select_npy_type<bool> { const static NPY_TYPES type = NPY_BOOL; }; +template <> struct select_npy_type<int8_t> { const static NPY_TYPES type = NPY_INT8; }; +template <> struct select_npy_type<int16_t> { const static NPY_TYPES type = NPY_SHORT; }; +template <> struct select_npy_type<int32_t> { const static NPY_TYPES type = NPY_INT; }; +template <> struct select_npy_type<int64_t> { const static NPY_TYPES type = NPY_INT64; }; +template <> struct select_npy_type<uint8_t> { const static NPY_TYPES type = NPY_UINT8; }; +template <> struct select_npy_type<uint16_t> { const static NPY_TYPES type = NPY_USHORT; }; +template <> struct select_npy_type<uint32_t> { const static NPY_TYPES type = NPY_ULONG; }; +template <> struct select_npy_type<uint64_t> { const static NPY_TYPES type = NPY_UINT64; }; + +template<typename Numeric> +PyObject* get_array(const std::vector<Numeric>& v) +{ + detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work + NPY_TYPES type = select_npy_type<Numeric>::type; + if (type == NPY_NOTYPE) + { + std::vector<double> vd(v.size()); + npy_intp vsize = v.size(); + std::copy(v.begin(),v.end(),vd.begin()); + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, NPY_DOUBLE, (void*)(vd.data())); + return varray; + } + + npy_intp vsize = v.size(); + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; +} + +#else // fallback if we don't have numpy: copy every element of the given vector + +template<typename Numeric> +PyObject* get_array(const std::vector<Numeric>& v) +{ + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; +} + +#endif // WITHOUT_NUMPY + +template<typename Numeric> +bool plot(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords) +{ + assert(x.size() == y.size()); + + // using numpy arrays + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template<typename Numeric> +bool stem(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords) +{ + assert(x.size() == y.size()); + + // using numpy arrays + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map<std::string, std::string>::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_stem, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill_between(const std::vector<Numeric>& x, const std::vector<Numeric>& y1, const std::vector<Numeric>& y2, const std::map<std::string, std::string>& keywords) +{ + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + // using numpy arrays + PyObject* xarray = get_array(x); + PyObject* y1array = get_array(y1); + PyObject* y2array = get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map<std::string, std::string>::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool hist(const std::vector<Numeric>& y, long bins=10,std::string color="b", double alpha=1.0) +{ + + PyObject* yarray = get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool named_hist(std::string label,const std::vector<Numeric>& y, long bins=10, std::string color="b", double alpha=1.0) +{ + PyObject* yarray = get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template<typename NumericX, typename NumericY> +bool plot(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template<typename NumericX, typename NumericY> +bool stem(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_stem, plot_args); + + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template<typename NumericX, typename NumericY> +bool semilogx(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template<typename NumericX, typename NumericY> +bool semilogy(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template<typename NumericX, typename NumericY> +bool loglog(const std::vector<NumericX>& x, const std::vector<NumericY>& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template<typename NumericX, typename NumericY> +bool errorbar(const std::vector<NumericX> &x, const std::vector<NumericY> &y, const std::vector<NumericX> &yerr, const std::string &s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + PyObject* yerrarray = get_array(yerr); + + PyObject *kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *pystring = PyString_FromString(s.c_str()); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; +} + +template<typename Numeric> +bool named_plot(const std::string& name, const std::vector<Numeric>& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template<typename Numeric> +bool named_plot(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template<typename Numeric> +bool named_semilogx(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template<typename Numeric> +bool named_semilogy(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template<typename Numeric> +bool named_loglog(const std::string& name, const std::vector<Numeric>& x, const std::vector<Numeric>& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template<typename Numeric> +bool plot(const std::vector<Numeric>& y, const std::string& format = "") +{ + std::vector<Numeric> x(y.size()); + for(size_t i=0; i<x.size(); ++i) x.at(i) = i; + return plot(x,y,format); +} + +template<typename Numeric> +bool stem(const std::vector<Numeric>& y, const std::string& format = "") +{ + std::vector<Numeric> x(y.size()); + for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; + return stem(x, y, format); +} + +inline void figure() +{ + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to figure() failed."); + + Py_DECREF(res); +} + +inline void figure_size(size_t w, size_t h) +{ + const size_t dpi = 100; + PyObject* size = PyTuple_New(2); + PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); + PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "figsize", size); + PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if(!res) throw std::runtime_error("Call to figure_size() failed."); + Py_DECREF(res); +} + +inline void legend() +{ + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(res); +} + +template<typename Numeric> +void ylim(Numeric left, Numeric right) +{ + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template<typename Numeric> +void xlim(Numeric left, Numeric right) +{ + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline double* xlim() +{ + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; +} + + +inline double* ylim() +{ + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; +} + +inline void subplot(long nrows, long ncols, long plot_number) +{ + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void title(const std::string &titlestr) +{ + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_title, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void axis(const std::string &axisstr) +{ + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void xlabel(const std::string &str) +{ + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlabel, args); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void ylabel(const std::string &str) +{ + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylabel, args); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void grid(bool flag) +{ + PyObject* pyflag = flag ? Py_True : Py_False; + Py_INCREF(pyflag); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void show(const bool block = true) +{ + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + Py_DECREF(kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void close() +{ + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_close, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to close() failed."); + + Py_DECREF(res); +} + +inline void xkcd() { + PyObject* res; + PyObject *kwargs = PyDict_New(); + + res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if (!res) + throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void draw() +{ + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); +} + +template<typename Numeric> +inline void pause(Numeric interval) +{ + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void save(const std::string& filename) +{ + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); + if (!res) throw std::runtime_error("Call to save() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void clf() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_clf, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to clf() failed."); + + Py_DECREF(res); +} + + inline void ion() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_ion, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to ion() failed."); + + Py_DECREF(res); +} + +// Actually, is there any reason not to call this automatically for every plot? +inline void tight_layout() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); +} + +#if __cplusplus > 199711L || _MSC_VER > 1800 +// C++11-exclusive content starts here (variadic plot() and initializer list support) + +namespace detail { + +template<typename T> +using is_function = typename std::is_function<std::remove_pointer<std::remove_reference<T>>>::type; + +template<bool obj, typename T> +struct is_callable_impl; + +template<typename T> +struct is_callable_impl<false, T> +{ + typedef is_function<T> type; +}; // a non-object is callable iff it is a function + +template<typename T> +struct is_callable_impl<true, T> +{ + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template<typename U, U> struct Check; + + template<typename U> + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template<typename U> + static std::false_type test( Check<void(Fallback::*)(), &U::operator()>* ); + +public: + typedef decltype(test<Derived>(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; +}; // an object is callable iff it defines operator() + +template<typename T> +struct is_callable +{ + // dispatch to is_callable_impl<true, T> or is_callable_impl<false, T> depending on whether T is of class type or not + typedef typename is_callable_impl<std::is_class<T>::value, T>::type type; +}; + +template<typename IsYDataCallable> +struct plot_impl { }; + +template<> +struct plot_impl<std::false_type> +{ + template<typename IterableX, typename IterableY> + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } +}; + +template<> +struct plot_impl<std::true_type> +{ + template<typename Iterable, typename Callable> + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector<double> y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl<std::false_type>()(ticks,y,format); + } +}; + +} // end namespace detail + +// recursion stop for the above +template<typename... Args> +bool plot() { return true; } + +template<typename A, typename B, typename... Args> +bool plot(const A& a, const B& b, const std::string& format, Args... args) +{ + return detail::plot_impl<typename detail::is_callable<B>::type>()(a,b,format) && plot(args...); +} + +/* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ +inline bool plot(const std::vector<double>& x, const std::vector<double>& y, const std::string& format = "") { + return plot<double,double>(x,y,format); +} + +inline bool plot(const std::vector<double>& y, const std::string& format = "") { + return plot<double>(y,format); +} + +inline bool plot(const std::vector<double>& x, const std::vector<double>& y, const std::map<std::string, std::string>& keywords) { + return plot<double>(x,y,keywords); +} + +#endif + +} // end namespace matplotlibcpp diff --git a/SoftTrunk/src/mpa/mpa.cpp b/SoftTrunk/src/mpa/mpa.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c349ebc53f34174fa1049378a4c0207fd1b9a5cb --- /dev/null +++ b/SoftTrunk/src/mpa/mpa.cpp @@ -0,0 +1,110 @@ +#include "../../include/modbus/modbus-tcp.h" +#include "../../include/mpa/mpa.h" +#include <stdexcept> + + +namespace { + const int address_input_start = 45392; + const int address_output_start = 40001; + const int cpx_input_offset = 3; + const int cpx_output_offset = 2; + const int num_valves = 16; +} + +MPA::MPA(const char* node, const char* service) { + // Not connected. + connected_ = false; + + // Resize buffer space. + // Input buffer for each valve is of the format (actual, setpoint, diagnostic). + input_buffer_.resize(3 * num_valves); + output_buffer_.resize(num_valves); + + // Create Modbus context. + ctx_ = modbus_new_tcp_pi(node, service); +} + +MPA::~MPA() { + // Close if not yet closed. + if (connected_) { + disconnect(); + } + + // Deallocate Modbus context. + modbus_free(ctx_); +} + +bool MPA::connect() { + if (modbus_connect(ctx_) != -1) { + connected_ = true; + return true; + } + + return false; +} + +bool MPA::disconnect() { + if (connected_) { + modbus_close(ctx_); + connected_ = false; + return true; + } + + return false; +} + +void MPA::ensure_connection() const { + if (!connected_) { + throw std::runtime_error("Operation requires a connection."); + } +} + +int MPA::get_single_pressure(const int index) { + ensure_connection(); + const auto dest = &input_buffer_[index]; + const auto addr = address_input_start + cpx_input_offset + 3 * index; + + if (modbus_read_registers(ctx_, addr, 1, dest) == -1) { + throw std::runtime_error("Failed to read VPPM register."); + } + + return *dest; +} + +void MPA::set_single_pressure(const int index, const int pressure) { + ensure_connection(); + const auto addr = address_output_start + cpx_output_offset + index; + + if (modbus_write_register(ctx_, addr, pressure) == -1) { + throw std::runtime_error("Failed to write VPPM register."); + } +} + +void MPA::get_all_pressures(std::vector<int>* output) { + ensure_connection(); + const auto dest = &input_buffer_[0]; + const auto addr = address_input_start + cpx_input_offset; + + if (modbus_read_registers(ctx_, addr, num_valves * 3, dest) == -1) { + throw std::runtime_error("Failed to read VPPM registers."); + } + + // Only read the actual value and ignore setpoint/diagonstic. + for (auto i = 0; i < num_valves; i++) { + output->at(i) = input_buffer_[i * 3]; + } +} + +void MPA::set_all_pressures(const std::vector<int>& pressures) { + ensure_connection(); + const auto data = &output_buffer_[0]; + const auto addr = address_output_start + cpx_output_offset; + + for (auto i = 0; i < num_valves; i++) { + output_buffer_[i] = pressures.at(i); + } + + if (modbus_write_registers(ctx_, addr, num_valves, data) == -1) { + throw std::runtime_error("Failed to write VPPM registers."); + } +} diff --git a/SoftTrunk/src/test_valve.cpp b/SoftTrunk/src/test_valve.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6fe9f8c3b0582d94257cd96e3bbe31d1d6e567aa --- /dev/null +++ b/SoftTrunk/src/test_valve.cpp @@ -0,0 +1,115 @@ +#include "MiniPID.h" +#include "matplotlibcpp.h" +#include "mpa.h" +#include <chrono> +#include <iostream> +#include <thread> +#define USE_PID true // turn on/off the PID control implemention with MiniPID. +#define STEP_TEST false // send step signals to valve + +/* + Test program for actuating a single valve. + Can test out PID controller. + Outputs graph of pressure profile as output. + + example_forceController.cpp has similar function but uses the + forceController class, and can actuate multiple valves. + */ +namespace plt = matplotlibcpp; + +int step_func(int i) { + // step function to send to valve + if (i % 1500 < 750) { + return 300; + } else { + return 450; + } +} + +int main() { + // Create MPA controller. + MPA mpa("192.168.1.101", "502"); + unsigned int valveNum = 0; // test valves 0 to 15 + unsigned int commandPressure = 450; // tested 0 to 1000 mbar + unsigned int endPressure = 0; + + // Ziegler-Nichols method + double Ku = 2.6; + double Tu = 0.14; + double KP = 0.6 * Ku; + double KI = KP / (Tu / 2.0) * 0.002; + double KD = KP * Tu / 2.0 / 0.002; + + MiniPID pid(KP, KI, KD); // create PID controller + pid.setOutputLimits( + 50); // setting a good output limit is important so as not oscillate + + // Connect. + if (!mpa.connect()) { + std::cout << "Failed to connect to MPA." << std::endl; + return -1; + } + + // Set valve 0 to 1 bar. + mpa.set_single_pressure(valveNum, commandPressure); + int sensorvalue; + double output; + + int cycles = 1000; + std::vector<double> x(cycles), pressures(cycles), + commandpressures(cycles); // for logging pressure profile + for (int i = 0; i < cycles; i++) { + // Wait 1 ms. + // std::this_thread::sleep_for(std::chrono::millisecofalsends(1)); + + if (STEP_TEST) { + commandPressure = step_func(i); + } + commandpressures.at(i) = commandPressure; + // Read pressure of valve 0. + sensorvalue = mpa.get_single_pressure(valveNum); + x.at(i) = i; + pressures.at(i) = sensorvalue; + output = commandPressure + pid.getOutput(sensorvalue, commandPressure); + + if (USE_PID) { + mpa.set_single_pressure(valveNum, static_cast<int>(output)); + } else { + mpa.set_single_pressure(valveNum, commandPressure); + } + + if (i % 50 == 0) { + std::cout << "Valve " << valveNum << " sensor: " << sensorvalue + << " mbar\toutput: " << output << " mbar" << std::endl; + } + } + + // Set valve 0 to 0 bar (off). + mpa.set_single_pressure(valveNum, endPressure); + + // Wait 100 ms. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // Read pressure of valve 0. + std::cout << "Valve " << valveNum << " :" << mpa.get_single_pressure(valveNum) + << " mbar" << std::endl; + + // Disconnect. + mpa.disconnect(); + + plt::figure_size(1200, 780); + if (USE_PID) { + plt::title("P:" + std::to_string(KP) + ", I:" + std::to_string(KI) + + ", D:" + std::to_string(KD)); + } else { + plt::title("w/o PID"); + } + plt::named_plot("measured pressure", x, pressures); + plt::named_plot("commanded pressure", x, commandpressures); + // plt::ylim(800,1200); + plt::legend(); + plt::save("./graph.png"); + std::cout << "graph output to ./graph.png" << '\n'; + + return 0; +} diff --git a/SoftTrunk/urdf/.gitignore b/SoftTrunk/urdf/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..8b15f7e20f00628dc108706fe7f5ca03991f982b --- /dev/null +++ b/SoftTrunk/urdf/.gitignore @@ -0,0 +1 @@ +*.urdf diff --git a/SoftTrunk/urdf/README.md b/SoftTrunk/urdf/README.md new file mode 100644 index 0000000000000000000000000000000000000000..4ef8763e9eed13150522a11c9b5b21b2b011636e --- /dev/null +++ b/SoftTrunk/urdf/README.md @@ -0,0 +1,6 @@ +Holds URDF files that describe the robot. +Uses [XACRO](http://wiki.ros.org/xacro) for macros and such to make the URDF simpler. + +To generate a URDF file from XACRO file, run `rosrun xacro xacro --inorder -o robot.urdf robot.urdf.xacro`. + +View the model in Rviz with `roslaunch rviz.launch`. diff --git a/SoftTrunk/urdf/create_urdf.sh b/SoftTrunk/urdf/create_urdf.sh new file mode 100755 index 0000000000000000000000000000000000000000..e5b6a6cd19e39b756f31e042c6c0624250411163 --- /dev/null +++ b/SoftTrunk/urdf/create_urdf.sh @@ -0,0 +1,2 @@ + #!/usr/bin/env bash +rosrun xacro xacro --inorder -o robot.urdf robot.urdf.xacro diff --git a/SoftTrunk/urdf/robot.urdf.xacro b/SoftTrunk/urdf/robot.urdf.xacro new file mode 100644 index 0000000000000000000000000000000000000000..b9f0761b111d219b8760e401a6a55ea4913684e4 --- /dev/null +++ b/SoftTrunk/urdf/robot.urdf.xacro @@ -0,0 +1,108 @@ +<?xml version="1.0"?> + +<!-- +convert this to a URDF with $./create_urdf.sh +--> + +<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot"> + <material name="white"> + <color rgba="1 1 1 1"/> + </material> + <xacro:macro name="ball_joint" params="id parent child"> + <!--creates a ball joint.--> + <joint name="${parent}_${id}a_joint" type="revolute"> + <parent link="${parent}"/> + <child link="${id}a"/> + <axis xyz="0 0 1"/> + <limit lower="-3.14" upper="3.14" effort="1000" velocity="1000"/> + </joint> + <link name="${id}a"> + <visual> + <geometry> + <sphere radius="${length/20}"/> + </geometry> + <material name="white"/> + </visual> + <inertial> + <mass value="1"/> + <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/> + </inertial> + </link> + <joint name="${id}a_${child}_joint" type="revolute"> + <parent link="${id}a"/> + <child link="${child}"/> + <axis xyz="1 0 0"/> + <limit lower="-3.14" upper="3.14" effort="1000" velocity="1000"/> + </joint> + </xacro:macro> + + <xacro:macro name="PCC" params="id parent child length"> + <!--creates a PCC piece--> + <xacro:ball_joint id="${parent}_${id}a_balljoint" parent="${parent}" child="${id}a"/> + <link name="${id}a"> + <inertial> + <mass value="1"/> + <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/> + </inertial> + </link> + <joint name="${id}a_${id}b_joint" type="prismatic"> + <parent link="${id}a"/> + <child link="${id}b"/> + <axis xyz="0 0 1"/> + <origin xyz="0 0 ${-length/2}"/> + <limit lower="0" upper="${length/2}" effort="1000" velocity="1000"/> + </joint> + <link name="${id}b"> + <visual> + <geometry> + <sphere radius="${length/10}"/> + </geometry> + <material name="white"/> + </visual> + <inertial> + <mass value="1000"/> + <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/> + </inertial> + </link> + <joint name="${id}b_${id}c_joint" type="prismatic"> + <parent link="${id}b"/> + <child link="${id}c"/> + <axis xyz="0 0 1"/> + <origin xyz="0 0 ${-length/2}"/> + <limit lower="0" upper="${length/2}" effort="1000" velocity="1000"/> + </joint> + <link name="${id}c"> + <inertial> + <mass value="1"/> + <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/> + </inertial> + </link> + <xacro:ball_joint id="${id}c_${child}_balljoint" parent="${id}c" child="${child}"/> + </xacro:macro> + + <link name="base_link"> + <visual> + <geometry> + <box size="0.05 0.05 0.05" /> + <material name="white"/> + </geometry> + </visual> + <inertial> + <mass value="1"/> + <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/> + </inertial> + </link> + <xacro:PCC id="0" parent="base_link" child="end" length="0.5"/> + <link name="end"> + <visual> + <geometry> + <box size="0.05 0.05 0.05" /> + <material name="white"/> + </geometry> + </visual> + <inertial> + <mass value="1"/> + <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/> + </inertial> + </link> +</robot> diff --git a/SoftTrunk/urdf/rviz.launch b/SoftTrunk/urdf/rviz.launch new file mode 100644 index 0000000000000000000000000000000000000000..71073c1717532a84825e940c783a92fa14bf944d --- /dev/null +++ b/SoftTrunk/urdf/rviz.launch @@ -0,0 +1,20 @@ +<launch> + <!-- + $ roslaunch rviz.launch +--> + <!-- Generate/Load robot description file --> + <param name="robot_description" command="cat ./robot.urdf" /> + + + <!-- Joint state publisher opens a GUI to manually input joint states --> + <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> + <param name="use_gui" value="true"/> + </node> + + <!-- Robot state publisher subscribe to joint states and publish "tf" transforms --> + <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> + + <!-- Rviz to vizualize robot --> + <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d './robot.rviz'"/> + +</launch> diff --git a/SoftTrunk/urdfreader_example/.gitignore b/SoftTrunk/urdfreader_example/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..ab78fd08fd890feb81ab380a72eaf60ea4c9f7db --- /dev/null +++ b/SoftTrunk/urdfreader_example/.gitignore @@ -0,0 +1,5 @@ +CMakeFiles/* +cmake_install.cmake +CMakeCache.txt +example_urdfreader +Makefile diff --git a/SoftTrunk/urdfreader_example/CMakeLists.txt b/SoftTrunk/urdfreader_example/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..40d8bca71206a42b4a8837b23fc48165063fa6db --- /dev/null +++ b/SoftTrunk/urdfreader_example/CMakeLists.txt @@ -0,0 +1,23 @@ +PROJECT (RBDLEXAMPLE CXX) + +CMAKE_MINIMUM_REQUIRED(VERSION 3.0) + +# We need to add the project source path to the CMake module path so that +# the FindRBDL.cmake script can be found. +LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR} ) + +# Search for the RBDL include directory and library +FIND_PACKAGE (RBDL COMPONENTS URDFReader REQUIRED) +FIND_PACKAGE (Eigen3 REQUIRED) + +# Add the include directory to the include paths +INCLUDE_DIRECTORIES ( ${RBDL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) + +# Create an executable +ADD_EXECUTABLE (example_urdfreader example_urdfreader.cc) + +# And link the library against the executable +TARGET_LINK_LIBRARIES ( example_urdfreader + ${RBDL_LIBRARY} + ${RBDL_URDFReader_LIBRARY} + ) diff --git a/SoftTrunk/urdfreader_example/FindEigen3.cmake b/SoftTrunk/urdfreader_example/FindEigen3.cmake new file mode 100644 index 0000000000000000000000000000000000000000..66ffe8e1e5b462e565582a0930291ae0f54cf00b --- /dev/null +++ b/SoftTrunk/urdfreader_example/FindEigen3.cmake @@ -0,0 +1,80 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org> +# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr> +# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) diff --git a/SoftTrunk/urdfreader_example/FindRBDL.cmake b/SoftTrunk/urdfreader_example/FindRBDL.cmake new file mode 100644 index 0000000000000000000000000000000000000000..6804b119798d1b168d53d999fe031cb7c11f8418 --- /dev/null +++ b/SoftTrunk/urdfreader_example/FindRBDL.cmake @@ -0,0 +1,126 @@ +# Searches for RBDL includes and library files, including Addons. +# +# Sets the variables +# RBDL_FOUND +# RBDL_INCLUDE_DIR +# RBDL_LIBRARY +# +# You can use the following components: +# LuaModel +# URDFReader +# and then link to them e.g. using RBDL_LuaModel_LIBRARY. + +SET (RBDL_FOUND FALSE) +SET (RBDL_LuaModel_FOUND FALSE) +SET (RBDL_URDFReader_FOUND FALSE) + +FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LIBRARY NAMES rbdl + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH}/lib + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_LuaModel_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_LuaModel_LIBRARY NAMES rbdl_luamodel + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +FIND_PATH (RBDL_URDFReader_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h + HINTS + $ENV{HOME}/local/include + $ENV{RBDL_PATH}/src + $ENV{RBDL_PATH}/include + $ENV{RBDL_INCLUDE_PATH} + /usr/local/include + /usr/include + ) + +FIND_LIBRARY (RBDL_URDFReader_LIBRARY NAMES rbdl_urdfreader + PATHS + $ENV{HOME}/local/lib + $ENV{HOME}/local/lib/x86_64-linux-gnu + $ENV{RBDL_PATH} + $ENV{RBDL_LIBRARY_PATH} + /usr/local/lib + /usr/local/lib/x86_64-linux-gnu + /usr/lib + /usr/lib/x86_64-linux-gnu + ) + +IF (NOT RBDL_LIBRARY) + MESSAGE (ERROR "Could not find RBDL") +ENDIF (NOT RBDL_LIBRARY) + +IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + SET (RBDL_FOUND TRUE) +ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY) + +IF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + SET (RBDL_LuaModel_FOUND TRUE) +ENDIF (RBDL_LuaModel_INCLUDE_DIR AND RBDL_LuaModel_LIBRARY) + +IF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + SET (RBDL_URDFReader_FOUND TRUE) +ENDIF (RBDL_URDFReader_INCLUDE_DIR AND RBDL_URDFReader_LIBRARY) + +IF (RBDL_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + + foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} ) + IF (RBDL_${COMPONENT}_FOUND) + IF (NOT RBDL_FIND_QUIETLY) + MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}") + ENDIF (NOT RBDL_FIND_QUIETLY) + ELSE (RBDL_${COMPONENT}_FOUND) + MESSAGE(SEND_ERROR "Could not find RBDL ${COMPONENT}") + ENDIF (RBDL_${COMPONENT}_FOUND) + endforeach ( COMPONENT ) +ELSE (RBDL_FOUND) + IF (RBDL_FIND_REQUIRED) + MESSAGE(SEND_ERROR "Could not find RBDL") + ENDIF (RBDL_FIND_REQUIRED) +ENDIF (RBDL_FOUND) + +MARK_AS_ADVANCED ( + RBDL_INCLUDE_DIR + RBDL_LIBRARY + RBDL_LuaModel_INCLUDE_DIR + RBDL_LuaModel_LIBRARY + RBDL_URDFReader_INCLUDE_DIR + RBDL_URDFReader_LIBRARY + ) diff --git a/SoftTrunk/urdfreader_example/README.md b/SoftTrunk/urdfreader_example/README.md new file mode 100644 index 0000000000000000000000000000000000000000..18c79b37c2555f3ec821529c00fe4cb4afd237f8 --- /dev/null +++ b/SoftTrunk/urdfreader_example/README.md @@ -0,0 +1,5 @@ +This is basically a copy(just changed where it reads the URDF from) of RBDL's sample code for reading URDFs into a model and doing forward dynamics on it. The original can be found under /examples/urdfreader in the source code. + +* note: In the README for the URDF addon, it says that this feature hasn't properly been tested yet. Use with caution. +* the base link's name should be `base_link` to be read by the URDF reader. +* the links must have some mass and inertia for calculations to work. diff --git a/SoftTrunk/urdfreader_example/example_urdfreader.cc b/SoftTrunk/urdfreader_example/example_urdfreader.cc new file mode 100644 index 0000000000000000000000000000000000000000..87f84f416d7d91b40195f4e671a49ab78cd831fe --- /dev/null +++ b/SoftTrunk/urdfreader_example/example_urdfreader.cc @@ -0,0 +1,46 @@ +/* + * RBDL - Rigid Body Dynamics Library + * Copyright (c) 2011-2016 Martin Felis <martin@fysx.org> + * + * Licensed under the zlib license. See LICENSE for more details. + */ + +#include <iostream> + +#include <rbdl/rbdl.h> + +#ifndef RBDL_BUILD_ADDON_URDFREADER + #error "Error: RBDL addon URDFReader not enabled." +#endif + +#include <rbdl/addons/urdfreader/urdfreader.h> + +using namespace RigidBodyDynamics; +using namespace RigidBodyDynamics::Math; + +int main (int argc, char* argv[]) { + rbdl_check_api_version (RBDL_API_VERSION); + + Model* model = new Model(); + + if (!Addons::URDFReadFromFile ("../urdf/robot.urdf", model, false)) { + std::cerr << "Error loading model ../urdf/robot.urdf" << std::endl; + abort(); + } + + model->gravity = Vector3d (0., 0., -9.81); + + VectorNd Q = VectorNd::Zero (model->dof_count); + VectorNd QDot = VectorNd::Zero (model->dof_count); + VectorNd Tau = VectorNd::Zero (model->dof_count); + VectorNd QDDot = VectorNd::Zero (model->dof_count); + + ForwardDynamics (*model, Q, QDot, Tau, QDDot); + + std::cout << Q.transpose() << std::endl; + std::cout << QDDot.transpose() << std::endl; + + delete model; + + return 0; +}