From 35470df99eee7dfe88c2ea39497980e5b580758a Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Fri, 26 Jul 2024 19:03:36 -0400 Subject: [PATCH] latest pushes --- .../CMakeLists.txt | 2 +- .../package.xml | 4 +- .../setup.py | 2 +- .../CHANGELOG.rst | 82 ----------------- .../CMakeLists.txt | 60 ------------- .../motion_module.h | 87 ------------------- .../sensor_module.h | 57 ------------ .../singleton.h | 65 -------------- .../package.xml | 52 ----------- .../CMakeLists.txt | 2 +- .../package.xml | 4 +- .../setup.py | 2 +- .../CMakeLists.txt | 60 +++++++++++++ .../package.xml | 46 ++++++++++ .../setup.py | 26 ++++++ .../src/PIDController.py | 50 +++++++++++ 16 files changed, 190 insertions(+), 411 deletions(-) delete mode 100755 humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst delete mode 100755 humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt delete mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h delete mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h delete mode 100755 humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h delete mode 100755 humanoid_robot_intelligence_control_system_framework_common/package.xml create mode 100644 humanoid_robot_intelligence_control_system_structure_sensors/CMakeLists.txt create mode 100644 humanoid_robot_intelligence_control_system_structure_sensors/package.xml create mode 100644 humanoid_robot_intelligence_control_system_structure_sensors/setup.py create mode 100644 humanoid_robot_intelligence_control_system_structure_sensors/src/PIDController.py diff --git a/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt b/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt index e7de26d..3314559 100644 --- a/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt @@ -13,7 +13,7 @@ # the License. cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_controller) +project(humanoid_robot_intelligence_control_system_external_sensors) if($ENV{ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED COMPONENTS diff --git a/humanoid_robot_intelligence_control_system_external_sensors/package.xml b/humanoid_robot_intelligence_control_system_external_sensors/package.xml index 0ab0b27..df5cf0c 100644 --- a/humanoid_robot_intelligence_control_system_external_sensors/package.xml +++ b/humanoid_robot_intelligence_control_system_external_sensors/package.xml @@ -16,10 +16,10 @@ the License. --> - humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_external_sensors 0.0.1 - This Package is for Detection Math + This Package is for External Sensors Apache 2.0 Ronaldson Bellande diff --git a/humanoid_robot_intelligence_control_system_external_sensors/setup.py b/humanoid_robot_intelligence_control_system_external_sensors/setup.py index cbd6630..e411a0a 100644 --- a/humanoid_robot_intelligence_control_system_external_sensors/setup.py +++ b/humanoid_robot_intelligence_control_system_external_sensors/setup.py @@ -19,7 +19,7 @@ from catkin_pkg.python_setup import generate_distutils_setup # fetch values from package.xml setup_args = generate_distutils_setup( scripts=['src/PIDController.py'], - packages=['humanoid_robot_intelligence_control_system_controller'], + packages=['humanoid_robot_intelligence_control_system_external_sensors'], package_dir={'': 'src'}, ) diff --git a/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst b/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst deleted file mode 100755 index 0af7c89..0000000 --- a/humanoid_robot_intelligence_control_system_framework_common/CHANGELOG.rst +++ /dev/null @@ -1,82 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package humanoid_robot_intelligence_control_system_framework_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.2 (2023-10-03) ------------------- -* Make it compatible for ROS1/ROS2 -* Fix bugs -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.3.1 (2023-09-27) ------------------- -* Starting from this point it under a new license -* Fix errors and Issues -* Rename Repository for a completely different purpose -* Make it compatible with ROS/ROS2 -* Upgrade version of all builds and make it more compatible -* Update package.xml and CMakeList.txt for to the latest versions -* Contributors & Maintainer: Ronaldson Bellande - -0.2.9 (2018-03-22) ------------------- -* none - -0.2.8 (2018-03-20) ------------------- -* tested for system dependencies -* Contributors: Pyo - -0.2.7 (2018-03-15) ------------------- -* change the License and package format to version 2 -* Contributors: Pyo - -0.2.6 (2017-08-09) ------------------- -* none - -0.2.5 (2017-06-09) ------------------- -* none - -0.2.4 (2017-06-07) ------------------- -* none - -0.2.3 (2017-05-23) ------------------- -* updated the cmake file for ros install -* Contributors: SCH - -0.2.2 (2017-04-24) ------------------- -* updated for other packages -* Contributors: Zerom, Kayman - -0.2.1 (2016-11-23) ------------------- -* Merge the changes and update -* - dependencies fixed. (Pull requests `#26 `_) -* - modified dependency problem. -* Contributors: Jay Song, Pyo, Zerom - -0.2.0 (2016-08-31) ------------------- -* updated CHANGLOG.rst for minor release -* rename ControlMode(CurrentControl -> TorqueControl) -* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_) -* Contributors: Zerom, Pyo - -0.1.1 (2016-08-18) ------------------- -* updated the package information - -0.1.0 (2016-08-12) ------------------- -* modified the package information for release -* Setting the license to BSD. -* add SensorState - add Singleton template -* Contributors: Jay Song, Zerom, Pyo diff --git a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt b/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt deleted file mode 100755 index bc1e839..0000000 --- a/humanoid_robot_intelligence_control_system_framework_common/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -cmake_minimum_required(VERSION 3.8) -project(humanoid_robot_intelligence_control_system_framework_common) - - -if($ENV{ROS_VERSION} EQUAL 1) - find_package(catkin REQUIRED COMPONENTS - roscpp - humanoid_robot_intelligence_control_system_device - ) -else() - find_package(ament_cmake REQUIRED) -endif() - - -if($ENV{ROS_VERSION} EQUAL 1) - catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - humanoid_robot_intelligence_control_system_device - ) -endif() - - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} - include/${PROJECT_NAME}/motion_module.h - include/${PROJECT_NAME}/sensor_module.h - include/${PROJECT_NAME}/singleton.h -) -set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) - -install(TARGETS humanoid_robot_intelligence_control_system_framework_common - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h deleted file mode 100755 index 9df7326..0000000 --- a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/motion_module.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* - * motion_module.h - * - * Created on: 2016. 1. 15. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ - - -#include -#include - -#include "singleton.h" -#include "humanoid_robot_intelligence_control_system_device/robot.h" -#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" - -namespace humanoid_robot_intelligence_control_system_framework -{ - -enum ControlMode -{ - PositionControl, - VelocityControl, - TorqueControl -}; - -class MotionModule -{ -protected: - bool enable_; - std::string module_name_; - ControlMode control_mode_; - -public: - std::map result_; - - virtual ~MotionModule() { } - - std::string getModuleName() { return module_name_; } - ControlMode getControlMode() { return control_mode_; } - - void setModuleEnable(bool enable) - { - if(this->enable_ == enable) - return; - - this->enable_ = enable; - if(enable) - onModuleEnable(); - else - onModuleDisable(); - } - bool getModuleEnable() { return enable_; } - - virtual void onModuleEnable() { } - virtual void onModuleDisable() { } - - virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void process(std::map dxls, std::map sensors) = 0; - - virtual void stop() = 0; - virtual bool isRunning() = 0; -}; - - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_MOTION_MODULE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h deleted file mode 100755 index 23e6292..0000000 --- a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/sensor_module.h +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* - * sensor_module.h - * - * Created on: 2016. 3. 30. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ -#define ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ - - -#include -#include - -#include "singleton.h" -#include "humanoid_robot_intelligence_control_system_device/robot.h" -#include "humanoid_robot_intelligence_control_system_device/dynamixel.h" - -namespace humanoid_robot_intelligence_control_system_framework -{ - -class SensorModule -{ -protected: - std::string module_name_; - -public: - std::map result_; - - virtual ~SensorModule() { } - - std::string getModuleName() { return module_name_; } - - virtual void initialize(const int control_cycle_msec, Robot *robot) = 0; - virtual void process(std::map dxls, std::map sensors) = 0; -}; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_SENSOR_MODULE_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h b/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h deleted file mode 100755 index 5f1c4ab..0000000 --- a/humanoid_robot_intelligence_control_system_framework_common/include/humanoid_robot_intelligence_control_system_framework_common/singleton.h +++ /dev/null @@ -1,65 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* - * singleton.h - * - * Created on: 2016. 5. 17. - * Author: zerom - */ - -#ifndef ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ -#define ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ - - -namespace humanoid_robot_intelligence_control_system_framework -{ - -template -class Singleton -{ -private: - static T *unique_instance_; - -protected: - Singleton() { } - Singleton(Singleton const&) { } - Singleton& operator=(Singleton const&) { return *this; } - -public: - static T* getInstance() - { - if(unique_instance_ == NULL) - unique_instance_ = new T; - return unique_instance_; - } - - static void destroyInstance() - { - if(unique_instance_) - { - delete unique_instance_; - unique_instance_ = NULL; - } - } -}; - -template T* Singleton::unique_instance_ = NULL; - -} - - -#endif /* ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */ diff --git a/humanoid_robot_intelligence_control_system_framework_common/package.xml b/humanoid_robot_intelligence_control_system_framework_common/package.xml deleted file mode 100755 index 258c36e..0000000 --- a/humanoid_robot_intelligence_control_system_framework_common/package.xml +++ /dev/null @@ -1,52 +0,0 @@ - - - - - humanoid_robot_intelligence_control_system_framework_common - 0.3.2 - The package contains commonly used headers for the - humanoid_robot_intelligence_control_system_framework - Apache 2.0 - Ronaldson Bellande - - - catkin - - roscpp - humanoid_robot_intelligence_control_system_device - - roscpp - - humanoid_robot_intelligence_control_system_device - - roscpp - humanoid_robot_intelligence_control_system_device - - - ament_cmake - - roscpp - humanoid_robot_intelligence_control_system_device - - roscpp - - humanoid_robot_intelligence_control_system_device - - roscpp - humanoid_robot_intelligence_control_system_device - - diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt b/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt index e7de26d..ef0d055 100644 --- a/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt @@ -13,7 +13,7 @@ # the License. cmake_minimum_required(VERSION 3.0.2) -project(humanoid_robot_intelligence_control_system_controller) +project(humanoid_robot_intelligence_control_system_internal_sensors) if($ENV{ROS_VERSION} EQUAL 1) find_package(catkin REQUIRED COMPONENTS diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/package.xml b/humanoid_robot_intelligence_control_system_internal_sensors/package.xml index 0ab0b27..51a94fe 100644 --- a/humanoid_robot_intelligence_control_system_internal_sensors/package.xml +++ b/humanoid_robot_intelligence_control_system_internal_sensors/package.xml @@ -16,10 +16,10 @@ the License. --> - humanoid_robot_intelligence_control_system_controller + humanoid_robot_intelligence_control_system_internal_sensors 0.0.1 - This Package is for Detection Math + This Package is for Internal Sensors Apache 2.0 Ronaldson Bellande diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/setup.py b/humanoid_robot_intelligence_control_system_internal_sensors/setup.py index cbd6630..8f14186 100644 --- a/humanoid_robot_intelligence_control_system_internal_sensors/setup.py +++ b/humanoid_robot_intelligence_control_system_internal_sensors/setup.py @@ -19,7 +19,7 @@ from catkin_pkg.python_setup import generate_distutils_setup # fetch values from package.xml setup_args = generate_distutils_setup( scripts=['src/PIDController.py'], - packages=['humanoid_robot_intelligence_control_system_controller'], + packages=['humanoid_robot_intelligence_control_system_internal_sensors'], package_dir={'': 'src'}, ) diff --git a/humanoid_robot_intelligence_control_system_structure_sensors/CMakeLists.txt b/humanoid_robot_intelligence_control_system_structure_sensors/CMakeLists.txt new file mode 100644 index 0000000..9fb5e40 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_structure_sensors/CMakeLists.txt @@ -0,0 +1,60 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. + +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_robot_intelligence_control_system_structure_sensors) + +if($ENV{ROS_VERSION} EQUAL 1) + find_package(catkin REQUIRED COMPONENTS + rospy + ) + + catkin_package( + CATKIN_DEPENDS + rospy + ) + +else() + find_package(ament_cmake REQUIRED) + find_package(ament_cmake_python REQUIRED) + find_package(rclpy REQUIRED) + find_package(std_msgs REQUIRED) +endif() + +if($ENV{ROS_VERSION} EQUAL 1) + catkin_python_setup() + + catkin_install_python(PROGRAMS + src/PIDController.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + + install(DIRECTORY config launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +else() + ament_python_install_package(${PROJECT_NAME}) + + install(PROGRAMS + src/PIDController.py + DESTINATION lib/${PROJECT_NAME} + ) + + install(DIRECTORY config launch rviz + DESTINATION share/${PROJECT_NAME} + ) + + ament_package() +endif() diff --git a/humanoid_robot_intelligence_control_system_structure_sensors/package.xml b/humanoid_robot_intelligence_control_system_structure_sensors/package.xml new file mode 100644 index 0000000..7262a2a --- /dev/null +++ b/humanoid_robot_intelligence_control_system_structure_sensors/package.xml @@ -0,0 +1,46 @@ + + + + + humanoid_robot_intelligence_control_system_structure_sensors + 0.0.1 + + This Package is for Internal Sensors + + Apache 2.0 + Ronaldson Bellande + + catkin + ament_cmake + ament_cmake_python + + + rospy + + + rclpy + + + python3-opencv + python3-yaml + usb_cam + + + catkin + ament_cmake + + diff --git a/humanoid_robot_intelligence_control_system_structure_sensors/setup.py b/humanoid_robot_intelligence_control_system_structure_sensors/setup.py new file mode 100644 index 0000000..0c55c1c --- /dev/null +++ b/humanoid_robot_intelligence_control_system_structure_sensors/setup.py @@ -0,0 +1,26 @@ +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + scripts=['src/PIDController.py'], + packages=['humanoid_robot_intelligence_control_system_structure_sensors'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/humanoid_robot_intelligence_control_system_structure_sensors/src/PIDController.py b/humanoid_robot_intelligence_control_system_structure_sensors/src/PIDController.py new file mode 100644 index 0000000..0a87c58 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_structure_sensors/src/PIDController.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import time +from typing import Dict, List, Tuple + +class PIDController: + def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))): + self.kp, self.ki, self.kd = gains + self.name = name + self.output_limits = output_limits + self.reset() + + def reset(self): + self.last_error = 0 + self.integral = 0 + self.last_time = time.time() + + def compute(self, setpoint: float, process_variable: float) -> float: + current_time = time.time() + dt = current_time - self.last_time + error = setpoint - process_variable + + self.integral += error * dt + derivative = (error - self.last_error) / dt if dt > 0 else 0 + + output = self.kp * error + self.ki * self.integral + self.kd * derivative + output = max(min(output, self.output_limits[1]), self.output_limits[0]) + + self.last_error = error + self.last_time = current_time + + return output + + def update_config(self, gains: Tuple[float, float, float]): + self.kp, self.ki, self.kd = gains