diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt
index e7de26d..4ea5b7e 100644
--- a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt
+++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt
@@ -37,6 +37,7 @@ if($ENV{ROS_VERSION} EQUAL 1)
catkin_install_python(PROGRAMS
src/PIDController.py
+ src/BellandeController.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
@@ -49,6 +50,7 @@ else()
install(PROGRAMS
src/PIDController.py
+ src/BellandeController.py
DESTINATION lib/${PROJECT_NAME}
)
diff --git a/humanoid_robot_intelligence_control_system_controller/setup.py b/humanoid_robot_intelligence_control_system_controller/setup.py
index cbd6630..5269c92 100644
--- a/humanoid_robot_intelligence_control_system_controller/setup.py
+++ b/humanoid_robot_intelligence_control_system_controller/setup.py
@@ -18,7 +18,7 @@ from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
- scripts=['src/PIDController.py'],
+ scripts=['src/PIDController.py', "src/BellandeController.py"],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
diff --git a/humanoid_robot_intelligence_control_system_controller/src/BellandeController.py b/humanoid_robot_intelligence_control_system_controller/src/BellandeController.py
new file mode 100644
index 0000000..2496dd5
--- /dev/null
+++ b/humanoid_robot_intelligence_control_system_controller/src/BellandeController.py
@@ -0,0 +1,69 @@
+#!/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 os
+import time
+from typing import Tuple
+
+ros_version = os.getenv("ROS_VERSION")
+
+if ros_version == "1":
+ import rospy
+ from std_msgs.msg import Float64
+elif ros_version == "2":
+ import rclpy
+ from rclpy.node import Node
+ from std_msgs.msg import Float64
+else:
+ raise ValueError("Invalid ROS version. Set ROS_VERSION environment variable to '1' or '2'.")
+
+class BellandeController:
+ 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()
+
+ if ros_version == "1":
+ rospy.init_node('bellande_controller', anonymous=True)
+ self.subscriber = rospy.Subscriber('control_output', Float64, self.callback)
+ elif ros_version == "2":
+ rclpy.init()
+ self.node = Node('bellande_controller')
+ self.subscriber = self.node.create_subscription(Float64, 'control_output', self.callback, 10)
+
+ def reset(self):
+ self.last_error = 0
+ self.integral = 0
+ self.last_time = time.time()
+
+ def update_config(self, gains: Tuple[float, float, float]):
+ self.kp, self.ki, self.kd = gains
+
+ def callback(self, msg):
+ print(f"Received control output: {msg.data}")
+
+ def run(self):
+ if ros_version == "1":
+ rospy.spin()
+ elif ros_version == "2":
+ rclpy.spin(self.node)
+
+ def shutdown(self):
+ if ros_version == "2":
+ self.node.destroy_node()
+ rclpy.shutdown()
diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/src/cpu.py b/humanoid_robot_intelligence_control_system_internal_sensors/src/cpu.py
new file mode 100644
index 0000000..0a87c58
--- /dev/null
+++ b/humanoid_robot_intelligence_control_system_internal_sensors/src/cpu.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