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_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