diff --git a/.gitignore b/.gitignore
index 5c9c7c5..8c008b4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,3 +6,4 @@ log/
# Colcon metadata
.colcon/
+**/.vscode/
diff --git a/src/controller/controller/__init__.py b/src/controller/controller/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/controller/controller/angle_data.csv b/src/controller/controller/angle_data.csv
new file mode 100644
index 0000000..d20d74a
--- /dev/null
+++ b/src/controller/controller/angle_data.csv
@@ -0,0 +1,9 @@
+Angle,Value
+0, 220
+10, 260
+20, 315
+30, 370
+40, 480
+50, 610
+60, 770
+70, 1150
diff --git a/src/controller/controller/button_manager.py b/src/controller/controller/button_manager.py
new file mode 100755
index 0000000..9ee05b3
--- /dev/null
+++ b/src/controller/controller/button_manager.py
@@ -0,0 +1,322 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, Float32
+from mavros_msgs.srv import CommandBool, SetMode, CommandLong
+from mavros_msgs.msg import State
+from geometry_msgs.msg import Twist, Vector3
+from std_srvs.srv import SetBool
+from controller.helpers.button import Button
+from time import sleep
+from interfaces.srv import ToggleStepper, SetFloat, TogglePin
+
+
+
+VALID_MODES = [
+ 'GUIDED',
+ 'LAND',
+ 'STABILIZE',
+ 'LOITER'
+]
+
+TAKEOFF_HEIGHT = 1.0
+MAV_CMD_NAV_TAKEOFF = 22 # From Mavlink MAV_CMD: https://mavlink.io/en/messages/common.html#mav_commands)
+MAX_VELOCITY = 0.10
+
+
+class ButtonManagerNode(Node):
+ VALID_MODES = [
+ 'GUIDED',
+ 'LAND',
+ 'STABILIZE',
+ 'LOITER'
+ ]
+
+ TAKEOFF_HEIGHT = 1.0
+ MAV_CMD_NAV_TAKEOFF = 22 # From Mavlink MAV_CMD: https://mavlink.io/en/messages/common.html#mav_commands)
+ MAX_VELOCITY = 0.10
+
+ def __init__(self):
+ super().__init__('button_manager')
+
+ self.declare_parameter('hand', 'right')
+
+ self.hand = self.get_parameter('hand').get_parameter_value().string_value
+
+ print(self.hand)
+
+ if self.hand not in ['right', 'left']:
+ self.get_logger().error("Invalid hand parameter passed")
+ return
+ self.state_subscriber = self.create_subscription(State, '/mavros/state', self.state_callback, 10)
+ self.drone_state = False
+ self.drone_arm = True
+ self.drone_mode = None
+
+ if self.hand == 'right':
+ self.button1 = Button(topic='controller/right/but1', short_callback=self.guided_arm_takeoff)
+ self.button2 = Button(topic='controller/right/but2', short_callback=self.land_callback)
+ self.button3 = Button(topic='controller/right/but3', short_callback=self.recenter, long_callback=self.lock_zero, release_callback=self.unlock_zero)
+ self.button4 = Button(topic='controller/right/but4', short_callback=self.lock_axis, release_callback=self.unlock_axis)
+
+ self.get_logger().info('Right hand initialized')
+
+ # RIGHT CONTROLLER BUTTONS
+ self.but1_subscriber = self.create_subscription(Int32, 'controller/right/but1', self.button1.data_callback, 10)
+ self.but2_subscriber = self.create_subscription(Int32, 'controller/right/but2', self.button2.data_callback, 10)
+ self.but3_subscriber = self.create_subscription(Int32, 'controller/right/but3', self.button3.data_callback, 10)
+ self.but4_subscriber = self.create_subscription(Int32, 'controller/right/but4', self.button4.data_callback, 10)
+
+ else:
+ self.button1 = Button(topic='controller/left/but1', press_callback=self.vertical_up_movement_callback,release_callback=self.vertical_zero_movement_callback)
+ self.button2 = Button(topic='controller/left/but2', press_callback=self.vertical_down_movement_callback, release_callback=self.vertical_zero_movement_callback)
+ self.button3 = Button(topic='controller/left/but3', short_callback=self.bag_in_callback, long_callback=self.bag_in_hold_callback, release_callback=self.bag_stop_callback)
+ self.button4 = Button(topic='controller/left/but4', short_callback=self.bag_out_callback, long_callback=self.bag_out_hold_callback, release_callback=self.bag_stop_callback)
+
+ # LEFT CONTROLLER BUTTONS
+ self.but1_subscriber = self.create_subscription(Int32, 'controller/left/but1', self.button1.data_callback, 10)
+ self.but2_subscriber = self.create_subscription(Int32, 'controller/left/but2', self.button2.data_callback, 10)
+ self.but3_subscriber = self.create_subscription(Int32, 'controller/left/but3', self.button3.data_callback, 10)
+ self.but4_subscriber = self.create_subscription(Int32, 'controller/left/but4', self.button4.data_callback, 10)
+ self.pot_subscriber = self.create_subscription(Float32, '/left/pot_filter', self.pot_callback, 10)
+
+ self.get_logger().info('Left hand initialized')
+
+
+
+
+
+
+ self.mavros_clients = {
+ 'mode': self.create_client(SetMode, '/mavros/set_mode'),
+ 'arm': self.create_client(CommandBool, '/mavros/cmd/arming'),
+ 'takeoff': self.create_client(CommandLong, '/mavros/cmd/command'),
+ 'bag': self.create_client(ToggleStepper, '/rotate_stepper'),
+ 'bag_hold': self.create_client(ToggleStepper, '/toggle_stepper'),
+ 'move_vert': self.create_client(SetFloat, '/vert_vel'),
+ 'lock_axis': self.create_client(SetBool, '/lock_axis'),
+ 'lock_zero': self.create_client(SetBool, '/lock_zero'),
+ 'landing': self.create_client(SetBool, '/landing'),
+ 'camera': self.create_client(TogglePin, '/set_servo')
+ }
+
+ # for service_name, client in self.mavros_clients.items():
+ # while not client.wait_for_service(timeout_sec=1.0):
+ # self.get_logger().warn(f'Waiting for {service_name} service')
+
+ self.get_logger().info('All services online')
+
+ self.centering_clients = {
+ 'pot_right': self.create_client(SetBool, '/right/center_pot'),
+ 'pot_left': self.create_client(SetBool, '/left/center_pot'),
+ 'imu_right': self.create_client(SetBool, '/right/center_imu'),
+ 'imu_left': self.create_client(SetBool, '/left/center_imu')
+ }
+
+ def state_callback(self, msg):
+ self.drone_arm = msg.armed
+ self.drone_mode = msg.mode
+ if msg.system_status == 4:
+ self.drone_state = True
+ else:
+ self.drone_state = False
+ pass
+
+ def arm_takeoff_callback(self):
+ self.get_logger().info("arm and takeoff")
+ self.arm_drone()
+
+ def land_callback(self):
+ self._change_mode('LAND')
+ request = SetBool.Request()
+ request.data = True
+ self.mavros_clients['landing'].call_async(request)
+
+ def set_guided_callback(self):
+ self.get_logger().info("PRESSED")
+ self._change_mode('GUIDED')
+
+ def guided_arm_takeoff(self):
+ if self.drone_mode != 'GUIDED':
+ self._change_mode('GUIDED')
+ self.get_logger().info("Setting mode to guided")
+ elif not self.drone_arm:
+ self.arm_drone()
+ self.get_logger().info("Arming Drone")
+ elif not self.drone_state:
+ self.takeoff()
+ self.get_logger().info("Taking off")
+
+ def arm_callback(self):
+ self.arm_drone()
+
+ def takeoff_callback(self):
+ self.takeoff()
+
+ def vertical_up_movement_callback(self):
+ float_data_req = SetFloat.Request()
+ float_data_req.data = MAX_VELOCITY
+ future = self.mavros_clients['move_vert'].call_async(float_data_req)
+ self.get_logger().info("Positive vertical movement")
+
+ def vertical_zero_movement_callback(self):
+ float_data_req = SetFloat.Request()
+ float_data_req.data = float(0)
+ future = self.mavros_clients['move_vert'].call_async(float_data_req)
+ self.get_logger().info("Zero vertical movement")
+
+ def vertical_down_movement_callback(self):
+ float_data_req = SetFloat.Request()
+ float_data_req.data = -MAX_VELOCITY
+ future = self.mavros_clients['move_vert'].call_async(float_data_req)
+ self.get_logger().info("Negative vertical movement")
+
+
+ def bag_in_callback(self):
+ bag_in_req = ToggleStepper.Request()
+ bag_in_req.stepper_id = 1
+ bag_in_req.speed = float(1)
+ self.get_logger().info("sending bag in")
+ future = self.mavros_clients['bag'].call_async(bag_in_req)
+ try:
+ self.get_logger().info(f"bag_in result: {future.result()}")
+ except Exception as e:
+ self.get_logger().error('bag_in: Exception {e} occured')
+
+ def bag_in_hold_callback(self):
+ bag_in_req = ToggleStepper.Request()
+ bag_in_req.stepper_id = 1
+ bag_in_req.speed = float(1)
+ self.get_logger().info("sending bag in")
+ future = self.mavros_clients['bag_hold'].call_async(bag_in_req)
+ try:
+ self.get_logger().info(f"bag result: {future.result()}")
+ except Exception as e:
+ self.get_logger().error('bag: Exception {e} occured')
+
+ def bag_out_callback(self):
+ bag_out_req = ToggleStepper.Request()
+ bag_out_req.stepper_id = 1
+ bag_out_req.speed = float(-1)
+ future = self.mavros_clients['bag'].call_async(bag_out_req)
+ try:
+ self.get_logger().info(f"bag_in result: {future.result()}")
+ except Exception as e:
+ self.get_logger().error('bag_in: Exception {e} occured')
+
+ def bag_out_hold_callback(self):
+ bag_out_req = ToggleStepper.Request()
+ bag_out_req.stepper_id = 1
+ bag_out_req.speed = float(-1)
+ future = self.mavros_clients['bag_hold'].call_async(bag_out_req)
+ try:
+ self.get_logger().info(f"bag result: {future.result()}")
+ except Exception as e:
+ self.get_logger().error('bag: Exception {e} occured')
+
+ def bag_stop_callback(self):
+ bag_out_req = ToggleStepper.Request()
+ bag_out_req.stepper_id = 1
+ bag_out_req.speed = float(0)
+ future = self.mavros_clients['bag_hold'].call_async(bag_out_req)
+ try:
+ self.get_logger().info(f"bag result: {future.result()}")
+ except Exception as e:
+ self.get_logger().error('bag: Exception {e} occured')
+
+ def _change_mode(self, mode: str):
+ mode_upper = mode.upper()
+ self.get_logger().info("{mode}, {mode_upper}")
+ if mode_upper not in VALID_MODES:
+ self.get_logger().error(f"{mode.upper()} is not a valid mode")
+ return
+ mode_req = SetMode.Request(custom_mode=mode_upper)
+ future = self.mavros_clients['mode'].call_async(mode_req)
+ self.get_logger().info(f"Change mode to {mode_upper} result: {future.result()}")
+
+ def arm_drone(self):
+ arm_req = CommandBool.Request(value=True)
+ future = self.mavros_clients['arm'].call_async(arm_req)
+ self.get_logger().info(f"Arm drone result: {future.result()}")
+
+ # # rclpy.spin_until_future_complete(self, future)
+ # if future.exception():
+ # print('exception raised')
+ # elif future.result() is None:
+ # print('result returned none')
+ # else:
+ # print(f"{future.result()}")
+
+ # self.get_logger().info(f"{future.done()}")
+ # return future.done()
+
+ def takeoff(self):
+ takeoff_req = CommandLong.Request(
+ broadcast=False,
+ command=MAV_CMD_NAV_TAKEOFF,
+ confirmation=0,
+ param1=0.0,
+ param2=0.0,
+ param3=0.0,
+ param4=0.0,
+ param5=0.0,
+ param6=0.0,
+ param7=TAKEOFF_HEIGHT
+ )
+ future = self.mavros_clients['takeoff'].call_async(takeoff_req)
+ self.get_logger().info(f"Takeoff command result: {future.result()}")
+
+ def recenter(self):
+ request = SetBool.Request()
+ request.data = True
+ self.get_logger().info(f"Recentering")
+ for service_name, client in self.centering_clients.items():
+ client.call_async(request)
+
+ def lock_axis(self):
+ request = SetBool.Request()
+ request.data = True
+ self.mavros_clients['lock_axis'].call_async(request)
+
+ def unlock_axis(self):
+ request = SetBool.Request()
+ request.data = False
+ self.mavros_clients['lock_axis'].call_async(request)
+
+ def lock_zero(self):
+ request = SetBool.Request()
+ request.data = True
+ self.get_logger().info(f"Lock zero")
+ self.mavros_clients['lock_zero'].call_async(request)
+
+ def unlock_zero(self):
+ request = SetBool.Request()
+ request.data = False
+ self.get_logger().info(f"Unlock zero")
+ self.mavros_clients['lock_zero'].call_async(request)
+
+
+ def pot_callback(self, data):
+ angle = 0
+ value = data.data
+ if data.data > 35:
+ angle = 180
+ elif data.data > 30:
+ angle = 180 * (data.data - 30)/30
+ # self.get_logger().info(f"Moving camera to angle {angle}")
+ request = TogglePin.Request()
+ request.angle = int(angle)
+ request.pin = int(18)
+ self.mavros_clients['camera'].call_async(request)
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ button_manager = ButtonManagerNode()
+ rclpy.spin(button_manager)
+ button_manager.destroy_node()
+
+if __name__ == '__main__':
+ main()
+
+
diff --git a/src/controller/controller/controller_pub.py b/src/controller/controller/controller_pub.py
new file mode 100644
index 0000000..2312d0f
--- /dev/null
+++ b/src/controller/controller/controller_pub.py
@@ -0,0 +1,116 @@
+import time
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Int32 # Import Int32 message type
+from controller.helpers.serial_helper import SerialHelper
+
+VALID_MODES = [
+ 'GUIDED',
+ 'LAND',
+ 'STABILIZE',
+ 'LOITER'
+]
+
+
+
+class ControllerPubNode(Node):
+ def __init__(self):
+ super().__init__('controller_pub')
+
+ # Declare parameters for serial port and baud rate with default values
+ self.declare_parameter('serial_port', '/dev/ttyUSB0')
+ self.declare_parameter('baud_rate', 115200)
+ self.declare_parameter('hand', 'right') # Declare 'hand' parameter
+
+
+ # Get the parameters set at runtime (or use the defaults)
+ serial_port = self.get_parameter('serial_port').get_parameter_value().string_value
+ baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
+ self.hand = self.get_parameter('hand').get_parameter_value().string_value
+
+ # Validate 'hand' parameter
+ if self.hand not in ["right", "left"]:
+ self.get_logger().warn("Invalid 'hand' parameter! Must be 'right' or 'left'. Defaulting to 'right'.")
+ self.hand = "right"
+
+ # Initialize the SerialHelper with the parameters
+ self.ser = SerialHelper(serial_port, baud_rate)
+
+
+ # Create publishers
+ self.imu_publisher = self.create_publisher(Imu, 'imu/' + self.hand + '/data', 10)
+ self.pot_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/pot', 10)
+ self.but1_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/but1', 10)
+ self.but2_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/but2', 10)
+ self.but3_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/but3', 10)
+ self.but4_publisher = self.create_publisher(Int32, 'controller/' + self.hand + '/but4', 10)
+
+
+ # TODO: Subscribe to get state of drone for deconfliction
+
+ # TODO: Subscribe to get state of drone for deconfliction
+
+ # Start a timer to update IMU data every 50ms (20 Hz)
+ self.timer = self.create_timer(0.05, self.update)
+
+ def update(self):
+ try:
+ # Read and process serial data
+ line = self.ser.read_from_serial()
+ # print(line)
+ # TODO: Send data from ESP in dict format and convert it from its string representation
+ if line:
+ # print(line)
+ split = line.split(',')
+ if len(split) != 11:
+ return
+
+ # Convert data to float (accelerometer and gyroscope data)
+ accel_data = list(map(float, split[:3])) # accelx, accely, accelz
+ gyro_data = list(map(lambda x: float(x) * (3.141592653589793 / 180), split[3:6])) # Convert to radians/sec
+ pot_value = int(split[6]) # Potentiometer value (7th number)
+ but1_value = int(split[7]) # Button state (8th number)
+ but2_value = int(split[8])
+ but3_value = int(split[9])
+ but4_value = int(split[10])
+
+ # Create and populate IMU message
+ imu_msg = Imu()
+ imu_msg.linear_acceleration.x = accel_data[0]
+ imu_msg.linear_acceleration.y = accel_data[1]
+ imu_msg.linear_acceleration.z = accel_data[2]
+ imu_msg.angular_velocity.x = gyro_data[0]
+ imu_msg.angular_velocity.y = gyro_data[1]
+ imu_msg.angular_velocity.z = gyro_data[2]
+ self.imu_publisher.publish(imu_msg)
+
+ # Create and publish Potentiometer message
+ pot_msg = Int32()
+ pot_msg.data = pot_value
+ self.pot_publisher.publish(pot_msg)
+
+ # Create and publish Button message
+ self.but1_publisher.publish(Int32(data=but1_value))
+ self.but2_publisher.publish(Int32(data=but2_value))
+ self.but3_publisher.publish(Int32(data=but3_value))
+ self.but4_publisher.publish(Int32(data=but4_value))
+
+
+ except Exception as e:
+ self.get_logger().error(f"Error in update: {e}")
+
+
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ controller_pub = ControllerPubNode()
+ rclpy.spin(controller_pub)
+ controller_pub.destroy_node()
+ # rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/controller/controller/drone_movement.py b/src/controller/controller/drone_movement.py
new file mode 100644
index 0000000..ec3826d
--- /dev/null
+++ b/src/controller/controller/drone_movement.py
@@ -0,0 +1,116 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float32
+from std_srvs.srv import SetBool
+from mavros_msgs.msg import State
+from interfaces.srv import SetFloat
+import time
+import threading
+
+class DroneMovement(Node):
+ def __init__(self):
+ super().__init__('drone_movement')
+
+ # Subscribe to pot topic
+ self.debug_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+ self.pub = self.create_publisher(Twist, '/mavros/setpoint_velocity/cmd_vel_unstamped', 10)
+ self.hor_vel_sub = self.create_subscription(Twist, '/right/cmd_vel_hor', self.hor_vel_callback, 10)
+ self.yaw_vel_sub = self.create_subscription(Twist, '/left/cmd_vel_hor', self.yaw_vel_callback, 10)
+ self.state_sub = self.create_subscription(State, '/mavros/state', self.state_callback, 10)
+ # self.vert_vel_sub = self.create_subscription(Float32, '/left/cmd_vel_vert', self.vert_vel_callback, 10)
+ self.vert_srv = self.create_service(SetFloat, '/vert_vel', self.vert_vel_srv_callback)
+ self.lock_srv = self.create_service(SetBool, '/lock_axis', self.lock_axis_callback)
+ self.lock_zero_srv = self.create_service(SetBool, '/lock_zero', self.lock_zero_callback)
+ self.land_srv = self.create_service(SetBool, '/landing', self.landing_callback)
+ self.linear_x = 0
+ self.linear_y = 0
+ self.linear_z = 0
+ self.angular_z = 0
+ self.zero_lock = False
+ self.axis_lock = False
+ self.drone_state = False
+ self.drone_landing = False
+
+ def publish_vel(self):
+ msg = Twist()
+ if self.zero_lock:
+ msg.linear.x = 0.0
+ msg.linear.y = 0.0
+ msg.linear.z = 0.0
+ msg.angular.z = 0.0
+ self.get_logger().info("zero lock")
+ else:
+ msg.linear.x = float(self.linear_x)
+ msg.linear.y = float(self.linear_y)
+ msg.linear.z = float(self.linear_z)
+ msg.angular.z = float(self.angular_z)
+ self.debug_pub.publish(msg)
+ if not self.drone_state or self.drone_landing:
+ # self.get_logger().info("Drone not activated")
+ return
+
+ self.pub.publish(msg)
+ return
+ def state_callback(self, msg):
+ if msg.system_status != 4:
+ self.drone_state = False
+ self.drone_landing = False
+ else:
+ self.drone_state = True
+
+ def landing_callback(self, request, response):
+ self.drone_landing = request.data
+ response = SetBool.Response()
+ response.success = True
+ return response
+
+ def hor_vel_callback(self, data):
+ if self.axis_lock:
+ if abs(data.linear.x) > abs(data.linear.y):
+ self.linear_y = -data.linear.x
+ self.linear_x = 0
+ else:
+ self.linear_x = data.linear.y
+ self.linear_y = 0
+ self.angular_z = 0
+ else:
+ self.linear_x = data.linear.y
+ self.linear_y = -data.linear.x
+ self.publish_vel()
+
+
+ def yaw_vel_callback(self, data):
+ if self.axis_lock:
+ self.angular_z = 0
+ else:
+ self.angular_z = data.linear.y
+ self.publish_vel()
+
+ def vert_vel_srv_callback(self, request, response):
+ self.linear_z = request.data
+ response.success = True
+ self.publish_vel()
+ return response
+
+ def lock_zero_callback(self, request, response):
+ self.zero_lock = request.data
+ response.success = True
+ return response
+
+ def lock_axis_callback(self, request, response):
+ self.axis_lock = request.data
+ response.success = True
+ return response
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ drone_movement = DroneMovement()
+ rclpy.spin(drone_movement)
+ drone_movement.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/controller/controller/helpers/__init__.py b/src/controller/controller/helpers/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/controller/controller/helpers/__pycache__/__init__.cpython-310.pyc b/src/controller/controller/helpers/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000..97a866b
Binary files /dev/null and b/src/controller/controller/helpers/__pycache__/__init__.cpython-310.pyc differ
diff --git a/src/controller/controller/helpers/__pycache__/button.cpython-310.pyc b/src/controller/controller/helpers/__pycache__/button.cpython-310.pyc
new file mode 100644
index 0000000..499aa13
Binary files /dev/null and b/src/controller/controller/helpers/__pycache__/button.cpython-310.pyc differ
diff --git a/src/controller/controller/helpers/__pycache__/filters.cpython-312.pyc b/src/controller/controller/helpers/__pycache__/filters.cpython-312.pyc
new file mode 100644
index 0000000..c6cb6a0
Binary files /dev/null and b/src/controller/controller/helpers/__pycache__/filters.cpython-312.pyc differ
diff --git a/src/controller/controller/helpers/__pycache__/serial_helper.cpython-312.pyc b/src/controller/controller/helpers/__pycache__/serial_helper.cpython-312.pyc
new file mode 100644
index 0000000..d1e0520
Binary files /dev/null and b/src/controller/controller/helpers/__pycache__/serial_helper.cpython-312.pyc differ
diff --git a/src/controller/controller/helpers/button.py b/src/controller/controller/helpers/button.py
new file mode 100644
index 0000000..72171e6
--- /dev/null
+++ b/src/controller/controller/helpers/button.py
@@ -0,0 +1,35 @@
+from std_msgs.msg import Int32
+import time
+
+class Button:
+ def __init__(self, topic, long_press_time=1, press_callback=None, release_callback=None, short_callback=None, long_callback=None):
+ self.topic = topic
+ # self.node = node
+ # self.sub = self.node.create_subscription(Int32, topic, self.data_callback, 10)
+ self.press_callback = press_callback
+ self.release_callback = release_callback
+ self.short_callback = short_callback
+ self.long_callback = long_callback
+ self.last_state = 0
+ self.last_time = None
+ self.long_press_time = long_press_time
+
+ def data_callback(self, msg):
+ if msg.data == 1:
+ if self.press_callback is not None:
+ self.press_callback()
+ if self.last_state == 0:
+ self.last_state = 1
+ self.last_time = time.time()
+ if self.short_callback is not None and self.long_callback is None:
+ self.short_callback()
+ elif (time.time() - self.last_time) > self.long_press_time and self.long_callback is not None:
+ self.long_callback()
+ else:
+ if self.last_state == 1:
+ if self.short_callback is not None and self.long_callback is not None and (time.time() - self.last_time) < self.long_press_time:
+ self.short_callback()
+ elif self.release_callback is not None:
+ self.release_callback()
+ self.last_state = 0
+
diff --git a/src/controller/controller/helpers/filters.py b/src/controller/controller/helpers/filters.py
new file mode 100644
index 0000000..4bbe140
--- /dev/null
+++ b/src/controller/controller/helpers/filters.py
@@ -0,0 +1,86 @@
+
+class RangedFilter:
+ def __init__(self, length, range):
+ self.length = length
+ self.range = range
+ self.array = []
+ def update(self, new_value):
+ if len(self.array) < self.length:
+ self.array.append(new_value)
+ else:
+ self.array.pop(0)
+ self.array.append(new_value)
+ min, max = 1000, 0
+ for i in self.array:
+ if i < min:
+ min = i
+ elif i > max:
+ max = i
+ # print(max-min)
+ if 0 < (max - min) < self.range:
+ return new_value
+ else:
+ return None
+
+
+class LowPassFilter:
+ def __init__(self, alpha, initial_value=None):
+ """
+ Initializes the low-pass filter.
+
+ Args:
+ alpha (float): Smoothing factor (0 < alpha <= 1).
+ Lower values mean stronger filtering.
+ initial_value (float): Initial output value.
+ """
+ self.alpha = alpha
+ self.filtered_value = initial_value
+
+ def update(self, new_value):
+ """
+ Updates the filter with a new input value.
+
+ Args:
+ new_value (float): The new input value.
+
+ Returns:
+ float: The filtered value.
+ """
+
+ if self.filtered_value is None:
+ self.filtered_value = new_value
+ else:
+ self.filtered_value = self.alpha * new_value + (1 - self.alpha) * self.filtered_value
+ return self.filtered_value
+
+
+class HighPassFilter:
+ def __init__(self, alpha, initial_value=None):
+ """
+ Initializes the high-pass filter.
+
+ Args:
+ alpha (float): Smoothing factor (0 < alpha <= 1).
+ Lower values mean stronger filtering.
+ initial_value (float): Initial input and output values.
+ """
+ self.alpha = alpha
+ self.previous_input = initial_value
+ self.filtered_value = 0
+
+ def update(self, new_value):
+ """
+ Updates the filter with a new input value.
+
+ Args:
+ new_value (float): The new input value.
+
+ Returns:
+ float: The filtered value.
+ """
+ if self.previous_input is None:
+ self.previous_input = new_value
+ else:
+ self.filtered_value = self.alpha * (self.filtered_value + new_value - self.previous_input)
+ self.previous_input = new_value
+ return self.filtered_value
diff --git a/src/controller/controller/helpers/joystick.py b/src/controller/controller/helpers/joystick.py
new file mode 100644
index 0000000..e48c512
--- /dev/null
+++ b/src/controller/controller/helpers/joystick.py
@@ -0,0 +1,35 @@
+class Joystick:
+ def __init__(self, zero, dead_zone, max_input, min_input, max_output, min_output = None):
+ self.zero = zero
+ self.dead_zone = dead_zone
+ self.max_input = max_input
+ self.min_input = min_input
+ self.max_output = max_output
+ self.min_output = 0 - (max_output - 0) if not min_output else min_output
+ # print(f"Min_output: {self.min_output}, Max_output: {self.max_output}, input: {min_output}")
+
+ def get_output(self, input):
+ # Using ROS 2 logging instead of print statements for visibility
+ # self.get_logger().info(f"min_input: {self.min_input}, input: {input}, max_input: {self.max_input}")
+
+ if input > self.max_input:
+ return self.max_output
+ elif input <= self.min_input:
+ return self.min_output
+ elif input < (self.zero - self.dead_zone / 2): # Below zero deadzone
+ # Log scaling output below deadzone
+ # print(f"Scaling output below deadzone: -({self.min_output}) * ({input} - {self.min_input}) / ({self.zero} - {self.min_input}) = {-(self.min_output) * (input - self.min_input) / (self.zero - self.min_input)}")
+ return (self.min_output) * (self.zero - input ) / (self.zero - self.min_input) # Fix scaling
+ elif input > (self.zero + self.dead_zone / 2): # Above zero deadzone
+ # Log scaling output above deadzone
+ # self.get_logger().info(f"Scaling output above deadzone: {(self.max_output) * (input - self.zero) / (self.max_input - self.zero)}")
+ return (self.max_output) * (input - self.zero) / (self.max_input - self.zero) # Fix scaling
+ else: # Deadzone region
+ return 0
+
+ def recenter(self, center, min_input=None, max_input=None):
+ self.zero = center
+ self.min_input = min_input if min_input is not None else self.min_input
+ self.max_input = max_input if max_input is not None else self.max_input
+
+ return True
diff --git a/src/controller/controller/helpers/serial_helper.py b/src/controller/controller/helpers/serial_helper.py
new file mode 100644
index 0000000..e744c54
--- /dev/null
+++ b/src/controller/controller/helpers/serial_helper.py
@@ -0,0 +1,54 @@
+import serial
+import time
+
+class SerialHelper:
+ def __init__(self, com_port, baud_rate=115200, timeout=1):
+ self.com_port = com_port
+ self.baud_rate = baud_rate
+ self.timeout = timeout
+ self.ser = serial.Serial(com_port, baudrate=baud_rate, timeout=timeout)
+
+ def restart(self):
+ self.ser = serial.Serial(self.com_port, baudrate=self.baud_rate, timeout=self.timeout)
+
+ def clear_buffer(self):
+ """
+ Clears the serial input buffer by reading and discarding any unread data.
+ """
+ try:
+ self.ser.reset_input_buffer() # Clears the input buffer
+ # print("Serial buffer cleared.")
+ except serial.SerialException as e:
+ print(f"Error clearing the serial buffer: {e}")
+
+ def read_from_serial(self):
+ """
+ Reads data from an Arduino connected to the specified COM port after clearing the buffer.
+
+ Returns:
+ string: A string containing data sent. Returns None if no valid data is read.
+ """
+ try:
+ self.clear_buffer() # Clear the buffer before reading
+ # Wait until data is available and then read it
+ while self.ser.in_waiting == 0: # Wait for data to arrive
+ time.sleep(0.1) # Small delay to avoid busy-waiting
+
+ # Read the line of data once it's available
+ line = self.ser.readline().decode('utf-8').strip()
+ return line
+ except:
+ print(f"Error reading from serial port: {e}")
+ return None
+
+ def write_to_serial(self, message):
+ """
+ Write a string to the specified serial port.
+
+ :param message: The string to send to the serial device.
+ """
+ try:
+ self.ser.write(message.encode('utf-8'))
+ print(f"Message sent: {message}")
+ except serial.SerialException as e:
+ print(f"Error writing to serial port: {e}")
diff --git a/src/controller/controller/imu_converter.py b/src/controller/controller/imu_converter.py
new file mode 100644
index 0000000..b0f654d
--- /dev/null
+++ b/src/controller/controller/imu_converter.py
@@ -0,0 +1,230 @@
+import rclpy
+from rclpy.parameter import Parameter
+from rcl_interfaces.msg import SetParametersResult
+from std_srvs.srv import SetBool
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from geometry_msgs.msg import Twist, Vector3
+from controller.madgwick_py import madgwickahrs, quaternion
+from controller.helpers.joystick import Joystick
+from std_msgs.msg import Float32MultiArray
+import time
+
+# maximum velocity in m/s when at full throttle
+MAX_VELOCITY = 0.1
+
+class ImuConverter(Node):
+ def __init__(self):
+ super().__init__('imu_converter')
+
+ self.declare_parameter('hand', 'right') # Declare 'hand' parameter
+
+ # Declare parameters with default values
+ self.declare_parameter('beta', 0.2)
+ self.declare_parameter('zeta', 0.3)
+ self.declare_parameter('zero', 0)
+ self.declare_parameter('max_vel', 1.0)
+ self.declare_parameter('max_imu_pitch', 45)
+ self.declare_parameter('min_imu_pitch', -20)
+ self.declare_parameter('zero_imu_pitch', None)
+ self.declare_parameter('deadzone_imu_pitch', 15)
+
+ self.declare_parameter('max_imu_roll', 30)
+ self.declare_parameter('min_imu_roll', -30)
+ self.declare_parameter('zero_imu_roll', None)
+ self.declare_parameter('deadzone_imu_roll', 15)
+
+ self.hand = self.get_parameter('hand').value
+
+ # Validate 'hand' parameter
+ if self.hand not in ["right", "left"]:
+ self.get_logger().warn("Invalid 'hand' parameter! Must be 'right' or 'left'. Defaulting to 'right'.")
+ self.hand = "right"
+
+ # Initialize parameters
+ self.beta = self.get_parameter('beta').value
+ self.zeta = self.get_parameter('zeta').value
+ self.max_vel = self.get_parameter('max_vel').value
+ self.max_imu_pitch = self.get_parameter('max_imu_pitch').value
+ self.min_imu_pitch = self.get_parameter('min_imu_pitch').value
+ self.zero_imu_pitch = self.get_parameter('zero_imu_pitch').value
+ if self.zero_imu_pitch is None or self.zero_imu_pitch == 0:
+ self.zero_imu_pitch = (self.max_imu_pitch + self.min_imu_pitch) / 2
+ self.pitch_deadzone = self.get_parameter('deadzone_imu_pitch').value
+ self.pitch_joystick = Joystick(self.zero_imu_pitch, self.pitch_deadzone, self.max_imu_pitch, self.min_imu_pitch, self.max_vel)
+
+ self.max_imu_roll = self.get_parameter('max_imu_roll').value
+ self.min_imu_roll = self.get_parameter('min_imu_roll').value
+ self.zero_imu_roll = self.get_parameter('zero_imu_roll').value
+ if self.zero_imu_roll is None or self.zero_imu_roll == 0:
+ self.zero_imu_roll = (self.max_imu_roll + self.min_imu_roll) / 2
+ self.roll_deadzone = self.get_parameter('deadzone_imu_roll').value
+ self.roll_joystick = Joystick(self.zero_imu_roll, self.roll_deadzone, self.max_imu_roll, self.min_imu_roll, self.max_vel)
+
+ base_quat = quaternion.Quaternion([1, 0, 0, 0])
+
+ # Initialize Madgwick filter
+ self.madgwick = madgwickahrs.MadgwickAHRS(quaternion=base_quat, beta=self.beta, zeta=self.zeta)
+
+ self.current_time = time.time()
+
+ # Subscriber to IMU data
+ self.subscription = self.create_subscription(
+ Imu,
+ '/imu/' + self.hand + '/data', # Topic name
+ self.imu_callback,
+ 10 # Queue size
+ )
+
+ # Publisher for velocity command
+ self.velocity_publisher = self.create_publisher(
+ Twist,
+ '/' + self.hand + '/cmd_vel_hor', # Topic name for velocity command
+ 10 # Queue size
+ )
+
+ # Publisher for Euler angles
+ self.euler_publisher = self.create_publisher(
+ Float32MultiArray,
+ '/imu/' + self.hand + '/euler', # Topic name for Euler angles
+ 10 # Queue size
+ )
+
+ # Parameter event callback for dynamic reconfiguration
+ self.add_on_set_parameters_callback(self.parameter_callback)
+
+ self.get_logger().info('IMU Subscriber and Velocity Publisher node has been started.')
+
+ # Service to recenter imu
+ self.center_srv = self.create_service(SetBool, self.hand + '/center_imu', self.center_imu)
+
+
+ def parameter_callback(self, params):
+ """
+ Callback to update internal parameters when they're changed dynamically.
+ """
+ for param in params:
+ if param.name == 'beta':
+ self.beta = param.value
+ self.madgwick.beta = self.beta
+ elif param.name == 'zeta':
+ self.zeta = param.value
+ self.madgwick.zeta = self.zeta
+ elif param.name == 'max_vel':
+ self.max_vel = param.value
+ self.pitch_joystick.max_output = self.max_vel
+ self.roll_joystick.max_output = self.max_vel
+ elif param.name == 'max_imu_pitch':
+ self.max_imu_pitch = param.value
+ if self.zero_imu_pitch == 0 or self.zero_imu_pitch is None:
+ self.zero_imu_pitch = (self.max_imu_pitch + self.min_imu_pitch) / 2
+ self.pitch_joystick.max_input = self.max_imu_pitch
+ elif param.name == 'min_imu_pitch':
+ self.min_imu_pitch = param.value
+ if self.zero_imu_pitch == 0 or self.zero_imu_pitch is None:
+ self.zero_imu_pitch = (self.max_imu_pitch + self.min_imu_pitch) / 2
+ self.pitch_joystick.min_input = self.min_imu_pitch
+ elif param.name == 'zero_imu_pitch':
+ self.zero_imu_pitch = param.value
+ if self.zero_imu_pitch == 0 or self.zero_imu_pitch is None:
+ self.zero_imu_pitch = (self.max_imu_pitch + self.min_imu_pitch) / 2
+ self.pitch_joystick.zero = self.zero_imu_pitch
+ elif param.name == 'deadzone_imu_pitch':
+ self.pitch_deadzone = param.value
+ self.pitch_joystick.dead_zone = self.pitch_deadzone
+ elif param.name == 'max_imu_roll':
+ self.max_imu_roll = param.value
+ if self.zero_imu_roll == 0 or self.zero_imu_roll is None:
+ self.zero_imu_roll = (self.max_imu_roll + self.min_imu_roll) / 2
+ self.roll_joystick.max_input = self.max_imu_roll
+ elif param.name == 'min_imu_roll':
+ self.min_imu_roll = param.value
+ if self.zero_imu_roll == 0 or self.zero_imu_roll is None:
+ self.zero_imu_roll = (self.max_imu_roll + self.min_imu_roll) / 2
+ self.roll_joystick.min_input = self.min_imu_roll
+ elif param.name == 'zero_imu_roll':
+ self.zero_imu_roll = param.value
+ if self.zero_imu_roll == 0 or self.zero_imu_roll is None:
+ self.zero_imu_roll = (self.max_imu_roll + self.min_imu_roll) / 2
+ self.roll_joystick.zero = self.zero_imu_roll
+ elif param.name == 'deadzone_imu_roll':
+ self.roll_deadzone = param.value
+ self.roll_joystick.dead_zone = self.roll_deadzone
+
+ return SetParametersResult(successful=True)
+
+ def imu_callback(self, msg):
+ # Process IMU data (you can modify the logic based on your use case)
+ linear_acceleration = msg.linear_acceleration
+ angular_velocity = msg.angular_velocity
+
+ # Convert data to float
+ accel_data =[float(linear_acceleration.x), float(linear_acceleration.y), float(linear_acceleration.z)]
+
+ gyro_data = [
+ float(angular_velocity.x) * (3.141592653589793 / 180),
+ float(angular_velocity.y) * (3.141592653589793 / 180),
+ float(angular_velocity.z) * (3.141592653589793 / 180)
+ ]
+
+
+ interval = time.time() - self.current_time
+ self.current_time = time.time()
+ self.madgwick.samplePeriod = interval
+ # Update Madgwick filter
+ self.madgwick.update_imu(accelerometer=accel_data, gyroscope=gyro_data)
+
+ # Get Euler angles
+ pitch, yaw, roll = self.madgwick.quaternion.to_euler_angles()
+
+ # Publish Euler angles
+ euler_msg = Float32MultiArray()
+ if self.hand == 'left':
+ pitch = -pitch
+ roll = -roll
+ euler_msg.data = [pitch, roll, yaw]
+ self.euler_publisher.publish(euler_msg)
+
+ # Example: Using IMU data to create simple velocity commands
+ # Here, we use linear acceleration along the x-axis to control forward speed
+ # and angular velocity along the z-axis for yaw control.
+
+ forward_output = self.pitch_joystick.get_output(pitch)
+ side_output = self.roll_joystick.get_output(roll)
+
+ # cast as as Python's float object as some instances appear as a numpy.float64
+ forward_vel = float(forward_output * MAX_VELOCITY)
+ side_vel = float(side_output * MAX_VELOCITY)
+
+ velocity_linear = Vector3(x=forward_vel, y=side_vel, z=0.0)
+ velocity_angular = Vector3()
+ velocity_msg = Twist(linear=velocity_linear, angular=velocity_angular)
+
+ # Publish velocity command
+ self.velocity_publisher.publish(velocity_msg)
+
+ # Log the velocity command and Euler angles for debugging
+ # self.get_logger().info(f"linear.x={velocity_msg.linear.x}, linear.y={velocity_msg.linear.y}")
+ # self.get_logger().info(f"pitch={pitch}, roll={roll}, yaw={yaw}")
+
+ def center_imu(self, request, response):
+ pitch, yaw, roll = self.madgwick.quaternion.to_euler_angles()
+ pitch_max = self.max_imu_pitch - self.zero_imu_pitch + pitch
+ pitch_min = pitch - (self.zero_imu_pitch - self.min_imu_pitch)
+ self.pitch_joystick.recenter(center=pitch, min_input=pitch_min, max_input=pitch_max)
+ roll_max = self.max_imu_roll - self.zero_imu_roll + roll
+ roll_min = roll - (self.zero_imu_roll - self.min_imu_roll)
+ self.roll_joystick.recenter(center=roll, min_input=roll_min, max_input=roll_max)
+ response.success = True
+ self.get_logger().info(f"IMU recentered to pitch {pitch}, roll {roll}")
+ return response
+
+def main(args=None):
+ rclpy.init(args=args)
+ imu_converter = ImuConverter()
+ rclpy.spin(imu_converter)
+ imu_converter.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/controller/controller/madgwick_py/README.md b/src/controller/controller/madgwick_py/README.md
new file mode 100644
index 0000000..d6f0324
--- /dev/null
+++ b/src/controller/controller/madgwick_py/README.md
@@ -0,0 +1,28 @@
+# madgwick_py: A Python implementation of Madgwick's IMU and AHRS algorithm.
+
+More information on the algorithm can be found at
+.
+
+This implementation was done at the Cognitive Systems Lab (CSL) of the
+Karlsruhe Institute of Technology:
+
+# Requirements
+* Python 3.x (tested with Python 3.4)
+* NumPy
+
+# License
+
+Copyright (c) 2015-2022 Jonas Böer, webmaster@morgil.de
+
+This program 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 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 Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program. If not, see .
diff --git a/src/controller/controller/madgwick_py/__init__.py b/src/controller/controller/madgwick_py/__init__.py
new file mode 100644
index 0000000..5bcf951
--- /dev/null
+++ b/src/controller/controller/madgwick_py/__init__.py
@@ -0,0 +1,19 @@
+# -*- coding: utf-8 -*-
+"""
+ Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu
+
+ This program 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 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see .
+"""
+
+__all__ = ["madgwickahrs", "quaternion"]
diff --git a/src/controller/controller/madgwick_py/madgwickahrs.py b/src/controller/controller/madgwick_py/madgwickahrs.py
new file mode 100644
index 0000000..029fafb
--- /dev/null
+++ b/src/controller/controller/madgwick_py/madgwickahrs.py
@@ -0,0 +1,157 @@
+# -*- coding: utf-8 -*-
+"""
+ Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu
+
+ This program 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 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see .
+"""
+
+import warnings
+import numpy as np
+from numpy.linalg import norm
+from .quaternion import Quaternion
+
+
+class MadgwickAHRS:
+ samplePeriod = 1/256
+ quaternion = Quaternion(1, 0, 0, 0)
+ beta = 1
+ zeta = 0
+
+ def __init__(self, sampleperiod=None, quaternion=None, beta=None, zeta=None):
+ """
+ Initialize the class with the given parameters.
+ :param sampleperiod: The sample period
+ :param quaternion: Initial quaternion
+ :param beta: Algorithm gain beta
+ :param beta: Algorithm gain zeta
+ :return:
+ """
+ if sampleperiod is not None:
+ self.samplePeriod = sampleperiod
+ if quaternion is not None:
+ self.quaternion = quaternion
+ if beta is not None:
+ self.beta = beta
+ if zeta is not None:
+ self.zeta = zeta
+
+ def update(self, gyroscope, accelerometer, magnetometer):
+ """
+ Perform one update step with data from a AHRS sensor array
+ :param gyroscope: A three-element array containing the gyroscope data in radians per second.
+ :param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
+ :param magnetometer: A three-element array containing the magnetometer data. Can be any unit since a normalized value is used.
+ :return:
+ """
+ q = self.quaternion
+
+ gyroscope = np.array(gyroscope, dtype=float).flatten()
+ accelerometer = np.array(accelerometer, dtype=float).flatten()
+ magnetometer = np.array(magnetometer, dtype=float).flatten()
+
+ # Normalise accelerometer measurement
+ if norm(accelerometer) == 0:
+ warnings.warn("accelerometer is zero")
+ return
+ accelerometer /= norm(accelerometer)
+
+ # Normalise magnetometer measurement
+ if norm(magnetometer) == 0:
+ warnings.warn("magnetometer is zero")
+ return
+ magnetometer /= norm(magnetometer)
+
+ h = q * (Quaternion(0, magnetometer[0], magnetometer[1], magnetometer[2]) * q.conj())
+ b = np.array([0, norm(h[1:3]), 0, h[3]])
+
+ # Gradient descent algorithm corrective step
+ f = np.array([
+ 2*(q[1]*q[3] - q[0]*q[2]) - accelerometer[0],
+ 2*(q[0]*q[1] + q[2]*q[3]) - accelerometer[1],
+ 2*(0.5 - q[1]**2 - q[2]**2) - accelerometer[2],
+ 2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - magnetometer[0],
+ 2*b[1]*(q[1]*q[2] - q[0]*q[3]) + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - magnetometer[1],
+ 2*b[1]*(q[0]*q[2] + q[1]*q[3]) + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - magnetometer[2]
+ ])
+ j = np.array([
+ [-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
+ [2*q[1], 2*q[0], 2*q[3], 2*q[2]],
+ [0, -4*q[1], -4*q[2], 0],
+ [-2*b[3]*q[2], 2*b[3]*q[3], -4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]],
+ [-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0], 2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]],
+ [2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2], 2*b[1]*q[1]]
+ ])
+ step = j.T.dot(f)
+ step /= norm(step) # normalise step magnitude
+
+ # Gyroscope compensation drift
+ gyroscopeQuat = Quaternion(0, gyroscope[0], gyroscope[1], gyroscope[2])
+ stepQuat = Quaternion(step.T[0], step.T[1], step.T[2], step.T[3])
+
+ gyroscopeQuat = gyroscopeQuat + (q.conj() * stepQuat) * 2 * self.samplePeriod * self.zeta * -1
+
+ # Compute rate of change of quaternion
+ qdot = (q * gyroscopeQuat) * 0.5 - self.beta * step.T
+
+ # Integrate to yield quaternion
+ q += qdot * self.samplePeriod
+ self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
+
+ def update_imu(self, gyroscope, accelerometer):
+ """
+ Perform one update step with data from a IMU sensor array
+ :param gyroscope: A three-element array containing the gyroscope data in radians per second.
+ :param accelerometer: A three-element array containing the accelerometer data. Can be any unit since a normalized value is used.
+ """
+ q = self.quaternion
+
+ gyroscope = np.array(gyroscope, dtype=float).flatten()
+ accelerometer = np.array(accelerometer, dtype=float).flatten()
+
+ # Normalise accelerometer measurement
+ if norm(accelerometer) == 0:
+ warnings.warn("accelerometer is zero")
+ return
+ accelerometer /= norm(accelerometer)
+ # print(q.q)
+
+ # Gradient descent algorithm corrective step
+ f = np.array([
+ 2*(q[1]*q[3] - q[0]*q[2]) - accelerometer[0],
+ 2*(q[0]*q[1] + q[2]*q[3]) - accelerometer[1],
+ 2*(0.5 - q[1]**2 - q[2]**2) - accelerometer[2]
+ ])
+ j = np.array([
+ [-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
+ [2*q[1], 2*q[0], 2*q[3], 2*q[2]],
+ [0, -4*q[1], -4*q[2], 0]
+ ])
+ step = j.T.dot(f)
+ # Normalization with small regularization
+ step /= (norm(step) + 1e-8)
+
+ # Skip update if Jacobian is zero
+ if np.all(j == 0):
+ warnings.warn("Jacobian is zero; skipping update step.")
+ return
+
+ # print(j, '/t', f)
+ # print(step)
+
+ # Compute rate of change of quaternion
+ qdot = (q * Quaternion(0, gyroscope[0], gyroscope[1], gyroscope[2])) * 0.5 - self.beta * step.T
+
+ # Integrate to yield quaternion
+ q += qdot * self.samplePeriod
+ self.quaternion = Quaternion(q / norm(q)) # normalise quaternion
diff --git a/src/controller/controller/madgwick_py/quaternion.py b/src/controller/controller/madgwick_py/quaternion.py
new file mode 100644
index 0000000..d79240f
--- /dev/null
+++ b/src/controller/controller/madgwick_py/quaternion.py
@@ -0,0 +1,157 @@
+# -*- coding: utf-8 -*-
+"""
+ Copyright (c) 2015 Jonas Böer, jonas.boeer@student.kit.edu
+
+ This program 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 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 Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with this program. If not, see .
+"""
+
+import numpy as np
+import numbers
+
+
+class Quaternion:
+ """
+ A simple class implementing basic quaternion arithmetic.
+ """
+ def __init__(self, w_or_q, x=None, y=None, z=None):
+ """
+ Initializes a Quaternion object
+ :param w_or_q: A scalar representing the real part of the quaternion, another Quaternion object or a
+ four-element array containing the quaternion values
+ :param x: The first imaginary part if w_or_q is a scalar
+ :param y: The second imaginary part if w_or_q is a scalar
+ :param z: The third imaginary part if w_or_q is a scalar
+ """
+ self._q = np.array([1, 0, 0, 0])
+
+ if x is not None and y is not None and z is not None:
+ w = w_or_q
+ q = np.array([w, x, y, z])
+ elif isinstance(w_or_q, Quaternion):
+ q = np.array(w_or_q.q)
+ else:
+ q = np.array(w_or_q)
+ if len(q) != 4:
+ raise ValueError("Expecting a 4-element array or w x y z as parameters")
+
+ self.q = q
+
+ # Quaternion specific interfaces
+
+ def conj(self):
+ """
+ Returns the conjugate of the quaternion
+ :rtype : Quaternion
+ :return: the conjugate of the quaternion
+ """
+ return Quaternion(self._q[0], -self._q[1], -self._q[2], -self._q[3])
+
+ def to_angle_axis(self):
+ """
+ Returns the quaternion's rotation represented by an Euler angle and axis.
+ If the quaternion is the identity quaternion (1, 0, 0, 0), a rotation along the x axis with angle 0 is returned.
+ :return: rad, x, y, z
+ """
+ if self[0] == 1 and self[1] == 0 and self[2] == 0 and self[3] == 0:
+ return 0, 1, 0, 0
+ rad = np.arccos(self[0]) * 2
+ imaginary_factor = np.sin(rad / 2)
+ if abs(imaginary_factor) < 1e-8:
+ return 0, 1, 0, 0
+ x = self._q[1] / imaginary_factor
+ y = self._q[2] / imaginary_factor
+ z = self._q[3] / imaginary_factor
+ return rad, x, y, z
+
+ @staticmethod
+ def from_angle_axis(rad, x, y, z):
+ s = np.sin(rad / 2)
+ return Quaternion(np.cos(rad / 2), x*s, y*s, z*s)
+
+ def to_euler_angles(self):
+ pitch = np.arcsin(2 * self[1] * self[2] + 2 * self[0] * self[3])
+ if np.abs(self[1] * self[2] + self[3] * self[0] - 0.5) < 1e-8:
+ roll = 0
+ yaw = 2 * np.arctan2(self[1], self[0])
+ elif np.abs(self[1] * self[2] + self[3] * self[0] + 0.5) < 1e-8:
+ roll = -2 * np.arctan2(self[1], self[0])
+ yaw = 0
+ else:
+ roll = np.arctan2(2 * self[0] * self[1] - 2 * self[2] * self[3], 1 - 2 * self[1] ** 2 - 2 * self[3] ** 2)
+ yaw = np.arctan2(2 * self[0] * self[2] - 2 * self[1] * self[3], 1 - 2 * self[2] ** 2 - 2 * self[3] ** 2)
+ # Convert radians to degrees
+ roll = np.degrees(roll)
+ pitch = np.degrees(pitch)
+ yaw = np.degrees(yaw)
+
+ # Normalize to [-180, 180]
+ roll = (roll + 180) % 360 - 180
+ pitch = (pitch + 180) % 360 - 180
+ yaw = (yaw + 180) % 360 - 180
+
+ return roll, pitch, yaw
+
+ def to_euler123(self):
+ roll = np.arctan2(-2 * (self[2] * self[3] - self[0] * self[1]), self[0] ** 2 - self[1] ** 2 - self[2] ** 2 + self[3] ** 2)
+ pitch = np.arcsin(2 * (self[1] * self[3] + self[0] * self[1]))
+ yaw = np.arctan2(-2 * (self[1] * self[2] - self[0] * self[3]), self[0] ** 2 + self[1] ** 2 - self[2] ** 2 - self[3] ** 2)
+ return roll, pitch, yaw
+
+ def __mul__(self, other):
+ """
+ multiply the given quaternion with another quaternion or a scalar
+ :param other: a Quaternion object or a number
+ :return:
+ """
+ if isinstance(other, Quaternion):
+ w = self._q[0]*other._q[0] - self._q[1]*other._q[1] - self._q[2]*other._q[2] - self._q[3]*other._q[3]
+ x = self._q[0]*other._q[1] + self._q[1]*other._q[0] + self._q[2]*other._q[3] - self._q[3]*other._q[2]
+ y = self._q[0]*other._q[2] - self._q[1]*other._q[3] + self._q[2]*other._q[0] + self._q[3]*other._q[1]
+ z = self._q[0]*other._q[3] + self._q[1]*other._q[2] - self._q[2]*other._q[1] + self._q[3]*other._q[0]
+
+ return Quaternion(w, x, y, z)
+ elif isinstance(other, numbers.Number):
+ q = self._q * other
+ return Quaternion(q)
+
+ def __add__(self, other):
+ """
+ add two quaternions element-wise or add a scalar to each element of the quaternion
+ :param other:
+ :return:
+ """
+ if not isinstance(other, Quaternion):
+ if len(other) != 4:
+ raise TypeError("Quaternions must be added to other quaternions or a 4-element array")
+ q = self._q + other
+ else:
+ q = self._q + other._q
+
+ return Quaternion(q)
+
+ # Implementing other interfaces to ease working with the class
+
+ @property
+ def q(self):
+ return self._q
+
+ @q.setter
+ def q(self, q):
+ self._q = q
+
+ def __getitem__(self, item):
+ return self._q[item]
+
+ def __array__(self):
+ return self._q
diff --git a/src/controller/controller/pot_calibration.csv b/src/controller/controller/pot_calibration.csv
new file mode 100644
index 0000000..718f53b
--- /dev/null
+++ b/src/controller/controller/pot_calibration.csv
@@ -0,0 +1,4097 @@
+Y,X
+0.0,9.612378414340206
+1.0,9.625352811101532
+2.0,9.638306050984967
+3.0,9.651238292104974
+4.0,9.664149690853833
+5.0,9.677040401928336
+6.0,9.689910578355956
+7.0,9.702760371520528
+8.0,9.715589931187392
+9.0,9.72839940552807
+10.0,9.741188941144456
+11.0,9.753958683092543
+12.0,9.766708774905691
+13.0,9.77943935861744
+14.0,9.792150574783918
+15.0,9.804842562505776
+16.0,9.817515459449764
+17.0,9.83016940186985
+18.0,9.84280452462798
+19.0,9.855420961214424
+20.0,9.86801884376777
+21.0,9.880598303094514
+22.0,9.89315946868833
+23.0,9.905702468748945
+24.0,9.918227430200709
+25.0,9.930734478710779
+26.0,9.943223738707028
+27.0,9.955695333395576
+28.0,9.968149384778064
+29.0,9.980586013668553
+30.0,9.993005339710175
+31.0,10.005407481391467
+32.0,10.017792556062405
+33.0,10.030160679950185
+34.0,10.042511968174692
+35.0,10.054846534763719
+36.0,10.067164492667924
+37.0,10.079465953775513
+38.0,10.091751028926677
+39.0,10.104019827927777
+40.0,10.116272459565296
+41.0,10.128509031619537
+42.0,10.14072965087809
+43.0,10.152934423149093
+44.0,10.165123453274227
+45.0,10.177296845141539
+46.0,10.189454701698015
+47.0,10.201597124961951
+48.0,10.213724216035141
+49.0,10.225836075114822
+50.0,10.237932801505458
+51.0,10.250014493630305
+52.0,10.262081249042806
+53.0,10.27413316443779
+54.0,10.286170335662483
+55.0,10.298192857727347
+56.0,10.31020082481675
+57.0,10.322194330299453
+58.0,10.334173466738932
+59.0,10.346138325903539
+60.0,10.358088998776495
+61.0,10.370025575565736
+62.0,10.381948145713576
+63.0,10.393856797906261
+64.0,10.405751620083326
+65.0,10.41763269944685
+66.0,10.42950012247052
+67.0,10.4413539749086
+68.0,10.453194341804725
+69.0,10.465021307500589
+70.0,10.476834955644465
+71.0,10.488635369199633
+72.0,10.500422630452654
+73.0,10.512196821021512
+74.0,10.523958021863665
+75.0,10.535706313283939
+76.0,10.547441774942326
+77.0,10.559164485861654
+78.0,10.570874524435146
+79.0,10.582571968433866
+80.0,10.594256895014054
+81.0,10.605929380724351
+82.0,10.617589501512924
+83.0,10.629237332734473
+84.0,10.640872949157146
+85.0,10.65249642496936
+86.0,10.664107833786495
+87.0,10.675707248657522
+88.0,10.687294742071522
+89.0,10.6988703859641
+90.0,10.710434251723727
+91.0,10.721986410197983
+92.0,10.733526931699698
+93.0,10.745055886013038
+94.0,10.756573342399463
+95.0,10.768079369603626
+96.0,10.7795740358592
+97.0,10.791057408894588
+98.0,10.802529555938586
+99.0,10.813990543725948
+100.0,10.82544043850288
+101.0,10.83687930603246
+102.0,10.848307211599977
+103.0,10.8597242200182
+104.0,10.871130395632576
+105.0,10.882525802326358
+106.0,10.893910503525651
+107.0,10.905284562204411
+108.0,10.916648040889353
+109.0,10.928001001664807
+110.0,10.93934350617751
+111.0,10.950675615641323
+112.0,10.961997390841894
+113.0,10.973308892141244
+114.0,10.984610179482324
+115.0,10.995901312393476
+116.0,11.00718234999285
+117.0,11.01845335099277
+118.0,11.029714373704026
+119.0,11.040965476040128
+120.0,11.052206715521491
+121.0,11.063438149279566
+122.0,11.074659834060933
+123.0,11.08587182623131
+124.0,11.097074181779552
+125.0,11.108266956321561
+126.0,11.119450205104163
+127.0,11.13062398300894
+128.0,11.141788344556
+129.0,11.152943343907705
+130.0,11.164089034872358
+131.0,11.175225470907838
+132.0,11.186352705125179
+133.0,11.197470790292128
+134.0,11.208579778836631
+135.0,11.219679722850303
+136.0,11.230770674091827
+137.0,11.241852683990338
+138.0,11.252925803648742
+139.0,11.263990083847007
+140.0,11.275045575045413
+141.0,11.286092327387752
+142.0,11.297130390704512
+143.0,11.308159814515989
+144.0,11.31918064803539
+145.0,11.330192940171882
+146.0,11.34119673953362
+147.0,11.352192094430706
+148.0,11.363179052878163
+149.0,11.37415766259882
+150.0,11.385127971026204
+151.0,11.396090025307375
+152.0,11.407043872305731
+153.0,11.417989558603786
+154.0,11.428927130505912
+155.0,11.43985663404105
+156.0,11.450778114965376
+157.0,11.461691618764963
+158.0,11.472597190658385
+159.0,11.483494875599316
+160.0,11.494384718279061
+161.0,11.505266763129107
+162.0,11.516141054323604
+163.0,11.527007635781834
+164.0,11.537866551170664
+165.0,11.548717843906932
+166.0,11.559561557159864
+167.0,11.570397733853396
+168.0,11.581226416668537
+169.0,11.592047648045648
+170.0,11.602861470186737
+171.0,11.6136679250577
+172.0,11.624467054390553
+173.0,11.635258899685642
+174.0,11.646043502213804
+175.0,11.65682090301854
+176.0,11.667591142918127
+177.0,11.67835426250774
+178.0,11.689110302161529
+179.0,11.699859302034675
+180.0,11.710601302065436
+181.0,11.721336341977157
+182.0,11.732064461280268
+183.0,11.742785699274256
+184.0,11.753500095049608
+185.0,11.76420768748976
+186.0,11.774908515272978
+187.0,11.785602616874277
+188.0,11.796290030567265
+189.0,11.806970794426007
+190.0,11.817644946326851
+191.0,11.82831252395023
+192.0,11.838973564782462
+193.0,11.84962810611752
+194.0,11.860276185058778
+195.0,11.870917838520757
+196.0,11.881553103230836
+197.0,11.892182015730944
+198.0,11.902804612379256
+199.0,11.913420929351847
+200.0,11.924031002644341
+201.0,11.93463486807354
+202.0,11.945232561279036
+203.0,11.955824117724813
+204.0,11.966409572700822
+205.0,11.976988961324551
+206.0,11.987562318542562
+207.0,11.998129679132036
+208.0,12.00869107770229
+209.0,12.019246548696268
+210.0,12.029796126392037
+211.0,12.040339844904262
+212.0,12.050877738185651
+213.0,12.061409840028414
+214.0,12.071936184065681
+215.0,12.082456803772915
+216.0,12.092971732469332
+217.0,12.103481003319258
+218.0,12.113984649333526
+219.0,12.124482703370834
+220.0,12.134975198139077
+221.0,12.145462166196697
+222.0,12.155943639954002
+223.0,12.166419651674461
+224.0,12.176890233476014
+225.0,12.187355417332354
+226.0,12.197815235074193
+227.0,12.208269718390522
+228.0,12.218718898829865
+229.0,12.2291628078015
+230.0,12.239601476576699
+231.0,12.25003493628993
+232.0,12.26046321794007
+233.0,12.27088635239157
+234.0,12.281304370375663
+235.0,12.291717302491513
+236.0,12.302125179207383
+237.0,12.312528030861769
+238.0,12.322925887664548
+239.0,12.333318779698095
+240.0,12.343706736918405
+241.0,12.354089789156193
+242.0,12.364467966117989
+243.0,12.37484129738723
+244.0,12.385209812425321
+245.0,12.395573540572721
+246.0,12.405932511049983
+247.0,12.416286752958802
+248.0,12.426636295283062
+249.0,12.43698116688986
+250.0,12.447321396530516
+251.0,12.4576570128416
+252.0,12.467988044345919
+253.0,12.47831451945352
+254.0,12.488636466462664
+255.0,12.49895391356081
+256.0,12.509266888825579
+257.0,12.519575420225713
+258.0,12.52987953562202
+259.0,12.54017926276833
+260.0,12.550474629312413
+261.0,12.56076566279692
+262.0,12.571052390660286
+263.0,12.581334840237657
+264.0,12.591613038761782
+265.0,12.601887013363912
+266.0,12.61215679107469
+267.0,12.622422398825032
+268.0,12.632683863446987
+269.0,12.64294121167463
+270.0,12.653194470144895
+271.0,12.663443665398441
+272.0,12.673688823880491
+273.0,12.683929971941678
+274.0,12.694167135838866
+275.0,12.704400341735983
+276.0,12.714629615704835
+277.0,12.724854983725914
+278.0,12.735076471689219
+279.0,12.745294105395025
+280.0,12.755507910554714
+281.0,12.765717912791525
+282.0,12.775924137641354
+283.0,12.786126610553525
+284.0,12.796325356891554
+285.0,12.806520401933911
+286.0,12.816711770874782
+287.0,12.82689948882481
+288.0,12.837083580811841
+289.0,12.847264071781677
+290.0,12.857440986598782
+291.0,12.86761435004704
+292.0,12.877784186830448
+293.0,12.88795052157386
+294.0,12.89811337882367
+295.0,12.908272783048544
+296.0,12.918428758640104
+297.0,12.928581329913632
+298.0,12.938730521108749
+299.0,12.948876356390118
+300.0,12.95901885984811
+301.0,12.96915805549948
+302.0,12.979293967288056
+303.0,12.98942661908537
+304.0,12.999556034691352
+305.0,13.009682237834973
+306.0,13.019805252174894
+307.0,13.029925101300115
+308.0,13.040041808730622
+309.0,13.05015539791802
+310.0,13.060265892246168
+311.0,13.070373315031807
+312.0,13.080477689525186
+313.0,13.090579038910684
+314.0,13.100677386307424
+315.0,13.110772754769886
+316.0,13.120865167288516
+317.0,13.130954646790327
+318.0,13.1410412161395
+319.0,13.151124898137985
+320.0,13.16120571552608
+321.0,13.17128369098304
+322.0,13.18135884712764
+323.0,13.191431206518764
+324.0,13.201500791655986
+325.0,13.211567624980137
+326.0,13.221631728873879
+327.0,13.231693125662263
+328.0,13.241751837613299
+329.0,13.251807886938511
+330.0,13.261861295793492
+331.0,13.271912086278459
+332.0,13.281960280438796
+333.0,13.2920059002656
+334.0,13.302048967696226
+335.0,13.312089504614827
+336.0,13.322127532852875
+337.0,13.332163074189712
+338.0,13.342196150353066
+339.0,13.352226783019582
+340.0,13.362254993815336
+341.0,13.372280804316368
+342.0,13.382304236049187
+343.0,13.39232531049129
+344.0,13.402344049071672
+345.0,13.41236047317133
+346.0,13.422374604123767
+347.0,13.432386463215506
+348.0,13.442396071686572
+349.0,13.452403450730996
+350.0,13.46240862149731
+351.0,13.472411605089038
+352.0,13.482412422565183
+353.0,13.492411094940707
+354.0,13.50240764318702
+355.0,13.512402088232466
+356.0,13.522394450962787
+357.0,13.532384752221608
+358.0,13.542373012810907
+359.0,13.552359253491487
+360.0,13.562343494983441
+361.0,13.572325757966619
+362.0,13.582306063081099
+363.0,13.592284430927627
+364.0,13.602260882068109
+365.0,13.612235437026031
+366.0,13.622208116286942
+367.0,13.632178940298889
+368.0,13.642147929472873
+369.0,13.652115104183304
+370.0,13.662080484768428
+371.0,13.672044091530793
+372.0,13.682005944737675
+373.0,13.691966064621523
+374.0,13.701924471380398
+375.0,13.711881185178402
+376.0,13.721836226146126
+377.0,13.731789614381064
+378.0,13.741741369948059
+379.0,13.751691512879715
+380.0,13.761640063176843
+381.0,13.771587040808868
+382.0,13.78153246571426
+383.0,13.791476357800956
+384.0,13.80141873694678
+385.0,13.811359622999845
+386.0,13.821299035778997
+387.0,13.83123699507421
+388.0,13.841173520646997
+389.0,13.851108632230837
+390.0,13.861042349531566
+391.0,13.870974692227799
+392.0,13.88090567997133
+393.0,13.890835332387542
+394.0,13.900763669075804
+395.0,13.910690709609874
+396.0,13.920616473538312
+397.0,13.930540980384858
+398.0,13.94046424964885
+399.0,13.950386300805611
+400.0,13.960307153306847
+401.0,13.97022682658103
+402.0,13.980145340033815
+403.0,13.99006271304841
+404.0,13.99997896498597
+405.0,14.009894115185999
+406.0,14.01980818296672
+407.0,14.029721187625476
+408.0,14.039633148439108
+409.0,14.049544084664339
+410.0,14.059454015538165
+411.0,14.069362960278225
+412.0,14.079270938083202
+413.0,14.089177968133173
+414.0,14.099084069590022
+415.0,14.108989261597795
+416.0,14.118893563283086
+417.0,14.128796993755408
+418.0,14.13869957210758
+419.0,14.148601317416087
+420.0,14.158502248741463
+421.0,14.168402385128665
+422.0,14.178301745607437
+423.0,14.188200349192682
+424.0,14.198098214884846
+425.0,14.207995361670267
+426.0,14.217891808521562
+427.0,14.22778757439798
+428.0,14.23768267824578
+429.0,14.247577138998599
+430.0,14.257470975577803
+431.0,14.26736420689287
+432.0,14.277256851841743
+433.0,14.287148929311204
+434.0,14.297040458177225
+435.0,14.306931457305339
+436.0,14.316821945551
+437.0,14.326711941759953
+438.0,14.336601464768577
+439.0,14.346490533404266
+440.0,14.356379166485775
+441.0,14.366267382823583
+442.0,14.376155201220259
+443.0,14.386042640470812
+444.0,14.395929719363057
+445.0,14.40581645667797
+446.0,14.415702871190039
+447.0,14.425588981667637
+448.0,14.43547480687337
+449.0,14.44536036556442
+450.0,14.455245676492938
+451.0,14.465130758406358
+452.0,14.475015630046766
+453.0,14.484900310155497
+454.0,14.494784817466822
+455.0,14.504669170712951
+456.0,14.514553388623177
+457.0,14.52443748992423
+458.0,14.53432149334063
+459.0,14.544205417595048
+460.0,14.554089281408649
+461.0,14.563973103501448
+462.0,14.573856902592688
+463.0,14.58374069740116
+464.0,14.59362450664558
+465.0,14.60350834904494
+466.0,14.613392243318856
+467.0,14.623276208187924
+468.0,14.633160262374084
+469.0,14.64304442460096
+470.0,14.652928713594234
+471.0,14.662813148081971
+472.0,14.672697746795016
+473.0,14.682582528467309
+474.0,14.692467511836261
+475.0,14.702352715644434
+476.0,14.712238158634808
+477.0,14.72212385955845
+478.0,14.732009837170189
+479.0,14.741896110230117
+480.0,14.75178269750392
+481.0,14.761669617763241
+482.0,14.771556889786043
+483.0,14.781444532356968
+484.0,14.79133256426769
+485.0,14.80122100431727
+486.0,14.811109871312526
+487.0,14.820999184068384
+488.0,14.830888961408235
+489.0,14.840779222164308
+490.0,14.850669985178008
+491.0,14.860561269300309
+492.0,14.870453093392083
+493.0,14.880345476324484
+494.0,14.8902384369793
+495.0,14.900131994249323
+496.0,14.910026167038705
+497.0,14.919920974263329
+498.0,14.929816434851173
+499.0,14.939712567742669
+500.0,14.949609391891084
+501.0,14.959506926262865
+502.0,14.969405189838032
+503.0,14.979304201610525
+504.0,14.989203980588584
+505.0,14.999104545795126
+506.0,15.009005916268089
+507.0,15.01890811106084
+508.0,15.028811149242516
+509.0,15.038715049898412
+510.0,15.048619832130361
+511.0,15.058525515057095
+512.0,15.068432117814622
+513.0,15.07833965955662
+514.0,15.088248159454793
+515.0,15.098157636699264
+516.0,15.10806811049895
+517.0,15.11797960008194
+518.0,15.127892124695887
+519.0,15.137805703608375
+520.0,15.147720356107317
+521.0,15.157636101501339
+522.0,15.167552959120151
+523.0,15.17747094831496
+524.0,15.187390088458832
+525.0,15.197310398947097
+526.0,15.207231899197742
+527.0,15.217154608651796
+528.0,15.227078546773722
+529.0,15.237003733051818
+530.0,15.246930186998611
+531.0,15.256857928151252
+532.0,15.266786976071923
+533.0,15.27671735034822
+534.0,15.286649070593576
+535.0,15.296582156447643
+536.0,15.306516627576718
+537.0,15.31645250367413
+538.0,15.326389804460655
+539.0,15.336328549684934
+540.0,15.34626875912386
+541.0,15.356210452583015
+542.0,15.366153649897074
+543.0,15.376098370930208
+544.0,15.386044635576527
+545.0,15.395992463760473
+546.0,15.405941875437259
+547.0,15.415892890593277
+548.0,15.425845529246534
+549.0,15.435799811447062
+550.0,15.445755757277366
+551.0,15.455713386852837
+552.0,15.465672720322189
+553.0,15.475633777867891
+554.0,15.485596579706598
+555.0,15.495561146089605
+556.0,15.505527497303254
+557.0,15.515495653669403
+558.0,15.525465635545864
+559.0,15.535437463326833
+560.0,15.545411157443352
+561.0,15.555386738363755
+562.0,15.56536422659411
+563.0,15.575343642678686
+564.0,15.585325007200401
+565.0,15.595308340781276
+566.0,15.605293664082904
+567.0,15.615280997806906
+568.0,15.6252703626954
+569.0,15.635261779531463
+570.0,15.645255269139607
+571.0,15.655250852386239
+572.0,15.665248550180152
+573.0,15.675248383472987
+574.0,15.68525037325972
+575.0,15.695254540579143
+576.0,15.705260906514342
+577.0,15.715269492193196
+578.0,15.725280318788853
+579.0,15.735293407520235
+580.0,15.74530877965252
+581.0,15.755326456497658
+582.0,15.765346459414845
+583.0,15.775368809811054
+584.0,15.785393529141519
+585.0,15.795420638910262
+586.0,15.805450160670594
+587.0,15.81548211602563
+588.0,15.825516526628812
+589.0,15.835553414184428
+590.0,15.845592800448133
+591.0,15.855634707227482
+592.0,15.865679156382456
+593.0,15.875726169825999
+594.0,15.885775769524543
+595.0,15.895827977498572
+596.0,15.905882815823139
+597.0,15.915940306628428
+598.0,15.926000472100304
+599.0,15.93606333448086
+600.0,15.946128916068973
+601.0,15.956197239220883
+602.0,15.96626832635072
+603.0,15.97634219993111
+604.0,15.986418882493732
+605.0,15.99649839662988
+606.0,16.00658076499106
+607.0,16.016666010289565
+608.0,16.026754155299063
+609.0,16.03684522285518
+610.0,16.046939235856108
+611.0,16.057036217263192
+612.0,16.067136190101536
+613.0,16.077239177460605
+614.0,16.08734520249484
+615.0,16.097454288424274
+616.0,16.107566458535143
+617.0,16.117681736180515
+618.0,16.127800144780924
+619.0,16.137921707824976
+620.0,16.14804644887002
+621.0,16.158174391542754
+622.0,16.1683055595399
+623.0,16.178439976628834
+624.0,16.18857766664824
+625.0,16.19871865350879
+626.0,16.208862961193773
+627.0,16.219010613759806
+628.0,16.229161635337462
+629.0,16.239316050131972
+630.0,16.249473882423924
+631.0,16.259635156569914
+632.0,16.269799897003267
+633.0,16.279968128234724
+634.0,16.29013987485315
+635.0,16.300315161526235
+636.0,16.310494013001218
+637.0,16.3206764541056
+638.0,16.33086250974788
+639.0,16.341052204918263
+640.0,16.351245564689414
+641.0,16.361442614217196
+642.0,16.37164337874142
+643.0,16.381847883586595
+644.0,16.39205615416268
+645.0,16.402268215965865
+646.0,16.41248409457932
+647.0,16.422703815674
+648.0,16.43292740500941
+649.0,16.44315488843438
+650.0,16.453386291887906
+651.0,16.463621641399904
+652.0,16.47386096309205
+653.0,16.484104283178578
+654.0,16.494351627967106
+655.0,16.50460302385948
+656.0,16.514858497352574
+657.0,16.525118075039167
+658.0,16.535381783608774
+659.0,16.545649649848507
+660.0,16.555921700643932
+661.0,16.566197962979953
+662.0,16.57647846394167
+663.0,16.58676323071528
+664.0,16.597052290588955
+665.0,16.607345670953762
+666.0,16.61764339930454
+667.0,16.627945503240856
+668.0,16.63825201046788
+669.0,16.648562948797363
+670.0,16.658878346148533
+671.0,16.669198230549064
+672.0,16.67952263013605
+673.0,16.689851573156915
+674.0,16.700185087970443
+675.0,16.710523203047714
+676.0,16.72086594697312
+677.0,16.731213348445348
+678.0,16.74156543627839
+679.0,16.75192223940256
+680.0,16.762283786865517
+681.0,16.7726501078333
+682.0,16.783021231591366
+683.0,16.79339718754564
+684.0,16.803778005223595
+685.0,16.814163714275296
+686.0,16.824554344474496
+687.0,16.83494992571973
+688.0,16.8453504880354
+689.0,16.855756061572905
+690.0,16.866166676611744
+691.0,16.876582363560654
+692.0,16.887003152958766
+693.0,16.897429075476722
+694.0,16.90786016191788
+695.0,16.918296443219457
+696.0,16.928737950453723
+697.0,16.9391847148292
+698.0,16.94963676769186
+699.0,16.960094140526373
+700.0,16.970556864957285
+701.0,16.981024972750305
+702.0,16.99149849581355
+703.0,17.001977466198777
+704.0,17.012461916102723
+705.0,17.022951877868334
+706.0,17.0334473839861
+707.0,17.04394846709537
+708.0,17.054455159985665
+709.0,17.06496749559803
+710.0,17.075485507026396
+711.0,17.086009227518925
+712.0,17.09653869047941
+713.0,17.10707392946866
+714.0,17.117614978205907
+715.0,17.128161870570253
+716.0,17.138714640602064
+717.0,17.149273322504456
+718.0,17.159837950644746
+719.0,17.170408559555927
+720.0,17.180985183938187
+721.0,17.191567858660388
+722.0,17.202156618761617
+723.0,17.212751499452704
+724.0,17.223352536117815
+725.0,17.233959764315976
+726.0,17.244573219782712
+727.0,17.255192938431613
+728.0,17.265818956355975
+729.0,17.276451309830442
+730.0,17.287090035312655
+731.0,17.29773516944492
+732.0,17.30838674905591
+733.0,17.319044811162374
+734.0,17.329709392970845
+735.0,17.340380531879404
+736.0,17.351058265479452
+737.0,17.36174263155745
+738.0,17.372433668096775
+739.0,17.38313141327949
+740.0,17.39383590548822
+741.0,17.40454718330799
+742.0,17.415265285528122
+743.0,17.425990251144114
+744.0,17.436722119359583
+745.0,17.44746092958818
+746.0,17.458206721455582
+747.0,17.468959534801453
+748.0,17.47971940968147
+749.0,17.490486386369316
+750.0,17.501260505358776
+751.0,17.512041807365783
+752.0,17.522830333330507
+753.0,17.533626124419502
+754.0,17.544429222027812
+755.0,17.555239667781176
+756.0,17.566057503538197
+757.0,17.576882771392558
+758.0,17.587715513675278
+759.0,17.598555772956953
+760.0,17.609403592050075
+761.0,17.62025901401134
+762.0,17.631122082143975
+763.0,17.641992840000153
+764.0,17.65287133138335
+765.0,17.663757600350777
+766.0,17.674651691215868
+767.0,17.68555364855072
+768.0,17.696463517188636
+769.0,17.70738134222665
+770.0,17.718307169028115
+771.0,17.729241043225283
+772.0,17.74018301072195
+773.0,17.751133117696114
+774.0,17.76209141060268
+775.0,17.773057936176166
+776.0,17.784032741433496
+777.0,17.795015873676757
+778.0,17.806007380496037
+779.0,17.8170073097723
+780.0,17.828015709680262
+781.0,17.83903262869132
+782.0,17.850058115576523
+783.0,17.861092219409585
+784.0,17.872134989569894
+785.0,17.88318647574561
+786.0,17.894246727936775
+787.0,17.905315796458442
+788.0,17.916393731943902
+789.0,17.927480585347876
+790.0,17.938576407949814
+791.0,17.949681251357177
+792.0,17.96079516750882
+793.0,17.971918208678357
+794.0,17.983050427477618
+795.0,17.994191876860107
+796.0,18.005342610124554
+797.0,18.01650268091846
+798.0,18.027672143241723
+799.0,18.038851051450283
+800.0,18.050039460259857
+801.0,18.061237424749656
+802.0,18.072445000366237
+803.0,18.08366224292729
+804.0,18.094889208625606
+805.0,18.106125954032983
+806.0,18.117372536104263
+807.0,18.128629012181356
+808.0,18.139895439997385
+809.0,18.151171877680824
+810.0,18.162458383759727
+811.0,18.173755017166005
+812.0,18.185061837239758
+813.0,18.196378903733656
+814.0,18.207706276817408
+815.0,18.219044017082243
+816.0,18.2303921855455
+817.0,18.241750843655257
+818.0,18.253120053295014
+819.0,18.264499876788452
+820.0,18.275890376904275
+821.0,18.287291616861065
+822.0,18.29870366033228
+823.0,18.31012657145122
+824.0,18.321560414816187
+825.0,18.333005255495593
+826.0,18.344461159033226
+827.0,18.35592819145355
+828.0,18.36740641926709
+829.0,18.378895909475883
+830.0,18.390396729579017
+831.0,18.40190894757826
+832.0,18.413432631983717
+833.0,18.424967851819655
+834.0,18.4365146766303
+835.0,18.448073176485828
+836.0,18.459643421988364
+837.0,18.471225484278108
+838.0,18.482819435039513
+839.0,18.49442534650761
+840.0,18.506043291474363
+841.0,18.517673343295158
+842.0,18.529315575895353
+843.0,18.540970063776996
+844.0,18.552636882025528
+845.0,18.564316106316685
+846.0,18.57600781292346
+847.0,18.58771207872317
+848.0,18.599428981204646
+849.0,18.611158598475484
+850.0,18.622901009269494
+851.0,18.634656292954155
+852.0,18.646424529538262
+853.0,18.658205799679667
+854.0,18.67000018469311
+855.0,18.681807766558215
+856.0,18.693628627927577
+857.0,18.705462852134993
+858.0,18.7173105232038
+859.0,18.729171725855362
+860.0,18.74104654551768
+861.0,18.75293506833414
+862.0,18.764837381172395
+863.0,18.776753571633392
+864.0,18.78868372806053
+865.0,18.800627939548985
+866.0,18.812586295955153
+867.0,18.82455888790629
+868.0,18.836545806810236
+869.0,18.848547144865385
+870.0,18.860562995070737
+871.0,18.872593451236142
+872.0,18.884638607992745
+873.0,18.896698560803543
+874.0,18.90877340597415
+875.0,18.920863240663742
+876.0,18.932968162896145
+877.0,18.945088271571176
+878.0,18.957223666476096
+879.0,18.9693744482973
+880.0,18.981540718632214
+881.0,18.99372258000134
+882.0,19.00592013586055
+883.0,19.018133490613575
+884.0,19.030362749624718
+885.0,19.04260801923174
+886.0,19.054869406759043
+887.0,19.06714702053099
+888.0,19.079440969885543
+889.0,19.091751365188085
+890.0,19.104078317845477
+891.0,19.116421940320407
+892.0,19.12878234614593
+893.0,19.14115964994034
+894.0,19.153553967422223
+895.0,19.165965415425852
+896.0,19.178394111916784
+897.0,19.19084017600782
+898.0,19.203303727975165
+899.0,19.215784889274946
+900.0,19.22828378256001
+901.0,19.240800531697005
+902.0,19.25333526178381
+903.0,19.265888099167245
+904.0,19.27845917146116
+905.0,19.291048607564807
+906.0,19.303656537681583
+907.0,19.316283093338107
+908.0,19.328928407403676
+909.0,19.34159261411007
+910.0,19.354275849071726
+911.0,19.366978249306317
+912.0,19.379699953255706
+913.0,19.392441100807314
+914.0,19.40520183331587
+915.0,19.417982293625634
+916.0,19.43078262609299
+917.0,19.443602976609522
+918.0,19.45644349262554
+919.0,19.46930432317403
+920.0,19.482185618895123
+921.0,19.495087532061028
+922.0,19.50801021660146
+923.0,19.52095382812957
+924.0,19.533918523968424
+925.0,19.546904463177995
+926.0,19.559911806582676
+927.0,19.572940716799437
+928.0,19.58599135826645
+929.0,19.599063897272398
+930.0,19.61215850198631
+931.0,19.62527534248809
+932.0,19.63841459079962
+933.0,19.651576420916545
+934.0,19.664761008840717
+935.0,19.67796853261333
+936.0,19.69119917234878
+937.0,19.70445311026917
+938.0,19.71773053073966
+939.0,19.731031620304506
+940.0,19.7443565677239
+941.0,19.75770556401161
+942.0,19.771078802473454
+943.0,19.784476478746594
+944.0,19.79789879083972
+945.0,19.811345939174128
+946.0,19.824818126625672
+947.0,19.838315558567718
+948.0,19.851838442915017
+949.0,19.86538699016858
+950.0,19.878961413461603
+951.0,19.89256192860639
+952.0,19.90618875414241
+953.0,19.919842111385403
+954.0,19.93352222447769
+955.0,19.947229320439607
+956.0,19.96096362922217
+957.0,19.974725383760966
+958.0,19.98851482003133
+959.0,20.002332177003414
+960.0,20.016177663280935
+961.0,20.03005140786785
+962.0,20.043953528306027
+963.0,20.057884142768465
+964.0,20.071843370062428
+965.0,20.085831329632516
+966.0,20.099848141563726
+967.0,20.113893926584574
+968.0,20.127968806070104
+969.0,20.14207290204499
+970.0,20.156206337186536
+971.0,20.170369234827735
+972.0,20.184561718960293
+973.0,20.198783914237612
+974.0,20.213035945977797
+975.0,20.22731794016664
+976.0,20.241630023460555
+977.0,20.255972323189546
+978.0,20.27034496736013
+979.0,20.28474808465822
+980.0,20.29918180445202
+981.0,20.313646256794915
+982.0,20.32814157242827
+983.0,20.342667882784255
+984.0,20.357225319988668
+985.0,20.37181401686364
+986.0,20.386434106930434
+987.0,20.401085724412084
+988.0,20.41576900423613
+989.0,20.430484082037214
+990.0,20.445231094159727
+991.0,20.460010177660322
+992.0,20.474821470310527
+993.0,20.489665110599184
+994.0,20.504541237734912
+995.0,20.519449991648543
+996.0,20.534391512995487
+997.0,20.549365943158026
+998.0,20.56437342424763
+999.0,20.57941409910716
+1000.0,20.594488111313062
+1001.0,20.609595605177475
+1002.0,20.624736725750296
+1003.0,20.639911618821202
+1004.0,20.655120430921595
+1005.0,20.670363309326493
+1006.0,20.685640402056347
+1007.0,20.700951857878795
+1008.0,20.71629782631037
+1009.0,20.7316784576181
+1010.0,20.74709390282105
+1011.0,20.76254431369181
+1012.0,20.778029842757842
+1013.0,20.793550643302844
+1014.0,20.8091068693679
+1015.0,20.824698675752693
+1016.0,20.840326218016475
+1017.0,20.855989652479053
+1018.0,20.87168913622166
+1019.0,20.887424827087667
+1020.0,20.903196883683254
+1021.0,20.919005465377968
+1022.0,20.93485073230513
+1023.0,20.95073284536218
+1024.0,20.96665196621087
+1025.0,20.98260825727736
+1026.0,20.99860188175216
+1027.0,21.014633003589985
+1028.0,21.030701787509447
+1029.0,21.046808398992624
+1030.0,21.062953004284484
+1031.0,21.07913577039216
+1032.0,21.095356865084096
+1033.0,21.11161645688903
+1034.0,21.12791471509479
+1035.0,21.14425180974701
+1036.0,21.160627911647563
+1037.0,21.177043192352933
+1038.0,21.193497824172336
+1039.0,21.2099919801657
+1040.0,21.226525834141444
+1041.0,21.243099560654056
+1042.0,21.25971333500149
+1043.0,21.276367333222346
+1044.0,21.293061732092895
+1045.0,21.30979670912375
+1046.0,21.326572442556518
+1047.0,21.34338911136004
+1048.0,21.360246895226513
+1049.0,21.377145974567338
+1050.0,21.39408653050871
+1051.0,21.41106874488698
+1052.0,21.42809280024372
+1053.0,21.4451588798206
+1054.0,21.46226716755391
+1055.0,21.479417848068817
+1056.0,21.496611106673406
+1057.0,21.513847129352303
+1058.0,21.53112610276014
+1059.0,21.54844821421458
+1060.0,21.565813651689115
+1061.0,21.58322260380548
+1062.0,21.60067525982576
+1063.0,21.618171809644167
+1064.0,21.63571244377842
+1065.0,21.653297353360827
+1066.0,21.67092673012893
+1067.0,21.68860076641582
+1068.0,21.706319655140053
+1069.0,21.724083589795146
+1070.0,21.74189276443871
+1071.0,21.759747373681122
+1072.0,21.77764761267379
+1073.0,21.79559367709698
+1074.0,21.81358576314718
+1075.0,21.83162406752404
+1076.0,21.849708787416805
+1077.0,21.867840120490275
+1078.0,21.88601826487032
+1079.0,21.904243419128786
+1080.0,21.922515782268007
+1081.0,21.94083555370469
+1082.0,21.95920293325336
+1083.0,21.977618121109128
+1084.0,21.996081317830008
+1085.0,22.014592724318632
+1086.0,22.033152541803297
+1087.0,22.051760971818506
+1088.0,22.070418216184855
+1089.0,22.089124476988275
+1090.0,22.107879956558662
+1091.0,22.126684857447817
+1092.0,22.145539382406724
+1093.0,22.164443734362177
+1094.0,22.183398116392645
+1095.0,22.20240273170346
+1096.0,22.221457783601274
+1097.0,22.240563475467734
+1098.0,22.259720010732476
+1099.0,22.278927592845235
+1100.0,22.29818642524724
+1101.0,22.317496711341793
+1102.0,22.33685865446395
+1103.0,22.356272457849492
+1104.0,22.375738324602896
+1105.0,22.395256457664534
+1106.0,22.414827059776936
+1107.0,22.434450333450147
+1108.0,22.45412648092616
+1109.0,22.4738557041424
+1110.0,22.493638204694328
+1111.0,22.513474183796866
+1112.0,22.53336384224505
+1113.0,22.55330738037355
+1114.0,22.573304998015185
+1115.0,22.59335689445835
+1116.0,22.613463268403454
+1117.0,22.633624317918176
+1118.0,22.653840240391748
+1119.0,22.674111232487917
+1120.0,22.694437490096977
+1121.0,22.714819208286514
+1122.0,22.735256581250972
+1123.0,22.75574980226011
+1124.0,22.77629906360614
+1125.0,22.796904556549713
+1126.0,22.81756647126459
+1127.0,22.838284996781095
+1128.0,22.85906032092825
+1129.0,22.879892630274618
+1130.0,22.900782110067805
+1131.0,22.92172894417263
+1132.0,22.942733315007978
+1133.0,22.963795403482177
+1134.0,22.98491538892711
+1135.0,23.006093449030796
+1136.0,23.027329759768634
+1137.0,23.048624495333176
+1138.0,23.069977828062434
+1139.0,23.091389928366713
+1140.0,23.112860964654004
+1141.0,23.134391103253794
+1142.0,23.155980508339454
+1143.0,23.17762934184902
+1144.0,23.199337763404486
+1145.0,23.22110593022954
+1146.0,23.242933997065727
+1147.0,23.264822116087004
+1148.0,23.286770436812823
+1149.0,23.308779106019458
+1150.0,23.3308482676499
+1151.0,23.352978062722045
+1152.0,23.37516862923532
+1153.0,23.397420102075625
+1154.0,23.419732612918757
+1155.0,23.442106290132134
+1156.0,23.46454125867491
+1157.0,23.4870376399965
+1158.0,23.509595551933465
+1159.0,23.53221510860474
+1160.0,23.554896420305315
+1161.0,23.577639593398274
+1162.0,23.60044473020521
+1163.0,23.6233119288951
+1164.0,23.646241283371534
+1165.0,23.669232883158454
+1166.0,23.692286813284266
+1167.0,23.715403154164473
+1168.0,23.738581981482717
+1169.0,23.761823366070438
+1170.0,23.785127373784896
+1171.0,23.80849406538596
+1172.0,23.831923496411218
+1173.0,23.855415717049937
+1174.0,23.878970772015453
+1175.0,23.902588700416455
+1176.0,23.926269535626776
+1177.0,23.950013305154044
+1178.0,23.97382003050714
+1179.0,23.997689727062465
+1180.0,24.021622403929083
+1181.0,24.04561806381284
+1182.0,24.0696767028795
+1183.0,24.0937983106169
+1184.0,24.11798286969622
+1185.0,24.142230355832464
+1186.0,24.166540737644237
+1187.0,24.190913976512704
+1188.0,24.215350026440134
+1189.0,24.239848833907768
+1190.0,24.264410337733352
+1191.0,24.289034468928246
+1192.0,24.31372115055435
+1193.0,24.338470297580756
+1194.0,24.363281816740425
+1195.0,24.38815560638685
+1196.0,24.413091556350864
+1197.0,24.438089547797677
+1198.0,24.46314945308427
+1199.0,24.488271135617293
+1200.0,24.51345444971155
+1201.0,24.538699240449112
+1202.0,24.564005343539446
+1203.0,24.589372585180367
+1204.0,24.614800781920156
+1205.0,24.64028974052096
+1206.0,24.66583925782358
+1207.0,24.69144912061371
+1208.0,24.717119105490045
+1209.0,24.742848978734045
+1210.0,24.76863849618183
+1211.0,24.79448740309821
+1212.0,24.820395434052976
+1213.0,24.846362312799823
+1214.0,24.872387752157746
+1215.0,24.898471453895485
+1216.0,24.924613108618786
+1217.0,24.950812395660957
+1218.0,24.977068982976785
+1219.0,25.00338252703996
+1220.0,25.029752672744234
+1221.0,25.056179053308544
+1222.0,25.082661290186138
+1223.0,25.1091989929781
+1224.0,25.135791759351296
+1225.0,25.162439174960912
+1226.0,25.189140813377993
+1227.0,25.21589623602187
+1228.0,25.242704992097927
+1229.0,25.269566618540697
+1230.0,25.29648063996262
+1231.0,25.323446568608492
+1232.0,25.35046390431602
+1233.0,25.377532134482394
+1234.0,25.404650734037208
+1235.0,25.431819165421928
+1236.0,25.459036878576004
+1237.0,25.486303310929753
+1238.0,25.51361788740434
+1239.0,25.540980020418875
+1240.0,25.568389109904757
+1241.0,25.595844543327573
+1242.0,25.62334569571656
+1243.0,25.650891929701764
+1244.0,25.678482595559124
+1245.0,25.706117031263474
+1246.0,25.73379456254971
+1247.0,25.761514502982138
+1248.0,25.78927615403208
+1249.0,25.81707880516393
+1250.0,25.844921733929677
+1251.0,25.872804206071915
+1252.0,25.90072547563551
+1253.0,25.92868478508788
+1254.0,25.95668136544797
+1255.0,25.984714436423978
+1256.0,26.012783206559707
+1257.0,26.040886873389834
+1258.0,26.06902462360365
+1259.0,26.097195633217787
+1260.0,26.125399067757403
+1261.0,26.1536340824461
+1262.0,26.181899822404414
+1263.0,26.210195422856742
+1264.0,26.238520009346793
+1265.0,26.266872697961215
+1266.0,26.295252595561625
+1267.0,26.32365880002452
+1268.0,26.352090400489303
+1269.0,26.380546477614015
+1270.0,26.40902610383876
+1271.0,26.437528343656584
+1272.0,26.466052253891615
+1273.0,26.494596883984272
+1274.0,26.523161276283354
+1275.0,26.55174446634479
+1276.0,26.58034548323671
+1277.0,26.60896334985075
+1278.0,26.63759708321923
+1279.0,26.66624569483787
+1280.0,26.69490819099391
+1281.0,26.723583573099354
+1282.0,26.752270838028785
+1283.0,26.780968978461697
+1284.0,26.809676983229025
+1285.0,26.838393837663332
+1286.0,26.867118523952477
+1287.0,26.895850021496514
+1288.0,26.92458730726718
+1289.0,26.95332935616999
+1290.0,26.982075141408206
+1291.0,27.010823634848663
+1292.0,27.039573807388862
+1293.0,27.068324629325115
+1294.0,27.097075070720727
+1295.0,27.125824101775937
+1296.0,27.15457069319562
+1297.0,27.183313816558126
+1298.0,27.21205244468251
+1299.0,27.24078555199473
+1300.0,27.26951211489212
+1301.0,27.29823111210605
+1302.0,27.326941525062104
+1303.0,27.35564233823767
+1304.0,27.3843325395165
+1305.0,27.413011120539867
+1306.0,27.441677077054056
+1307.0,27.47032940925377
+1308.0,27.49896712212129
+1309.0,27.52758922576078
+1310.0,27.556194735727914
+1311.0,27.584782673353924
+1312.0,27.61335206606432
+1313.0,27.641901947691714
+1314.0,27.67043135878257
+1315.0,27.69893934689761
+1316.0,27.72742496690566
+1317.0,27.755887281270688
+1318.0,27.7843253603318
+1319.0,27.812738282576063
+1320.0,27.84112513490386
+1321.0,27.869485012886663
+1322.0,27.897817021017026
+1323.0,27.926120272950747
+1324.0,27.954393891740853
+1325.0,27.982637010063534
+1326.0,28.010848770435608
+1327.0,28.03902832542385
+1328.0,28.067174837845517
+1329.0,28.09528748096064
+1330.0,28.123365438655433
+1331.0,28.151407905617248
+1332.0,28.17941408750065
+1333.0,28.207383201084905
+1334.0,28.2353144744227
+1335.0,28.263207146980033
+1336.0,28.29106046976755
+1337.0,28.31887370546307
+1338.0,28.346646128525474
+1339.0,28.374377025299957
+1340.0,28.40206569411485
+1341.0,28.429711445369755
+1342.0,28.457313601615464
+1343.0,28.484871497625328
+1344.0,28.512384480458692
+1345.0,28.539851909515917
+1346.0,28.567273156585532
+1347.0,28.594647605883505
+1348.0,28.621974654084642
+1349.0,28.649253710346468
+1350.0,28.67648419632552
+1351.0,28.70366554618637
+1352.0,28.730797206603352
+1353.0,28.757878636755386
+1354.0,28.784909308313782
+1355.0,28.811888705423428
+1356.0,28.838816324677428
+1357.0,28.865691675085277
+1358.0,28.892514278035065
+1359.0,28.919283667249335
+1360.0,28.945999388735444
+1361.0,28.972661000730117
+1362.0,28.999268073638497
+1363.0,29.025820189968048
+1364.0,29.05231694425726
+1365.0,29.07875794299953
+1366.0,29.105142804562306
+1367.0,29.13147115910171
+1368.0,29.157742648472784
+1369.0,29.183956926135664
+1370.0,29.210113657057775
+1371.0,29.236212517612177
+1372.0,29.26225319547238
+1373.0,29.2882353895038
+1374.0,29.314158809651815
+1375.0,29.340023176826943
+1376.0,29.36582822278705
+1377.0,29.39157369001686
+1378.0,29.417259331604914
+1379.0,29.442884911118224
+1380.0,29.46845020247466
+1381.0,29.49395498981329
+1382.0,29.51939906736285
+1383.0,29.54478223930854
+1384.0,29.570104319657116
+1385.0,29.59536513210064
+1386.0,29.62056450987894
+1387.0,29.645702295640856
+1388.0,29.670778341304565
+1389.0,29.695792507916895
+1390.0,29.720744665512072
+1391.0,29.745634692969688
+1392.0,29.77046247787216
+1393.0,29.795227916362002
+1394.0,29.819930912998522
+1395.0,29.84457138061464
+1396.0,29.869149240173446
+1397.0,29.89366442062493
+1398.0,29.918116858762783
+1399.0,29.942506499081468
+1400.0,29.96683329363362
+1401.0,29.99109720188785
+1402.0,30.015298223337105
+1403.0,30.039436793186027
+1404.0,30.06351364324267
+1405.0,30.087529499115906
+1406.0,30.11148507405299
+1407.0,30.135381069238854
+1408.0,30.159218174086313
+1409.0,30.182997066517373
+1410.0,30.206718413235958
+1411.0,30.230382869992518
+1412.0,30.253991081840592
+1413.0,30.27754368338593
+1414.0,30.30104129902802
+1415.0,30.32448454319476
+1416.0,30.34787402057017
+1417.0,30.371210326315484
+1418.0,30.394494046284006
+1419.0,30.41772575722968
+1420.0,30.440906027009852
+1421.0,30.464035414782234
+1422.0,30.487114471196424
+1423.0,30.510143738579927
+1424.0,30.533123751119287
+1425.0,30.556055035035964
+1426.0,30.578938108757562
+1427.0,30.601773483084344
+1428.0,30.62456166135128
+1429.0,30.64730313958567
+1430.0,30.66999840666065
+1431.0,30.692647944444612
+1432.0,30.71525222794656
+1433.0,30.73781172545782
+1434.0,30.76032689868996
+1435.0,30.782798202909145
+1436.0,30.805226087067044
+1437.0,30.827610993928367
+1438.0,30.849953360195187
+1439.0,30.872253616628065
+1440.0,30.894512188164153
+1441.0,30.916729494032356
+1442.0,30.938905947865535
+1443.0,30.961041957810117
+1444.0,30.98313792663276
+1445.0,31.00519425182463
+1446.0,31.027211325702982
+1447.0,31.0491895355104
+1448.0,31.07112926351151
+1449.0,31.093030887087522
+1450.0,31.114894778828447
+1451.0,31.136721306623038
+1452.0,31.15851083374687
+1453.0,31.180263718948055
+1454.0,31.20198031653112
+1455.0,31.223660976438953
+1456.0,31.245306044332793
+1457.0,31.266915861670416
+1458.0,31.28849076578249
+1459.0,31.310031089947312
+1460.0,31.331537163463693
+1461.0,31.35300931172236
+1462.0,31.374447856275637
+1463.0,31.39585311490566
+1464.0,31.41722540169103
+1465.0,31.43856502707206
+1466.0,31.459872297914522
+1467.0,31.481147517572005
+1468.0,31.502390985947006
+1469.0,31.523602999550626
+1470.0,31.544783851560982
+1471.0,31.565933831880432
+1472.0,31.587053227191504
+1473.0,31.608142321011705
+1474.0,31.629201393747167
+1475.0,31.65023072274511
+1476.0,31.671230582345313
+1477.0,31.692201243930416
+1478.0,31.71314297597525
+1479.0,31.734056044095144
+1480.0,31.7549407110932
+1481.0,31.775797237006664
+1482.0,31.796625879152323
+1483.0,31.817426892171014
+1484.0,31.8382005280712
+1485.0,31.85894703627173
+1486.0,31.879666663643683
+1487.0,31.90035965455146
+1488.0,31.921026250893014
+1489.0,31.941666692139297
+1490.0,31.962281215372954
+1491.0,31.98287005532631
+1492.0,32.003433444418505
+1493.0,32.02397161279199
+1494.0,32.044484788348406
+1495.0,32.0649731967836
+1496.0,32.08543706162212
+1497.0,32.10587660425103
+1498.0,32.12629204395301
+1499.0,32.14668359793894
+1500.0,32.16705148137987
+1501.0,32.18739590743833
+1502.0,32.20771708729907
+1503.0,32.22801523019936
+1504.0,32.24829054345854
+1505.0,32.2685432325072
+1506.0,32.28877350091579
+1507.0,32.3089815504226
+1508.0,32.329167580961446
+1509.0,32.34933179068865
+1510.0,32.36947437600973
+1511.0,32.38959553160546
+1512.0,32.40969545045758
+1513.0,32.429774323874
+1514.0,32.44983234151359
+1515.0,32.46986969141052
+1516.0,32.48988655999821
+1517.0,32.509883132132835
+1518.0,32.52985959111643
+1519.0,32.54981611871963
+1520.0,32.56975289520406
+1521.0,32.589670099344175
+1522.0,32.60956790844897
+1523.0,32.62944649838315
+1524.0,32.64930604358802
+1525.0,32.669146717102045
+1526.0,32.68896869058097
+1527.0,32.70877213431781
+1528.0,32.728557217262264
+1529.0,32.74832410704
+1530.0,32.76807296997157
+1531.0,32.78780397109097
+1532.0,32.80751727416399
+1533.0,32.827213041706244
+1534.0,32.84689143500083
+1535.0,32.86655261411584
+1536.0,32.88619673792155
+1537.0,32.90582396410724
+1538.0,32.925434449197965
+1539.0,32.94502834857077
+1540.0,32.96460581647104
+1541.0,32.984167006028194
+1542.0,33.003712069271415
+1543.0,33.02324115714503
+1544.0,33.042754419523725
+1545.0,33.062252005227435
+1546.0,33.08173406203609
+1547.0,33.10120073670413
+1548.0,33.12065217497472
+1549.0,33.14008852159396
+1550.0,33.15950992032459
+1551.0,33.17891651395977
+1552.0,33.19830844433651
+1553.0,33.21768585234898
+1554.0,33.237048877961456
+1555.0,33.25639766022144
+1556.0,33.27573233727215
+1557.0,33.295053046365204
+1558.0,33.31435992387287
+1559.0,33.333653105300336
+1560.0,33.35293272529764
+1561.0,33.3721989176716
+1562.0,33.391451815397375
+1563.0,33.41069155063015
+1564.0,33.42991825471635
+1565.0,33.44913205820494
+1566.0,33.46833309085842
+1567.0,33.48752148166384
+1568.0,33.50669735884341
+1569.0,33.52586084986526
+1570.0,33.54501208145389
+1571.0,33.56415117960043
+1572.0,33.58327826957298
+1573.0,33.6023934759266
+1574.0,33.62149692251327
+1575.0,33.640588732491764
+1576.0,33.65966902833726
+1577.0,33.678737931850996
+1578.0,33.697795564169674
+1579.0,33.7168420457748
+1580.0,33.73587749650193
+1581.0,33.75490203554973
+1582.0,33.77391578148904
+1583.0,33.79291885227168
+1584.0,33.81191136523929
+1585.0,33.830893437131934
+1586.0,33.84986518409678
+1587.0,33.86882672169643
+1588.0,33.887778164917385
+1589.0,33.90671962817828
+1590.0,33.925651225338065
+1591.0,33.94457306970407
+1592.0,33.963485274040025
+1593.0,33.9823879505739
+1594.0,34.00128121100578
+1595.0,34.020165166515525
+1596.0,34.03903992777047
+1597.0,34.05790560493285
+1598.0,34.07676230766745
+1599.0,34.09561014514886
+1600.0,34.114449226068785
+1601.0,34.13327965864335
+1602.0,34.15210155062018
+1603.0,34.17091500928554
+1604.0,34.18972014147123
+1605.0,34.20851705356167
+1606.0,34.22730585150066
+1607.0,34.246086640798175
+1608.0,34.264859526537116
+1609.0,34.28362461337994
+1610.0,34.302382005575296
+1611.0,34.321131806964495
+1612.0,34.33987412098801
+1613.0,34.35860905069186
+1614.0,34.37733669873396
+1615.0,34.396057167390374
+1616.0,34.41477055856157
+1617.0,34.433476973778596
+1618.0,34.45217651420914
+1619.0,34.4708692806636
+1620.0,34.48955537360111
+1621.0,34.508234893135466
+1622.0,34.52690793904101
+1623.0,34.54557461075851
+1624.0,34.5642350074009
+1625.0,34.582889227759075
+1626.0,34.60153737030755
+1627.0,34.62017953321016
+1628.0,34.6388158143256
+1629.0,34.65744631121305
+1630.0,34.67607112113762
+1631.0,34.6946903410759
+1632.0,34.71330406772133
+1633.0,34.73191239748962
+1634.0,34.75051542652411
+1635.0,34.76911325070102
+1636.0,34.78770596563479
+1637.0,34.80629366668328
+1638.0,34.824876448952985
+1639.0,34.84345440730415
+1640.0,34.862027636355975
+1641.0,34.88059623049162
+1642.0,34.89916028386331
+1643.0,34.917719890397365
+1644.0,34.93627514379915
+1645.0,34.95482613755811
+1646.0,34.97337296495261
+1647.0,34.99191571905491
+1648.0,35.01045449273598
+1649.0,35.0289893786704
+1650.0,35.047520469341144
+1651.0,35.06604785704433
+1652.0,35.0845716338941
+1653.0,35.10309189182722
+1654.0,35.12160872260788
+1655.0,35.14012221783237
+1656.0,35.15863246893374
+1657.0,35.177139567186416
+1658.0,35.195643603710856
+1659.0,35.21414466947814
+1660.0,35.23264285531456
+1661.0,35.251138251906156
+1662.0,35.26963094980327
+1663.0,35.288121039425086
+1664.0,35.30660861106409
+1665.0,35.32509375489059
+1666.0,35.34357656095719
+1667.0,35.36205711920321
+1668.0,35.38053551945917
+1669.0,35.39901185145117
+1670.0,35.41748620480535
+1671.0,35.43595866905221
+1672.0,35.45442933363106
+1673.0,35.472898287894374
+1674.0,35.49136562111215
+1675.0,35.50983142247621
+1676.0,35.528295781104624
+1677.0,35.54675878604596
+1678.0,35.56522052628366
+1679.0,35.583681090740285
+1680.0,35.60214056828186
+1681.0,35.62059904772218
+1682.0,35.63905661782705
+1683.0,35.65751336731861
+1684.0,35.67596938487956
+1685.0,35.69442475915747
+1686.0,35.712879578765516
+1687.0,35.731333932301915
+1688.0,35.74978790832951
+1689.0,35.768241595397896
+1690.0,35.78669508204277
+1691.0,35.80514845679026
+1692.0,35.82360180816111
+1693.0,35.84205522467492
+1694.0,35.860508794854425
+1695.0,35.87896260722975
+1696.0,35.89741675034263
+1697.0,35.91587131275066
+1698.0,35.93432638303569
+1699.0,35.952782049792994
+1700.0,35.971238401656194
+1701.0,35.98969552728892
+1702.0,36.00815351539196
+1703.0,36.0266124547075
+1704.0,36.04507243402346
+1705.0,36.063533542177765
+1706.0,36.081995868062556
+1707.0,36.1004595006286
+1708.0,36.11892452888953
+1709.0,36.13739104192614
+1710.0,36.15585912889081
+1711.0,36.17432887901167
+1712.0,36.19280038159708
+1713.0,36.21127372603997
+1714.0,36.229749001822135
+1715.0,36.24822629851864
+1716.0,36.26670570580223
+1717.0,36.28518731344774
+1718.0,36.30367121133648
+1719.0,36.32215748946065
+1720.0,36.34064623792784
+1721.0,36.35913754696546
+1722.0,36.37763150692521
+1723.0,36.39612820828758
+1724.0,36.414627741666386
+1725.0,36.4331301978133
+1726.0,36.45163566762232
+1727.0,36.47014424213445
+1728.0,36.488656012542236
+1729.0,36.507171070194374
+1730.0,36.52568950660034
+1731.0,36.544211413435036
+1732.0,36.56273688254355
+1733.0,36.581266005945714
+1734.0,36.59979887584095
+1735.0,36.618335584612964
+1736.0,36.6368762248345
+1737.0,36.65542088927223
+1738.0,36.67396967089147
+1739.0,36.692522662861165
+1740.0,36.71107995855861
+1741.0,36.72964165157454
+1742.0,36.74820783571794
+1743.0,36.7667786050211
+1744.0,36.7853540537446
+1745.0,36.803934276382314
+1746.0,36.82251936766655
+1747.0,36.841109422573105
+1748.0,36.85970453632641
+1749.0,36.87830480440476
+1750.0,36.89691032254546
+1751.0,36.91552118675009
+1752.0,36.93413749328985
+1753.0,36.95275933871083
+1754.0,36.97138681983938
+1755.0,36.990020033787545
+1756.0,37.00865907795853
+1757.0,37.02730405005211
+1758.0,37.04595504807029
+1759.0,37.06461217032276
+1760.0,37.083275515432646
+1761.0,37.10194518234205
+1762.0,37.12062127031789
+1763.0,37.1393038789576
+1764.0,37.15799310819493
+1765.0,37.17668905830591
+1766.0,37.19539182991465
+1767.0,37.2141015239994
+1768.0,37.23281824189852
+1769.0,37.25154208531659
+1770.0,37.270273156330546
+1771.0,37.289011557395824
+1772.0,37.3077573913527
+1773.0,37.326510761432495
+1774.0,37.34527177126402
+1775.0,37.36404052487996
+1776.0,37.3828171267234
+1777.0,37.40160168165438
+1778.0,37.42039429495644
+1779.0,37.43919507234341
+1780.0,37.458004119966155
+1781.0,37.47682154441931
+1782.0,37.49564745274825
+1783.0,37.514481952456066
+1784.0,37.53332515151051
+1785.0,37.552177158351235
+1786.0,37.57103808189689
+1787.0,37.5899080315524
+1788.0,37.608787117216366
+1789.0,37.62767544928841
+1790.0,37.64657313867676
+1791.0,37.665480296805754
+1792.0,37.684397035623604
+1793.0,37.70332346761008
+1794.0,37.722259705784424
+1795.0,37.74120586371323
+1796.0,37.76016205551851
+1797.0,37.77912839588582
+1798.0,37.79810500007245
+1799.0,37.817091983915766
+1800.0,37.83608946384162
+1801.0,37.85509755687286
+1802.0,37.87411638063794
+1803.0,37.89314605337965
+1804.0,37.912186693963946
+1805.0,37.93123842188887
+1806.0,37.950301357293625
+1807.0,37.9693756209677
+1808.0,37.988461334360196
+1809.0,38.00755861958912
+1810.0,38.02666759945103
+1811.0,38.045788397430535
+1812.0,38.06492113771012
+1813.0,38.08406594518006
+1814.0,38.10322294544832
+1815.0,38.122392264850795
+1816.0,38.14157403046152
+1817.0,38.16076837010317
+1818.0,38.179975412357464
+1819.0,38.19919528657599
+1820.0,38.218428122890955
+1821.0,38.23767405222624
+1822.0,38.256933206308425
+1823.0,38.27620571767816
+1824.0,38.29549171970159
+1825.0,38.31479134658191
+1826.0,38.33410473337117
+1827.0,38.353432015982186
+1828.0,38.37277333120056
+1829.0,38.3921288166971
+1830.0,38.41149861104008
+1831.0,38.43088285370799
+1832.0,38.450281685102276
+1833.0,38.46969524656025
+1834.0,38.48912368036847
+1835.0,38.508567129775884
+1836.0,38.528025739007475
+1837.0,38.54749965327806
+1838.0,38.56698901880619
+1839.0,38.58649398282838
+1840.0,38.60601469361344
+1841.0,38.62555130047715
+1842.0,38.64510395379697
+1843.0,38.664672805027216
+1844.0,38.684258006714295
+1845.0,38.7038597125122
+1846.0,38.72347807719824
+1847.0,38.743113256689135
+1848.0,38.7627654080572
+1849.0,38.78243468954687
+1850.0,38.80212126059145
+1851.0,38.82182528183017
+1852.0,38.841546915125484
+1853.0,38.861286323580636
+1854.0,38.881043671557556
+1855.0,38.900819124694976
+1856.0,38.920612849926904
+1857.0,38.940425015501376
+1858.0,38.96025579099947
+1859.0,38.980105347354744
+1860.0,38.99997385687288
+1861.0,39.01986149325167
+1862.0,39.03976843160149
+1863.0,39.05969484846589
+1864.0,39.0796409218426
+1865.0,39.09960683120513
+1866.0,39.119592757524295
+1867.0,39.13959888329052
+1868.0,39.15962539253627
+1869.0,39.17967247085902
+1870.0,39.19974030544452
+1871.0,39.21982908509052
+1872.0,39.2399390002309
+1873.0,39.260070242960225
+1874.0,39.28022300705875
+1875.0,39.300397488017786
+1876.0,39.320593883065676
+1877.0,39.34081239119407
+1878.0,39.361053213184796
+1879.0,39.38131655163717
+1880.0,39.401602610995766
+1881.0,39.42191159757878
+1882.0,39.442243719606815
+1883.0,39.462599187232314
+1884.0,39.482978212569414
+1885.0,39.50338100972445
+1886.0,39.52380779482701
+1887.0,39.54425878606149
+1888.0,39.56473420369942
+1889.0,39.58523427013221
+1890.0,39.60575920990468
+1891.0,39.62630924974907
+1892.0,39.646884618619914
+1893.0,39.667485547729406
+1894.0,39.6881122705835
+1895.0,39.70876502301883
+1896.0,39.72944404324014
+1897.0,39.75014957185866
+1898.0,39.77088185193112
+1899.0,39.79164112899963
+1900.0,39.81242765113218
+1901.0,39.83324166896428
+1902.0,39.854083435740996
+1903.0,39.874953207360335
+1904.0,39.895851242417045
+1905.0,39.91677780224773
+1906.0,39.93773315097654
+1907.0,39.95871755556205
+1908.0,39.979731285845055
+1909.0,40.00077461459522
+1910.0,40.021847772740784
+1911.0,40.042950832445875
+1912.0,40.06408382979333
+1913.0,40.08524680047228
+1914.0,40.10643977977171
+1915.0,40.12766280257401
+1916.0,40.14891590334838
+1917.0,40.17019911614438
+1918.0,40.191512474585174
+1919.0,40.21285601186093
+1920.0,40.23422976072196
+1921.0,40.25563375347204
+1922.0,40.277068021961455
+1923.0,40.298532597580085
+1924.0,40.320027511250494
+1925.0,40.34155279342083
+1926.0,40.36310847405771
+1927.0,40.38469458263918
+1928.0,40.406311148147346
+1929.0,40.427958199061294
+1930.0,40.44963576334959
+1931.0,40.47134386846307
+1932.0,40.49308254132731
+1933.0,40.51485180833508
+1934.0,40.53665169533897
+1935.0,40.558482227643644
+1936.0,40.58034342999823
+1937.0,40.60223532658862
+1938.0,40.6241579410297
+1939.0,40.646111296357496
+1940.0,40.668095415021334
+1941.0,40.690110318875895
+1942.0,40.71215602917322
+1943.0,40.73423256655468
+1944.0,40.75633995104291
+1945.0,40.7784782020336
+1946.0,40.80064733828735
+1947.0,40.822847377921406
+1948.0,40.84507833840135
+1949.0,40.86734023653276
+1950.0,40.88963308845277
+1951.0,40.911956909621686
+1952.0,40.93431171481441
+1953.0,40.95669751811193
+1954.0,40.9791143328927
+1955.0,41.00156217182398
+1956.0,41.02404104685318
+1957.0,41.04655096919905
+1958.0,41.06909194934295
+1959.0,41.091663997020014
+1960.0,41.11426712121018
+1961.0,41.136901330129405
+1962.0,41.1595666312206
+1963.0,41.18226303114459
+1964.0,41.2049905357712
+1965.0,41.22774915017002
+1966.0,41.250538878601326
+1967.0,41.27335972450697
+1968.0,41.29621169050106
+1969.0,41.319094778360785
+1970.0,41.342008989017096
+1971.0,41.36495432254545
+1972.0,41.38793077815635
+1973.0,41.410938354186094
+1974.0,41.43397704808722
+1975.0,41.45704685641921
+1976.0,41.48014777483886
+1977.0,41.503279798090865
+1978.0,41.52644291999831
+1979.0,41.54963713345301
+1980.0,41.57286243040604
+1981.0,41.596118801858076
+1982.0,41.61940623784974
+1983.0,41.64272472745203
+1984.0,41.66607425875657
+1985.0,41.68945481886598
+1986.0,41.71286639388419
+1987.0,41.7363089689067
+1988.0,41.75978252801078
+1989.0,41.783287054245925
+1990.0,41.80682252962388
+1991.0,41.83038893510908
+1992.0,41.85398625060874
+1993.0,41.8776144549632
+1994.0,41.90127352593608
+1995.0,41.92496344020457
+1996.0,41.94868417334962
+1997.0,41.97243569984619
+1998.0,41.99621799305349
+1999.0,42.02003102520523
+2000.0,42.04387476739986
+2001.0,42.06774918959088
+2002.0,42.09165426057707
+2003.0,42.115589947992774
+2004.0,42.139556218298345
+2005.0,42.16355303677022
+2006.0,42.187580367491556
+2007.0,42.21163817334244
+2008.0,42.235726415990314
+2009.0,42.259845055880426
+2010.0,42.28399405222631
+2011.0,42.30817336300024
+2012.0,42.3323829449238
+2013.0,42.35662275345844
+2014.0,42.38089274279603
+2015.0,42.405192865849656
+2016.0,42.42952307424415
+2017.0,42.453883318306964
+2018.0,42.478273547058926
+2019.0,42.50269370820504
+2020.0,42.52714374812551
+2021.0,42.55162361186659
+2022.0,42.57613324313168
+2023.0,42.600672584272424
+2024.0,42.625241576279855
+2025.0,42.64984015877559
+2026.0,42.67446827000323
+2027.0,42.69912584681963
+2028.0,42.72381282468647
+2029.0,42.74852913766171
+2030.0,42.773274718391264
+2031.0,42.79804949810074
+2032.0,42.822853406587164
+2033.0,42.847686372210966
+2034.0,42.872548321887976
+2035.0,42.89743918108144
+2036.0,42.922358873794366
+2037.0,42.947307322561706
+2038.0,42.97228444844283
+2039.0,42.99729017101409
+2040.0,43.022324408361435
+2041.0,43.047387077073225
+2042.0,43.07247809223303
+2043.0,43.09759736741278
+2044.0,43.12274481466588
+2045.0,43.14792034452041
+2046.0,43.17312386597266
+2047.0,43.19835528648069
+2048.0,43.223614511957926
+2049.0,43.24890144676713
+2050.0,43.274215993714336
+2051.0,43.29955805404308
+2052.0,43.32492752742863
+2053.0,43.35032431197249
+2054.0,43.375748304197124
+2055.0,43.40119939904058
+2056.0,43.42667748985166
+2057.0,43.452182468385
+2058.0,43.4777142247963
+2059.0,43.503272647638
+2060.0,43.5288576238549
+2061.0,43.554469038779985
+2062.0,43.58010677613059
+2063.0,43.60577071800468
+2064.0,43.63146074487717
+2065.0,43.657176735596764
+2066.0,43.68291856738275
+2067.0,43.70868611582211
+2068.0,43.7344792548667
+2069.0,43.76029785683092
+2070.0,43.78614179238939
+2071.0,43.812010930574786
+2072.0,43.837905138776165
+2073.0,43.86382428273726
+2074.0,43.889768226555105
+2075.0,43.91573683267901
+2076.0,43.9417299619095
+2077.0,43.96774747339776
+2078.0,43.99378922464522
+2079.0,44.01985507150325
+2080.0,44.04594486817342
+2081.0,44.07205846720774
+2082.0,44.098195719509164
+2083.0,44.124356474332565
+2084.0,44.150540579285746
+2085.0,44.17674788033081
+2086.0,44.202978221785834
+2087.0,44.229231446326686
+2088.0,44.25550739498926
+2089.0,44.28180590717183
+2090.0,44.30812682063786
+2091.0,44.33446997151895
+2092.0,44.36083519431802
+2093.0,44.38722232191296
+2094.0,44.413631185560504
+2095.0,44.44006161490019
+2096.0,44.46651343795888
+2097.0,44.49298648115544
+2098.0,44.5194805693057
+2099.0,44.54599552562776
+2100.0,44.572531171747556
+2101.0,44.5990873277047
+2102.0,44.6256638119587
+2103.0,44.65226044139542
+2104.0,44.67887703133374
+2105.0,44.70551339553282
+2106.0,44.732169346199306
+2107.0,44.75884469399507
+2108.0,44.78553924804519
+2109.0,44.81225281594622
+2110.0,44.83898520377477
+2111.0,44.86573621609641
+2112.0,44.892505655974915
+2113.0,44.919293324981666
+2114.0,44.94609902320561
+2115.0,44.972922549263295
+2116.0,44.99976370030927
+2117.0,45.02662227204695
+2118.0,45.05349805873949
+2119.0,45.0803908532213
+2120.0,45.10730044690962
+2121.0,45.134226629816496
+2122.0,45.16116919056106
+2123.0,45.18812791638215
+2124.0,45.21510259315103
+2125.0,45.24209300538482
+2126.0,45.26909893625973
+2127.0,45.296120167625034
+2128.0,45.32315648001703
+2129.0,45.35020765267347
+2130.0,45.37727346354827
+2131.0,45.40435368932638
+2132.0,45.43144810543915
+2133.0,45.458556486079814
+2134.0,45.485678604219295
+2135.0,45.512814231622514
+2136.0,45.53996313886453
+2137.0,45.56712509534746
+2138.0,45.594299869317254
+2139.0,45.62148722788112
+2140.0,45.64868693702494
+2141.0,45.675898761630975
+2142.0,45.70312246549613
+2143.0,45.73035781135005
+2144.0,45.75760456087381
+2145.0,45.78486247471872
+2146.0,45.81213131252539
+2147.0,45.839410832943074
+2148.0,45.866700793649265
+2149.0,45.89400095136947
+2150.0,45.921311061897306
+2151.0,45.94863088011474
+2152.0,45.975960160012676
+2153.0,46.0032986547116
+2154.0,46.030646116482615
+2155.0,46.05800229676859
+2156.0,46.085366946205546
+2157.0,46.112739814644236
+2158.0,46.140120651171934
+2159.0,46.16750920413449
+2160.0,46.19490522115839
+2161.0,46.222308449173255
+2162.0,46.249718634434174
+2163.0,46.277135522544704
+2164.0,46.304558858479545
+2165.0,46.33198838660766
+2166.0,46.35942385071548
+2167.0,46.386864994030226
+2168.0,46.41431155924351
+2169.0,46.441763288534915
+2170.0,46.46921992359579
+2171.0,46.49668120565309
+2172.0,46.52414687549354
+2173.0,46.55161667348774
+2174.0,46.579090339614304
+2175.0,46.60656761348442
+2176.0,46.63404823436617
+2177.0,46.66153194120912
+2178.0,46.68901847266896
+2179.0,46.71650756713227
+2180.0,46.74399896274113
+2181.0,46.771492397418235
+2182.0,46.79898760889158
+2183.0,46.826484334719524
+2184.0,46.853982312315765
+2185.0,46.881481278974434
+2186.0,46.908980971891914
+2187.0,46.936481128206985
+2188.0,46.963981484998826
+2189.0,46.991481779334144
+2190.0,47.018981748285626
+2191.0,47.04648112895701
+2192.0,47.073979658517096
+2193.0,47.101477074195046
+2194.0,47.12897311334332
+2195.0,47.15646751344129
+2196.0,47.18396001212476
+2197.0,47.21145034721108
+2198.0,47.238938256724104
+2199.0,47.26642347891894
+2200.0,47.29390575230695
+2201.0,47.32138481568036
+2202.0,47.34886040813696
+2203.0,47.376332269104765
+2204.0,47.40380013836651
+2205.0,47.43126375608399
+2206.0,47.45872286282243
+2207.0,47.486177199574705
+2208.0,47.513626507785425
+2209.0,47.54107052937491
+2210.0,47.56850900676298
+2211.0,47.59594168289278
+2212.0,47.623368301254274
+2213.0,47.65078860590773
+2214.0,47.678202341506996
+2215.0,47.70560925332263
+2216.0,47.73300908726487
+2217.0,47.760401589906444
+2218.0,47.78778650850523
+2219.0,47.815163591026675
+2220.0,47.84253258616606
+2221.0,47.86989324337069
+2222.0,47.89724531286163
+2223.0,47.924588545655524
+2224.0,47.95192269358604
+2225.0,47.97924750932522
+2226.0,48.00656274640441
+2227.0,48.03386815923522
+2228.0,48.061163503130224
+2229.0,48.08844853432321
+2230.0,48.11572300998942
+2231.0,48.142986688265594
+2232.0,48.17023932826945
+2233.0,48.19748069011941
+2234.0,48.224710534953594
+2235.0,48.25192862494889
+2236.0,48.27913472333966
+2237.0,48.306328594436124
+2238.0,48.333510003642644
+2239.0,48.360678717475565
+2240.0,48.387834503580905
+2241.0,48.41497713075173
+2242.0,48.44210636894531
+2243.0,48.469221989299875
+2244.0,48.49632376415124
+2245.0,48.52341146704903
+2246.0,48.55048487277269
+2247.0,48.57754375734719
+2248.0,48.60458789805842
+2249.0,48.63161707346837
+2250.0,48.658631063429844
+2251.0,48.685629649101145
+2252.0,48.712612612960164
+2253.0,48.739579738818435
+2254.0,48.76653081183471
+2255.0,48.793465618528344
+2256.0,48.82038394679226
+2257.0,48.847285585905794
+2258.0,48.87417032654703
+2259.0,48.90103796080498
+2260.0,48.92788828219135
+2261.0,48.95472108565213
+2262.0,48.98153616757875
+2263.0,49.00833332581899
+2264.0,49.03511235968758
+2265.0,49.06187306997646
+2266.0,49.088615258964815
+2267.0,49.115338730428675
+2268.0,49.142043289650324
+2269.0,49.1687287434274
+2270.0,49.1953949000815
+2271.0,49.22204156946676
+2272.0,49.248668562977954
+2273.0,49.27527569355832
+2274.0,49.301862775707086
+2275.0,49.32842962548667
+2276.0,49.35497606052974
+2277.0,49.3815019000456
+2278.0,49.40800696482677
+2279.0,49.434491077254805
+2280.0,49.46095406130616
+2281.0,49.4873957425576
+2282.0,49.51381594819125
+2283.0,49.54021450699955
+2284.0,49.56659124938975
+2285.0,49.59294600738823
+2286.0,49.619278614644465
+2287.0,49.64558890643463
+2288.0,49.67187671966518
+2289.0,49.69814189287585
+2290.0,49.72438426624261
+2291.0,49.7506036815801
+2292.0,49.77679998234413
+2293.0,49.80297301363354
+2294.0,49.82912262219206
+2295.0,49.855248656409834
+2296.0,49.881350966324526
+2297.0,49.907429403622444
+2298.0,49.9334838216391
+2299.0,49.95951407535987
+2300.0,49.98552002142001
+2301.0,50.01150152099847
+2302.0,50.03745852521944
+2303.0,50.06339108830169
+2304.0,50.08929927000501
+2305.0,50.115183129851786
+2306.0,50.14104272712836
+2307.0,50.16687812088614
+2308.0,50.19268936994313
+2309.0,50.218476532884985
+2310.0,50.24423966806651
+2311.0,50.26997883361269
+2312.0,50.295694087420095
+2313.0,50.3213854871581
+2314.0,50.34705309027007
+2315.0,50.37269695397467
+2316.0,50.39831713526695
+2317.0,50.42391369091977
+2318.0,50.44948667748476
+2319.0,50.475036151293615
+2320.0,50.500562168459425
+2321.0,50.52606478487757
+2322.0,50.551544056227115
+2323.0,50.57700003797188
+2324.0,50.60243278536162
+2325.0,50.62784235343314
+2326.0,50.65322879701142
+2327.0,50.678592170710814
+2328.0,50.70393252893613
+2329.0,50.729249925883735
+2330.0,50.75454441554269
+2331.0,50.77981605169577
+2332.0,50.80506488792073
+2333.0,50.83029097759115
+2334.0,50.85549437387772
+2335.0,50.88067512974924
+2336.0,50.9058332979736
+2337.0,50.93096893111896
+2338.0,50.956082081554726
+2339.0,50.98117280145259
+2340.0,51.00624114278765
+2341.0,51.03128715733932
+2342.0,51.05631089669241
+2343.0,51.081312412238105
+2344.0,51.10629175517507
+2345.0,51.13124897651031
+2346.0,51.15618412706024
+2347.0,51.18109725745174
+2348.0,51.20598841812295
+2349.0,51.230857659324435
+2350.0,51.25570503112002
+2351.0,51.28053058338787
+2352.0,51.30533436582129
+2353.0,51.3301164279298
+2354.0,51.3548768190401
+2355.0,51.37961558829685
+2356.0,51.40433278466373
+2357.0,51.42902845692433
+2358.0,51.45370265368309
+2359.0,51.47835542336616
+2360.0,51.502986814222325
+2361.0,51.52759687432396
+2362.0,51.552185651567825
+2363.0,51.57675319367606
+2364.0,51.60129954819697
+2365.0,51.62582476250598
+2366.0,51.65032888380644
+2367.0,51.67481195913057
+2368.0,51.69927403534024
+2369.0,51.72371515912787
+2370.0,51.748135377017284
+2371.0,51.7725347353645
+2372.0,51.79691328035862
+2373.0,51.82127105802271
+2374.0,51.84560811421447
+2375.0,51.86992449462723
+2376.0,51.89422024479065
+2377.0,51.91849541007156
+2378.0,51.94275003567486
+2379.0,51.96698416664417
+2380.0,51.99119784786269
+2381.0,52.015391124054055
+2382.0,52.03956403978302
+2383.0,52.0637166394563
+2384.0,52.087848967323374
+2385.0,52.111961067477154
+2386.0,52.136052983854874
+2387.0,52.16012476023872
+2388.0,52.184176440256756
+2389.0,52.20820806738352
+2390.0,52.23221968494086
+2391.0,52.256211336098616
+2392.0,52.28018306387541
+2393.0,52.304134911139435
+2394.0,52.32806692060899
+2395.0,52.35197913485344
+2396.0,52.37587159629376
+2397.0,52.3997443472033
+2398.0,52.42359742970859
+2399.0,52.44743088578994
+2400.0,52.47124475728215
+2401.0,52.49503908587524
+2402.0,52.51881391311515
+2403.0,52.54256928040439
+2404.0,52.5663052290028
+2405.0,52.59002180002816
+2406.0,52.61371903445687
+2407.0,52.637396973124694
+2408.0,52.66105565672731
+2409.0,52.684695125821136
+2410.0,52.7083154208238
+2411.0,52.731916582015025
+2412.0,52.755498649537024
+2413.0,52.77906166339537
+2414.0,52.802605663459495
+2415.0,52.82613068946343
+2416.0,52.849636781006396
+2417.0,52.873123977553405
+2418.0,52.89659231843598
+2419.0,52.92004184285269
+2420.0,52.943472589869835
+2421.0,52.966884598422
+2422.0,52.9902779073128
+2423.0,53.01365255521528
+2424.0,53.037008580672754
+2425.0,53.06034602209922
+2426.0,53.083664917780105
+2427.0,53.10696530587272
+2428.0,53.130247224407015
+2429.0,53.153510711286025
+2430.0,53.17675580428654
+2431.0,53.199982541059676
+2432.0,53.22319095913139
+2433.0,53.24638109590313
+2434.0,53.26955298865243
+2435.0,53.29270667453338
+2436.0,53.315842190577285
+2437.0,53.33895957369314
+2438.0,53.36205886066829
+2439.0,53.3851400881689
+2440.0,53.408203292740545
+2441.0,53.43124851080878
+2442.0,53.45427577867963
+2443.0,53.47728513254018
+2444.0,53.50027660845913
+2445.0,53.523250242387256
+2446.0,53.546206070158
+2447.0,53.56914412748801
+2448.0,53.59206444997764
+2449.0,53.6149670731115
+2450.0,53.6378520322589
+2451.0,53.66071936267452
+2452.0,53.68356909949875
+2453.0,53.70640127775834
+2454.0,53.72921593236684
+2455.0,53.752013098125126
+2456.0,53.77479280972189
+2457.0,53.79755510173419
+2458.0,53.820300008627825
+2459.0,53.84302756475805
+2460.0,53.8657378043698
+2461.0,53.8884307615984
+2462.0,53.9111064704699
+2463.0,53.93376496490172
+2464.0,53.95640627870292
+2465.0,53.97903044557489
+2466.0,54.001637499111695
+2467.0,54.02422747280054
+2468.0,54.04680040002234
+2469.0,54.06935631405216
+2470.0,54.09189524805953
+2471.0,54.11441723510917
+2472.0,54.13692230816126
+2473.0,54.15941050007189
+2474.0,54.18188184359363
+2475.0,54.20433637137593
+2476.0,54.22677411596553
+2477.0,54.24919510980699
+2478.0,54.271599385243036
+2479.0,54.29398697451504
+2480.0,54.316357909763575
+2481.0,54.33871222302863
+2482.0,54.361049946250255
+2483.0,54.38337111126882
+2484.0,54.40567574982563
+2485.0,54.42796389356316
+2486.0,54.45023557402564
+2487.0,54.47249082265934
+2488.0,54.49472967081316
+2489.0,54.51695214973883
+2490.0,54.539158290591566
+2491.0,54.56134812443029
+2492.0,54.58352168221815
+2493.0,54.605678994822846
+2494.0,54.62782009301713
+2495.0,54.64994500747918
+2496.0,54.67205376879297
+2497.0,54.69414640744868
+2498.0,54.71622295384311
+2499.0,54.73828343828008
+2500.0,54.760327890970814
+2501.0,54.78235634203434
+2502.0,54.80436882149788
+2503.0,54.82636535929719
+2504.0,54.848345985277014
+2505.0,54.87031072919148
+2506.0,54.89225962070435
+2507.0,54.91419268938959
+2508.0,54.93610996473157
+2509.0,54.958011476125556
+2510.0,54.97989725287805
+2511.0,55.00176732420714
+2512.0,55.02362171924288
+2513.0,55.0454604670277
+2514.0,55.06728359651671
+2515.0,55.08909113657806
+2516.0,55.110883115993396
+2517.0,55.13265956345808
+2518.0,55.15442050758168
+2519.0,55.176165976888235
+2520.0,55.197895999816645
+2521.0,55.219610604721034
+2522.0,55.24130981987108
+2523.0,55.262993673452335
+2524.0,55.284662193566675
+2525.0,55.3063154082325
+2526.0,55.32795334538519
+2527.0,55.34957603287744
+2528.0,55.371183498479475
+2529.0,55.392775769879556
+2530.0,55.414352874684226
+2531.0,55.43591484041865
+2532.0,55.45746169452693
+2533.0,55.47899346437247
+2534.0,55.5005101772384
+2535.0,55.52201186032758
+2536.0,55.54349854076333
+2537.0,55.56497024558952
+2538.0,55.586427001770886
+2539.0,55.60786883619347
+2540.0,55.62929577566482
+2541.0,55.65070784691441
+2542.0,55.672105076593844
+2543.0,55.69348749127725
+2544.0,55.714855117461624
+2545.0,55.73620798156703
+2546.0,55.75754610993696
+2547.0,55.77886952883871
+2548.0,55.80017826446356
+2549.0,55.8214723429272
+2550.0,55.84275179026991
+2551.0,55.86401663245698
+2552.0,55.88526689537895
+2553.0,55.90650260485187
+2554.0,55.927723786617705
+2555.0,55.948930466344535
+2556.0,55.970122669626846
+2557.0,55.991300421985905
+2558.0,56.012463748869976
+2559.0,56.03361267565466
+2560.0,56.05474722764311
+2561.0,56.0758674300664
+2562.0,56.09697330808377
+2563.0,56.118064886782875
+2564.0,56.13914219118017
+2565.0,56.16020524622107
+2566.0,56.181254076780306
+2567.0,56.20228870766216
+2568.0,56.223309163600796
+2569.0,56.24431546926048
+2570.0,56.26530764923585
+2571.0,56.28628572805226
+2572.0,56.30724973016598
+2573.0,56.32819967996447
+2574.0,56.349135601766676
+2575.0,56.37005751982333
+2576.0,56.39096545831709
+2577.0,56.41185944136295
+2578.0,56.43273949300843
+2579.0,56.45360563723379
+2580.0,56.474457897952426
+2581.0,56.49529629901102
+2582.0,56.5161208641898
+2583.0,56.536931617202825
+2584.0,56.55772858169828
+2585.0,56.578511781258655
+2586.0,56.59928123940103
+2587.0,56.620036979577364
+2588.0,56.640779025174645
+2589.0,56.66150739951522
+2590.0,56.68222212585707
+2591.0,56.702923227393946
+2592.0,56.72361072725575
+2593.0,56.744284648508625
+2594.0,56.764945014155344
+2595.0,56.78559184713545
+2596.0,56.80622517032558
+2597.0,56.826845006539585
+2598.0,56.8474513785289
+2599.0,56.86804430898272
+2600.0,56.88862382052821
+2601.0,56.90918993573079
+2602.0,56.929742677094325
+2603.0,56.95028206706141
+2604.0,56.97080812801354
+2605.0,56.9913208822714
+2606.0,57.01182035209504
+2607.0,57.03230655968413
+2608.0,57.0527795271782
+2609.0,57.07323927665681
+2610.0,57.09368583013987
+2611.0,57.114119209587784
+2612.0,57.13453943690167
+2613.0,57.15494653392365
+2614.0,57.17534052243701
+2615.0,57.195721424166436
+2616.0,57.21608926077823
+2617.0,57.23644405388056
+2618.0,57.2567858250236
+2619.0,57.27711459569983
+2620.0,57.297430387344214
+2621.0,57.317733221334365
+2622.0,57.33802311899088
+2623.0,57.35830010157741
+2624.0,57.378564190300956
+2625.0,57.39881540631209
+2626.0,57.41905377070509
+2627.0,57.439279304518195
+2628.0,57.45949202873383
+2629.0,57.47969196427875
+2630.0,57.49987913202429
+2631.0,57.52005355278659
+2632.0,57.54021524732672
+2633.0,57.56036423635097
+2634.0,57.580500540510975
+2635.0,57.60062418040396
+2636.0,57.62073517657293
+2637.0,57.64083354950683
+2638.0,57.66091931964085
+2639.0,57.68099250735646
+2640.0,57.70105313298175
+2641.0,57.72110121679154
+2642.0,57.7411367790076
+2643.0,57.76115983979888
+2644.0,57.78117041928155
+2645.0,57.80116853751946
+2646.0,57.82115421452402
+2647.0,57.841127470254676
+2648.0,57.86108832461886
+2649.0,57.88103679747233
+2650.0,57.90097290861929
+2651.0,57.92089667781258
+2652.0,57.9408081247539
+2653.0,57.96070726909395
+2654.0,57.98059413043261
+2655.0,58.00046872831918
+2656.0,58.02033108225246
+2657.0,58.04018121168106
+2658.0,58.06001913600348
+2659.0,58.0798448745683
+2660.0,58.0996584466744
+2661.0,58.11945987157109
+2662.0,58.13924916845833
+2663.0,58.15902635648689
+2664.0,58.17879145475851
+2665.0,58.19854448232607
+2666.0,58.218285458193805
+2667.0,58.238014401317415
+2668.0,58.257731330604265
+2669.0,58.277436264913625
+2670.0,58.29712922305673
+2671.0,58.316810223796956
+2672.0,58.336479285850096
+2673.0,58.35613642788441
+2674.0,58.375781668520894
+2675.0,58.395415026333325
+2676.0,58.41503651984854
+2677.0,58.434646167546525
+2678.0,58.45424398786062
+2679.0,58.473829999177696
+2680.0,58.49340421983826
+2681.0,58.51296666813663
+2682.0,58.53251736232118
+2683.0,58.55205632059437
+2684.0,58.571583561112995
+2685.0,58.59109910198833
+2686.0,58.61060296128623
+2687.0,58.63009515702741
+2688.0,58.64957570718747
+2689.0,58.66904462969711
+2690.0,58.688501942442315
+2691.0,58.70794766326442
+2692.0,58.7273818099604
+2693.0,58.746804400282876
+2694.0,58.76621545194035
+2695.0,58.785614982597366
+2696.0,58.80500300987462
+2697.0,58.82437955134915
+2698.0,58.8437446245544
+2699.0,58.86309824698053
+2700.0,58.88244043607439
+2701.0,58.901771209239776
+2702.0,58.92109058383756
+2703.0,58.9403985771858
+2704.0,58.95969520655992
+2705.0,58.978980489192836
+2706.0,58.998254442275154
+2707.0,59.0175170829552
+2708.0,59.03676842833929
+2709.0,59.05600849549179
+2710.0,59.07523730143528
+2711.0,59.09445486315075
+2712.0,59.11366119757763
+2713.0,59.132856321614
+2714.0,59.15204025211676
+2715.0,59.17121300590171
+2716.0,59.190374599743734
+2717.0,59.20952505037684
+2718.0,59.228664374494464
+2719.0,59.247792588749455
+2720.0,59.26690970975428
+2721.0,59.28601575408116
+2722.0,59.3051107382622
+2723.0,59.32419467878951
+2724.0,59.34326759211535
+2725.0,59.36232949465225
+2726.0,59.38138040277315
+2727.0,59.40042033281158
+2728.0,59.41944930106171
+2729.0,59.4384673237785
+2730.0,59.4574744171779
+2731.0,59.476470597436894
+2732.0,59.495455880693626
+2733.0,59.514430283047695
+2734.0,59.533393820560036
+2735.0,59.55234650925319
+2736.0,59.57128836511146
+2737.0,59.5902194040809
+2738.0,59.60913964206963
+2739.0,59.628049094947784
+2740.0,59.64694777854773
+2741.0,59.66583570866419
+2742.0,59.68471290105432
+2743.0,59.70357937143787
+2744.0,59.722435135497335
+2745.0,59.74128020887802
+2746.0,59.76011460718815
+2747.0,59.778938345999066
+2748.0,59.79775144084529
+2749.0,59.81655390722467
+2750.0,59.83534576059848
+2751.0,59.85412701639154
+2752.0,59.87289768999238
+2753.0,59.891657796753286
+2754.0,59.91040735199047
+2755.0,59.929146370984185
+2756.0,59.94787486897881
+2757.0,59.96659286118299
+2758.0,59.985300362769756
+2759.0,60.00399738888873
+2760.0,60.022683956814724
+2761.0,60.041360088406414
+2762.0,60.06002580604787
+2763.0,60.078681132047045
+2764.0,60.09732608863635
+2765.0,60.11596069797275
+2766.0,60.13458498213831
+2767.0,60.15319896314051
+2768.0,60.171802662912484
+2769.0,60.190396103313574
+2770.0,60.20897930612951
+2771.0,60.22755229307284
+2772.0,60.246115085783266
+2773.0,60.26466770582792
+2774.0,60.28321017470182
+2775.0,60.30174251382812
+2776.0,60.32026474455847
+2777.0,60.33877688817336
+2778.0,60.357278965882486
+2779.0,60.37577099882497
+2780.0,60.3942530080698
+2781.0,60.41272501461618
+2782.0,60.431187039393684
+2783.0,60.44963910326273
+2784.0,60.46808122701492
+2785.0,60.4865134313732
+2786.0,60.50493573699235
+2787.0,60.523348164459186
+2788.0,60.541750734292926
+2789.0,60.56014346694551
+2790.0,60.57852638280186
+2791.0,60.596899502180214
+2792.0,60.61526284533243
+2793.0,60.63361643244433
+2794.0,60.651960283635965
+2795.0,60.670294418961866
+2796.0,60.688618858411424
+2797.0,60.70693362190917
+2798.0,60.72523872931508
+2799.0,60.74353420042477
+2800.0,60.76182005496995
+2801.0,60.78009631261857
+2802.0,60.79836299297521
+2803.0,60.81662011558128
+2804.0,60.8348676999154
+2805.0,60.85310576539359
+2806.0,60.87133433136965
+2807.0,60.88955341713535
+2808.0,60.90776304192072
+2809.0,60.92596322489441
+2810.0,60.94415398516387
+2811.0,60.962335341775706
+2812.0,60.98050731371584
+2813.0,60.998669919909915
+2814.0,61.01682317922345
+2815.0,61.03496711046217
+2816.0,61.05310173237229
+2817.0,61.07122706364071
+2818.0,61.08934312289536
+2819.0,61.10744992870536
+2820.0,61.125547499581366
+2821.0,61.14363585397584
+2822.0,61.16171501028322
+2823.0,61.17978498684022
+2824.0,61.19784580192611
+2825.0,61.215897473762965
+2826.0,61.23394002051587
+2827.0,61.251973460293215
+2828.0,61.26999781114692
+2829.0,61.2880130910727
+2830.0,61.30601931801027
+2831.0,61.324016509843666
+2832.0,61.34200468440141
+2833.0,61.35998385945681
+2834.0,61.37795405272817
+2835.0,61.395915281879
+2836.0,61.413867564518334
+2837.0,61.43181091820088
+2838.0,61.449745360427336
+2839.0,61.46767090864454
+2840.0,61.48558758024581
+2841.0,61.503495392571025
+2842.0,61.52139436290703
+2843.0,61.53928450848772
+2844.0,61.5571658464943
+2845.0,61.57503839405559
+2846.0,61.59290216824818
+2847.0,61.61075718609666
+2848.0,61.62860346457386
+2849.0,61.646441020601046
+2850.0,61.66426987104815
+2851.0,61.682090032734024
+2852.0,61.69990152242661
+2853.0,61.71770435684316
+2854.0,61.73549855265048
+2855.0,61.75328412646516
+2856.0,61.77106109485371
+2857.0,61.788829474332815
+2858.0,61.80658928136958
+2859.0,61.824340532381726
+2860.0,61.84208324373774
+2861.0,61.859817431757115
+2862.0,61.87754311271059
+2863.0,61.89526030282034
+2864.0,61.912969018260156
+2865.0,61.930669275155616
+2866.0,61.948361089584424
+2867.0,61.966044477576425
+2868.0,61.98371945511397
+2869.0,62.00138603813199
+2870.0,62.019044242518255
+2871.0,62.03669408411361
+2872.0,62.05433557871204
+2873.0,62.07196874206103
+2874.0,62.08959358986159
+2875.0,62.1072101377686
+2876.0,62.12481840139088
+2877.0,62.142418396291475
+2878.0,62.160010137987776
+2879.0,62.177593641951724
+2880.0,62.19516892361003
+2881.0,62.212735998344336
+2882.0,62.230294881491375
+2883.0,62.24784558834321
+2884.0,62.26538813414739
+2885.0,62.2829225341071
+2886.0,62.30044880338139
+2887.0,62.31796695708537
+2888.0,62.33547701029031
+2889.0,62.3529789780239
+2890.0,62.37047287527035
+2891.0,62.38795871697072
+2892.0,62.405436518022825
+2893.0,62.422906293281706
+2894.0,62.440368057559596
+2895.0,62.45782182562623
+2896.0,62.47526761220886
+2897.0,62.49270543199261
+2898.0,62.51013529962052
+2899.0,62.52755722969374
+2900.0,62.54497123677172
+2901.0,62.56237733537237
+2902.0,62.57977553997221
+2903.0,62.59716586500661
+2904.0,62.61454832486979
+2905.0,62.6319229339152
+2906.0,62.64928970645549
+2907.0,62.66664865676279
+2908.0,62.68399979906886
+2909.0,62.701343147565154
+2910.0,62.71867871640316
+2911.0,62.73600651969436
+2912.0,62.753326571510506
+2913.0,62.77063888588382
+2914.0,62.787943476807
+2915.0,62.80524035823348
+2916.0,62.8225295440776
+2917.0,62.8398110482147
+2918.0,62.8570848844813
+2919.0,62.87435106667527
+2920.0,62.891609608555925
+2921.0,62.90886052384426
+2922.0,62.92610382622308
+2923.0,62.94333952933702
+2924.0,62.96056764679291
+2925.0,62.97778819215976
+2926.0,62.99500117896899
+2927.0,63.01220662071453
+2928.0,63.02940453085299
+2929.0,63.046594922803806
+2930.0,63.06377780994935
+2931.0,63.08095320563517
+2932.0,63.098121123169996
+2933.0,63.115281575825975
+2934.0,63.13243457683882
+2935.0,63.14958013940788
+2936.0,63.16671827669635
+2937.0,63.183849001831355
+2938.0,63.20097232790416
+2939.0,63.21808826797024
+2940.0,63.23519683504943
+2941.0,63.25229804212609
+2942.0,63.26939190214923
+2943.0,63.28647842803266
+2944.0,63.30355763265504
+2945.0,63.32062952886016
+2946.0,63.33769412945695
+2947.0,63.35475144721966
+2948.0,63.371801494888004
+2949.0,63.388844285167295
+2950.0,63.405879830728495
+2951.0,63.42290814420848
+2952.0,63.43992923821006
+2953.0,63.456943125302146
+2954.0,63.473949818019925
+2955.0,63.49094932886488
+2956.0,63.50794167030502
+2957.0,63.52492685477496
+2958.0,63.541904894676016
+2959.0,63.55887580237643
+2960.0,63.575839590211395
+2961.0,63.59279627048325
+2962.0,63.60974585546151
+2963.0,63.62668835738311
+2964.0,63.64362378845244
+2965.0,63.660552160841476
+2966.0,63.677473486689955
+2967.0,63.694387778105465
+2968.0,63.71129504716351
+2969.0,63.72819530590774
+2970.0,63.74508856634994
+2971.0,63.761974840470295
+2972.0,63.77885414021738
+2973.0,63.79572647750834
+2974.0,63.812591864229
+2975.0,63.82945031223397
+2976.0,63.846301833346764
+2977.0,63.8631464393599
+2978.0,63.8799841420351
+2979.0,63.89681495310328
+2980.0,63.91363888426469
+2981.0,63.93045594718916
+2982.0,63.947266153515976
+2983.0,63.96406951485423
+2984.0,63.98086604278277
+2985.0,63.997655748850406
+2986.0,64.01443864457595
+2987.0,64.03121474144837
+2988.0,64.04798405092686
+2989.0,64.06474658444101
+2990.0,64.08150235339086
+2991.0,64.09825136914705
+2992.0,64.11499364305085
+2993.0,64.13172918641436
+2994.0,64.14845801052063
+2995.0,64.16518012662357
+2996.0,64.18189554594832
+2997.0,64.19860427969117
+2998.0,64.21530633901976
+2999.0,64.23200173507317
+3000.0,64.2486904789619
+3001.0,64.2653725817682
+3002.0,64.28204805454598
+3003.0,64.298716908321
+3004.0,64.31537915409093
+3005.0,64.33203480282549
+3006.0,64.34868386546655
+3007.0,64.36532635292818
+3008.0,64.3819622760968
+3009.0,64.39859164583125
+3010.0,64.41521447296292
+3011.0,64.4318307682958
+3012.0,64.4484405426066
+3013.0,64.46504380664493
+3014.0,64.48164057113325
+3015.0,64.49823084676702
+3016.0,64.51481464421484
+3017.0,64.53139197411855
+3018.0,64.54796284709325
+3019.0,64.56452727372744
+3020.0,64.58108526458312
+3021.0,64.59763683019592
+3022.0,64.61418198107503
+3023.0,64.63072072770355
+3024.0,64.64725308053833
+3025.0,64.66377905001028
+3026.0,64.68029864652425
+3027.0,64.69681188045932
+3028.0,64.71331876216875
+3029.0,64.72981930198014
+3030.0,64.74631351019549
+3031.0,64.7628013970913
+3032.0,64.77928297291865
+3033.0,64.79575824790335
+3034.0,64.81222723224589
+3035.0,64.82868993612169
+3036.0,64.84514636968106
+3037.0,64.8615965430494
+3038.0,64.87804046632715
+3039.0,64.89447814959003
+3040.0,64.91090960288896
+3041.0,64.92733483625034
+3042.0,64.94375385967595
+3043.0,64.9601666831431
+3044.0,64.97657331660484
+3045.0,64.99297376998983
+3046.0,65.00936805320252
+3047.0,65.02575617612334
+3048.0,65.0421381486086
+3049.0,65.05851398049066
+3050.0,65.07488368157807
+3051.0,65.09124726165551
+3052.0,65.10760473048403
+3053.0,65.12395609780097
+3054.0,65.14030137332018
+3055.0,65.1566405667321
+3056.0,65.17297368770363
+3057.0,65.1893007458785
+3058.0,65.20562175087719
+3059.0,65.22193671229697
+3060.0,65.2382456397121
+3061.0,65.25454854267386
+3062.0,65.27084543071058
+3063.0,65.28713631332778
+3064.0,65.30342120000822
+3065.0,65.31970010021197
+3066.0,65.33597302337652
+3067.0,65.35223997891681
+3068.0,65.36850097622539
+3069.0,65.38475602467233
+3070.0,65.4010051336055
+3071.0,65.4172483123505
+3072.0,65.4334855702108
+3073.0,65.44971691646778
+3074.0,65.46594236038084
+3075.0,65.48216191118745
+3076.0,65.4983755781032
+3077.0,65.51458337032193
+3078.0,65.5307852970158
+3079.0,65.54698136733526
+3080.0,65.56317159040928
+3081.0,65.5793559753453
+3082.0,65.59553453122932
+3083.0,65.61170726712605
+3084.0,65.62787419207889
+3085.0,65.64403531511005
+3086.0,65.66019064522058
+3087.0,65.67634019139051
+3088.0,65.69248396257882
+3089.0,65.70862196772363
+3090.0,65.72475421574217
+3091.0,65.74088071553086
+3092.0,65.75700147596545
+3093.0,65.773116505901
+3094.0,65.78922581417204
+3095.0,65.80532940959256
+3096.0,65.82142730095606
+3097.0,65.83751949703571
+3098.0,65.85360600658441
+3099.0,65.86968683833473
+3100.0,65.88576200099908
+3101.0,65.90183150326982
+3102.0,65.91789535381916
+3103.0,65.93395356129945
+3104.0,65.95000613434303
+3105.0,65.96605308156244
+3106.0,65.98209441155039
+3107.0,65.99813013287991
+3108.0,66.01416025410438
+3109.0,66.03018478375752
+3110.0,66.04620373035361
+3111.0,66.06221710238738
+3112.0,66.0782249083342
+3113.0,66.09422715665015
+3114.0,66.1102238557719
+3115.0,66.12621501411705
+3116.0,66.14220064008393
+3117.0,66.15818074205185
+3118.0,66.17415532838108
+3119.0,66.19012440741288
+3120.0,66.20608798746966
+3121.0,66.22204607685494
+3122.0,66.23799868385348
+3123.0,66.2539458167313
+3124.0,66.26988748373576
+3125.0,66.28582369309564
+3126.0,66.30175445302109
+3127.0,66.31767977170387
+3128.0,66.33359965731728
+3129.0,66.34951411801623
+3130.0,66.36542316193733
+3131.0,66.38132679719895
+3132.0,66.39722503190124
+3133.0,66.41311787412627
+3134.0,66.42900533193797
+3135.0,66.44488741338229
+3136.0,66.46076412648718
+3137.0,66.4766354792627
+3138.0,66.49250147970109
+3139.0,66.50836213577675
+3140.0,66.52421745544639
+3141.0,66.54006744664898
+3142.0,66.5559121173059
+3143.0,66.571751475321
+3144.0,66.58758552858052
+3145.0,66.60341428495332
+3146.0,66.61923775229084
+3147.0,66.63505593842713
+3148.0,66.65086885117903
+3149.0,66.66667649834602
+3150.0,66.6824788877105
+3151.0,66.69827602703769
+3152.0,66.71406792407574
+3153.0,66.72985458655575
+3154.0,66.74563602219189
+3155.0,66.76141223868137
+3156.0,66.77718324370458
+3157.0,66.79294904492505
+3158.0,66.80870964998957
+3159.0,66.82446506652819
+3160.0,66.84021530215436
+3161.0,66.85596036446489
+3162.0,66.87170026104002
+3163.0,66.88743499944351
+3164.0,66.90316458722268
+3165.0,66.91888903190838
+3166.0,66.9346083410152
+3167.0,66.9503225220414
+3168.0,66.96603158246894
+3169.0,66.98173552976365
+3170.0,66.99743437137516
+3171.0,67.01312811473704
+3172.0,67.02881676726676
+3173.0,67.04450033636583
+3174.0,67.06017882941981
+3175.0,67.07585225379833
+3176.0,67.09152061685516
+3177.0,67.10718392592831
+3178.0,67.12284218834
+3179.0,67.13849541139675
+3180.0,67.15414360238937
+3181.0,67.16978676859311
+3182.0,67.1854249172677
+3183.0,67.20105805565724
+3184.0,67.21668619099043
+3185.0,67.23230933048052
+3186.0,67.2479274813254
+3187.0,67.26354065070763
+3188.0,67.27914884579445
+3189.0,67.29475207373791
+3190.0,67.31035034167483
+3191.0,67.32594365672692
+3192.0,67.34153202600075
+3193.0,67.35711545658789
+3194.0,67.37269395556483
+3195.0,67.38826752999313
+3196.0,67.40383618691948
+3197.0,67.41939993337562
+3198.0,67.43495877637848
+3199.0,67.45051272293024
+3200.0,67.46606178001831
+3201.0,67.4816059546154
+3202.0,67.4971452536796
+3203.0,67.51267968415435
+3204.0,67.5282092529686
+3205.0,67.54373396703666
+3206.0,67.55925383325848
+3207.0,67.57476885851948
+3208.0,67.59027904969078
+3209.0,67.6057844136291
+3210.0,67.62128495717683
+3211.0,67.63678068716217
+3212.0,67.65227161039903
+3213.0,67.66775773368718
+3214.0,67.68323906381225
+3215.0,67.69871560754578
+3216.0,67.71418737164524
+3217.0,67.72965436285409
+3218.0,67.74511658790183
+3219.0,67.76057405350403
+3220.0,67.7760267663624
+3221.0,67.79147473316475
+3222.0,67.80691796058512
+3223.0,67.82235645528381
+3224.0,67.8377902239073
+3225.0,67.85321927308851
+3226.0,67.86864360944664
+3227.0,67.88406323958728
+3228.0,67.89947817010253
+3229.0,67.9148884075709
+3230.0,67.9302939585574
+3231.0,67.94569482961369
+3232.0,67.96109102727793
+3233.0,67.97648255807496
+3234.0,67.99186942851625
+3235.0,68.00725164510008
+3236.0,68.02262921431138
+3237.0,68.03800214262189
+3238.0,68.05337043649021
+3239.0,68.06873410236179
+3240.0,68.08409314666898
+3241.0,68.09944757583106
+3242.0,68.11479739625433
+3243.0,68.13014261433209
+3244.0,68.14548323644465
+3245.0,68.16081926895947
+3246.0,68.17615071823114
+3247.0,68.1914775906014
+3248.0,68.20679989239916
+3249.0,68.22211762994066
+3250.0,68.23743080952929
+3251.0,68.25273943745589
+3252.0,68.26804351999856
+3253.0,68.28334306342282
+3254.0,68.29863807398164
+3255.0,68.31392855791539
+3256.0,68.32921452145197
+3257.0,68.34449597080683
+3258.0,68.35977291218295
+3259.0,68.37504535177094
+3260.0,68.39031329574904
+3261.0,68.40557675028313
+3262.0,68.42083572152688
+3263.0,68.43609021562163
+3264.0,68.45134023869655
+3265.0,68.46658579686856
+3266.0,68.4818268962425
+3267.0,68.49706354291104
+3268.0,68.5122957429548
+3269.0,68.52752350244235
+3270.0,68.54274682743021
+3271.0,68.55796572396297
+3272.0,68.57318019807323
+3273.0,68.58839025578172
+3274.0,68.60359590309729
+3275.0,68.61879714601685
+3276.0,68.63399399052564
+3277.0,68.64918644259704
+3278.0,68.6643745081927
+3279.0,68.67955819326252
+3280.0,68.69473750374485
+3281.0,68.70991244556622
+3282.0,68.72508302464166
+3283.0,68.74024924687461
+3284.0,68.75541111815691
+3285.0,68.77056864436894
+3286.0,68.78572183137955
+3287.0,68.80087068504619
+3288.0,68.81601521121482
+3289.0,68.8311554157201
+3290.0,68.84629130438526
+3291.0,68.86142288302224
+3292.0,68.8765501574317
+3293.0,68.891673133403
+3294.0,68.9067918167143
+3295.0,68.92190621313256
+3296.0,68.93701632841356
+3297.0,68.95212216830193
+3298.0,68.96722373853127
+3299.0,68.98232104482398
+3300.0,68.99741409289156
+3301.0,69.01250288843437
+3302.0,69.02758743714188
+3303.0,69.04266774469251
+3304.0,69.05774381675388
+3305.0,69.07281565898263
+3306.0,69.08788327702457
+3307.0,69.10294667651466
+3308.0,69.11800586307707
+3309.0,69.1330608423252
+3310.0,69.14811161986172
+3311.0,69.16315820127853
+3312.0,69.17820059215694
+3313.0,69.19323879806753
+3314.0,69.20827282457024
+3315.0,69.2233026772145
+3316.0,69.23832836153913
+3317.0,69.25334988307236
+3318.0,69.268367247332
+3319.0,69.2833804598253
+3320.0,69.29838952604914
+3321.0,69.31339445148988
+3322.0,69.32839524162355
+3323.0,69.34339190191584
+3324.0,69.35838443782201
+3325.0,69.37337285478708
+3326.0,69.38835715824575
+3327.0,69.40333735362252
+3328.0,69.41831344633157
+3329.0,69.43328544177697
+3330.0,69.44825334535261
+3331.0,69.46321716244216
+3332.0,69.47817689841926
+3333.0,69.4931325586474
+3334.0,69.50808414848007
+3335.0,69.52303167326065
+3336.0,69.53797513832258
+3337.0,69.55291454898928
+3338.0,69.56784991057422
+3339.0,69.58278122838095
+3340.0,69.59770850770315
+3341.0,69.61263175382456
+3342.0,69.62755097201912
+3343.0,69.64246616755094
+3344.0,69.65737734567435
+3345.0,69.67228451163386
+3346.0,69.68718767066431
+3347.0,69.70208682799078
+3348.0,69.71698198882868
+3349.0,69.7318731583837
+3350.0,69.74676034185195
+3351.0,69.76164354441994
+3352.0,69.77652277126457
+3353.0,69.79139802755313
+3354.0,69.80626931844346
+3355.0,69.82113664908383
+3356.0,69.83600002461303
+3357.0,69.85085945016043
+3358.0,69.86571493084593
+3359.0,69.88056647178001
+3360.0,69.89541407806382
+3361.0,69.91025775478907
+3362.0,69.9250975070382
+3363.0,69.93993333988433
+3364.0,69.95476525839123
+3365.0,69.96959326761352
+3366.0,69.98441737259647
+3367.0,69.99923757837621
+3368.0,70.01405388997966
+3369.0,70.02886631242458
+3370.0,70.04367485071958
+3371.0,70.05847950986414
+3372.0,70.0732802948487
+3373.0,70.08807721065456
+3374.0,70.10287026225406
+3375.0,70.11765945461043
+3376.0,70.13244479267796
+3377.0,70.14722628140194
+3378.0,70.16200392571875
+3379.0,70.1767777305558
+3380.0,70.19154770083159
+3381.0,70.20631384145578
+3382.0,70.22107615732915
+3383.0,70.23583465334364
+3384.0,70.25058933438238
+3385.0,70.26534020531976
+3386.0,70.28008727102133
+3387.0,70.29483053634394
+3388.0,70.30957000613569
+3389.0,70.32430568523604
+3390.0,70.3390375784757
+3391.0,70.35376569067681
+3392.0,70.36849002665281
+3393.0,70.38321059120857
+3394.0,70.39792738914034
+3395.0,70.41264042523588
+3396.0,70.42734970427432
+3397.0,70.44205523102633
+3398.0,70.45675701025407
+3399.0,70.4714550467112
+3400.0,70.48614934514295
+3401.0,70.50083991028615
+3402.0,70.51552674686914
+3403.0,70.53020985961194
+3404.0,70.54488925322619
+3405.0,70.55956493241516
+3406.0,70.5742369018738
+3407.0,70.5889051662888
+3408.0,70.60356973033852
+3409.0,70.6182305986931
+3410.0,70.63288777601439
+3411.0,70.64754126695607
+3412.0,70.6621910761636
+3413.0,70.67683720827428
+3414.0,70.69147966791724
+3415.0,70.70611845971347
+3416.0,70.72075358827585
+3417.0,70.7353850582092
+3418.0,70.75001287411024
+3419.0,70.76463704056762
+3420.0,70.77925756216197
+3421.0,70.79387444346595
+3422.0,70.80848768904417
+3423.0,70.82309730345331
+3424.0,70.8377032912421
+3425.0,70.85230565695129
+3426.0,70.86690440511377
+3427.0,70.88149954025455
+3428.0,70.89609106689072
+3429.0,70.91067898953156
+3430.0,70.92526331267847
+3431.0,70.93984404082512
+3432.0,70.95442117845732
+3433.0,70.96899473005315
+3434.0,70.9835647000829
+3435.0,70.99813109300914
+3436.0,71.01269391328675
+3437.0,71.02725316536291
+3438.0,71.04180885367708
+3439.0,71.05636098266115
+3440.0,71.07090955673927
+3441.0,71.08545458032806
+3442.0,71.09999605783653
+3443.0,71.11453399366606
+3444.0,71.12906839221051
+3445.0,71.14359925785618
+3446.0,71.1581265949819
+3447.0,71.1726504079589
+3448.0,71.187170701151
+3449.0,71.20168747891456
+3450.0,71.21620074559843
+3451.0,71.23071050554407
+3452.0,71.24521676308558
+3453.0,71.25971952254955
+3454.0,71.27421878825527
+3455.0,71.28871456451469
+3456.0,71.3032068556324
+3457.0,71.31769566590565
+3458.0,71.33218099962441
+3459.0,71.34666286107137
+3460.0,71.36114125452195
+3461.0,71.37561618424432
+3462.0,71.39008765449942
+3463.0,71.404555669541
+3464.0,71.41902023361557
+3465.0,71.43348135096251
+3466.0,71.44793902581401
+3467.0,71.46239326239517
+3468.0,71.4768440649239
+3469.0,71.49129143761103
+3470.0,71.50573538466031
+3471.0,71.52017591026843
+3472.0,71.53461301862502
+3473.0,71.54904671391265
+3474.0,71.56347700030688
+3475.0,71.57790388197628
+3476.0,71.59232736308245
+3477.0,71.60674744778
+3478.0,71.62116414021656
+3479.0,71.63557744453288
+3480.0,71.6499873648628
+3481.0,71.6643939053332
+3482.0,71.67879707006412
+3483.0,71.69319686316872
+3484.0,71.70759328875333
+3485.0,71.72198635091742
+3486.0,71.73637605375365
+3487.0,71.75076240134788
+3488.0,71.76514539777921
+3489.0,71.77952504711996
+3490.0,71.79390135343566
+3491.0,71.80827432078517
+3492.0,71.8226439532206
+3493.0,71.83701025478736
+3494.0,71.85137322952416
+3495.0,71.86573288146306
+3496.0,71.88008921462949
+3497.0,71.8944422330422
+3498.0,71.90879194071333
+3499.0,71.92313834164841
+3500.0,71.93748143984641
+3501.0,71.9518212392997
+3502.0,71.9661577439941
+3503.0,71.98049095790891
+3504.0,71.99482088501684
+3505.0,72.00914752928416
+3506.0,72.02347089467061
+3507.0,72.03779098512943
+3508.0,72.05210780460747
+3509.0,72.06642135704504
+3510.0,72.08073164637612
+3511.0,72.09503867652816
+3512.0,72.10934245142226
+3513.0,72.1236429749732
+3514.0,72.13794025108923
+3515.0,72.15223428367241
+3516.0,72.16652507661837
+3517.0,72.18081263381639
+3518.0,72.19509695914952
+3519.0,72.20937805649446
+3520.0,72.22365592972164
+3521.0,72.23793058269521
+3522.0,72.25220201927311
+3523.0,72.26647024330698
+3524.0,72.28073525864232
+3525.0,72.29499706911834
+3526.0,72.3092556785681
+3527.0,72.32351109081849
+3528.0,72.33776330969019
+3529.0,72.35201233899781
+3530.0,72.36625818254976
+3531.0,72.38050084414833
+3532.0,72.39474032758976
+3533.0,72.40897663666414
+3534.0,72.42320977515551
+3535.0,72.43743974684186
+3536.0,72.4516665554951
+3537.0,72.46589020488115
+3538.0,72.48011069875986
+3539.0,72.49432804088511
+3540.0,72.50854223500477
+3541.0,72.52275328486074
+3542.0,72.53696119418898
+3543.0,72.55116596671945
+3544.0,72.56536760617621
+3545.0,72.5795661162774
+3546.0,72.59376150073524
+3547.0,72.60795376325606
+3548.0,72.6221429075403
+3549.0,72.63632893728253
+3550.0,72.65051185617152
+3551.0,72.66469166789014
+3552.0,72.67886837611545
+3553.0,72.69304198451869
+3554.0,72.70721249676537
+3555.0,72.7213799165151
+3556.0,72.73554424742181
+3557.0,72.74970549313363
+3558.0,72.76386365729297
+3559.0,72.77801874353649
+3560.0,72.79217075549512
+3561.0,72.80631969679412
+3562.0,72.82046557105302
+3563.0,72.83460838188572
+3564.0,72.8487481329004
+3565.0,72.86288482769962
+3566.0,72.87701846988031
+3567.0,72.89114906303372
+3568.0,72.90527661074559
+3569.0,72.91940111659594
+3570.0,72.93352258415926
+3571.0,72.94764101700447
+3572.0,72.96175641869495
+3573.0,72.97586879278848
+3574.0,72.98997814283727
+3575.0,73.00408447238817
+3576.0,73.01818778498235
+3577.0,73.03228808415554
+3578.0,73.04638537343799
+3579.0,73.06047965635449
+3580.0,73.07457093642434
+3581.0,73.08865921716139
+3582.0,73.10274450207412
+3583.0,73.1168267946655
+3584.0,73.13090609843312
+3585.0,73.1449824168692
+3586.0,73.15905575346055
+3587.0,73.17312611168859
+3588.0,73.1871934950294
+3589.0,73.20125790695373
+3590.0,73.21531935092693
+3591.0,73.22937783040912
+3592.0,73.243433348855
+3593.0,73.25748590971405
+3594.0,73.2715355164304
+3595.0,73.28558217244299
+3596.0,73.2996258811854
+3597.0,73.31366664608599
+3598.0,73.32770447056791
+3599.0,73.34173935804905
+3600.0,73.35577131194209
+3601.0,73.36980033565449
+3602.0,73.38382643258855
+3603.0,73.39784960614136
+3604.0,73.41186985970485
+3605.0,73.4258871966658
+3606.0,73.43990162040578
+3607.0,73.45391313430133
+3608.0,73.4679217417238
+3609.0,73.48192744603942
+3610.0,73.49593025060938
+3611.0,73.50993015878969
+3612.0,73.52392717393134
+3613.0,73.53792129938029
+3614.0,73.55191253847735
+3615.0,73.56590089455835
+3616.0,73.57988637095409
+3617.0,73.5938689709903
+3618.0,73.60784869798775
+3619.0,73.62182555526219
+3620.0,73.63579954612439
+3621.0,73.64977067388011
+3622.0,73.6637389418302
+3623.0,73.67770435327051
+3624.0,73.69166691149199
+3625.0,73.70562661978062
+3626.0,73.71958348141743
+3627.0,73.73353749967865
+3628.0,73.74748867783548
+3629.0,73.76143701915431
+3630.0,73.77538252689665
+3631.0,73.78932520431911
+3632.0,73.80326505467349
+3633.0,73.81720208120667
+3634.0,73.83113628716077
+3635.0,73.84506767577305
+3636.0,73.85899625027596
+3637.0,73.87292201389715
+3638.0,73.88684496985948
+3639.0,73.90076512138104
+3640.0,73.91468247167512
+3641.0,73.92859702395029
+3642.0,73.94250878141033
+3643.0,73.95641774725428
+3644.0,73.97032392467649
+3645.0,73.98422731686658
+3646.0,73.99812792700942
+3647.0,74.01202575828526
+3648.0,74.02592081386956
+3649.0,74.0398130969332
+3650.0,74.05370261064236
+3651.0,74.06758935815853
+3652.0,74.08147334263859
+3653.0,74.09535456723478
+3654.0,74.10923303509472
+3655.0,74.12310874936136
+3656.0,74.13698171317314
+3657.0,74.15085192966383
+3658.0,74.16471940196261
+3659.0,74.17858413319414
+3660.0,74.19244612647847
+3661.0,74.20630538493114
+3662.0,74.22016191166307
+3663.0,74.2340157097807
+3664.0,74.24786678238596
+3665.0,74.2617151325762
+3666.0,74.27556076344433
+3667.0,74.28940367807871
+3668.0,74.30324387956324
+3669.0,74.31708137097735
+3670.0,74.33091615539598
+3671.0,74.34474823588964
+3672.0,74.35857761552437
+3673.0,74.37240429736177
+3674.0,74.38622828445904
+3675.0,74.40004957986892
+3676.0,74.41386818663979
+3677.0,74.42768410781558
+3678.0,74.44149734643587
+3679.0,74.45530790553582
+3680.0,74.46911578814624
+3681.0,74.48292099729362
+3682.0,74.49672353600002
+3683.0,74.51052340728319
+3684.0,74.52432061415655
+3685.0,74.53811515962924
+3686.0,74.55190704670596
+3687.0,74.56569627838728
+3688.0,74.57948285766928
+3689.0,74.59326678754393
+3690.0,74.60704807099879
+3691.0,74.62082671101722
+3692.0,74.6346027105783
+3693.0,74.64837607265689
+3694.0,74.66214680022352
+3695.0,74.67591489624462
+3696.0,74.68968036368229
+3697.0,74.70344320549444
+3698.0,74.71720342463482
+3699.0,74.73096102405292
+3700.0,74.74471600669408
+3701.0,74.75846837549943
+3702.0,74.77221813340601
+3703.0,74.7859652833466
+3704.0,74.79970982824987
+3705.0,74.81345177104039
+3706.0,74.82719111463852
+3707.0,74.84092786196052
+3708.0,74.85466201591858
+3709.0,74.86839357942071
+3710.0,74.88212255537088
+3711.0,74.89584894666895
+3712.0,74.90957275621068
+3713.0,74.92329398688774
+3714.0,74.93701264158781
+3715.0,74.95072872319449
+3716.0,74.96444223458725
+3717.0,74.97815317864165
+3718.0,74.9918615582291
+3719.0,75.00556737621707
+3720.0,75.01927063546901
+3721.0,75.0329713388443
+3722.0,75.04666948919842
+3723.0,75.0603650893828
+3724.0,75.07405814224487
+3725.0,75.08774865062813
+3726.0,75.10143661737214
+3727.0,75.11512204531246
+3728.0,75.12880493728072
+3729.0,75.14248529610462
+3730.0,75.15616312460791
+3731.0,75.16983842561044
+3732.0,75.18351120192811
+3733.0,75.19718145637302
+3734.0,75.21084919175323
+3735.0,75.224514410873
+3736.0,75.23817711653268
+3737.0,75.25183731152877
+3738.0,75.2654949986539
+3739.0,75.27915018069682
+3740.0,75.29280286044248
+3741.0,75.30645304067193
+3742.0,75.32010072416243
+3743.0,75.33374591368742
+3744.0,75.3473886120165
+3745.0,75.36102882191545
+3746.0,75.37466654614632
+3747.0,75.3883017874673
+3748.0,75.40193454863278
+3749.0,75.41556483239347
+3750.0,75.42919264149623
+3751.0,75.44281797868418
+3752.0,75.45644084669668
+3753.0,75.47006124826936
+3754.0,75.48367918613411
+3755.0,75.49729466301912
+3756.0,75.51090768164879
+3757.0,75.52451824474383
+3758.0,75.5381263550213
+3759.0,75.5517320151945
+3760.0,75.56533522797305
+3761.0,75.57893599606291
+3762.0,75.59253432216634
+3763.0,75.60613020898195
+3764.0,75.61972365920467
+3765.0,75.6333146755258
+3766.0,75.64690326063298
+3767.0,75.6604894172102
+3768.0,75.67407314793785
+3769.0,75.68765445549266
+3770.0,75.70123334254782
+3771.0,75.71480981177281
+3772.0,75.72838386583359
+3773.0,75.74195550739246
+3774.0,75.75552473910821
+3775.0,75.76909156363598
+3776.0,75.7826559836274
+3777.0,75.79621800173047
+3778.0,75.80977762058968
+3779.0,75.823334842846
+3780.0,75.83688967113677
+3781.0,75.85044210809588
+3782.0,75.86399215635363
+3783.0,75.87753981853685
+3784.0,75.89108509726881
+3785.0,75.90462799516934
+3786.0,75.9181685148547
+3787.0,75.9317066589377
+3788.0,75.94524243002762
+3789.0,75.95877583073036
+3790.0,75.97230686364824
+3791.0,75.98583553138019
+3792.0,75.99936183652163
+3793.0,76.01288578166458
+3794.0,76.0264073693976
+3795.0,76.03992660230581
+3796.0,76.05344348297089
+3797.0,76.06695801397113
+3798.0,76.08047019788138
+3799.0,76.0939800372731
+3800.0,76.10748753471434
+3801.0,76.12099269276975
+3802.0,76.1344955140006
+3803.0,76.1479960009648
+3804.0,76.16149415621686
+3805.0,76.17498998230793
+3806.0,76.18848348178582
+3807.0,76.20197465719495
+3808.0,76.21546351107644
+3809.0,76.228950045968
+3810.0,76.24243426440412
+3811.0,76.25591616891586
+3812.0,76.26939576203102
+3813.0,76.28287304627406
+3814.0,76.29634802416615
+3815.0,76.30982069822512
+3816.0,76.32329107096562
+3817.0,76.33675914489888
+3818.0,76.3502249225329
+3819.0,76.36368840637245
+3820.0,76.377149598919
+3821.0,76.39060850267074
+3822.0,76.40406512012264
+3823.0,76.41751945376643
+3824.0,76.43097150609056
+3825.0,76.44442127958028
+3826.0,76.45786877671759
+3827.0,76.47131399998132
+3828.0,76.48475695184702
+3829.0,76.49819763478706
+3830.0,76.51163605127063
+3831.0,76.52507220376371
+3832.0,76.53850609472907
+3833.0,76.55193772662635
+3834.0,76.56536710191195
+3835.0,76.57879422303914
+3836.0,76.59221909245804
+3837.0,76.60564171261555
+3838.0,76.6190620859555
+3839.0,76.63248021491857
+3840.0,76.64589610194221
+3841.0,76.65930974946083
+3842.0,76.6727211599057
+3843.0,76.68613033570492
+3844.0,76.69953727928355
+3845.0,76.71294199306348
+3846.0,76.72634447946355
+3847.0,76.73974474089945
+3848.0,76.75314277978387
+3849.0,76.76653859852631
+3850.0,76.77993219953326
+3851.0,76.79332358520814
+3852.0,76.80671275795129
+3853.0,76.82009972015999
+3854.0,76.83348447422847
+3855.0,76.84686702254793
+3856.0,76.86024736750649
+3857.0,76.87362551148928
+3858.0,76.8870014568784
+3859.0,76.9003752060529
+3860.0,76.91374676138882
+3861.0,76.92711612525925
+3862.0,76.94048330003413
+3863.0,76.9538482880806
+3864.0,76.96721109176264
+3865.0,76.98057171344132
+3866.0,76.99393015547473
+3867.0,77.00728642021797
+3868.0,77.02064051002316
+3869.0,77.03399242723948
+3870.0,77.04734217421318
+3871.0,77.06068975328746
+3872.0,77.07403516680267
+3873.0,77.08737841709622
+3874.0,77.1007195065025
+3875.0,77.11405843735307
+3876.0,77.12739521197649
+3877.0,77.14072983269844
+3878.0,77.1540623018417
+3879.0,77.16739262172614
+3880.0,77.1807207946687
+3881.0,77.19404682298345
+3882.0,77.20737070898157
+3883.0,77.22069245497136
+3884.0,77.23401206325823
+3885.0,77.24732953614472
+3886.0,77.26064487593054
+3887.0,77.27395808491245
+3888.0,77.28726916538449
+3889.0,77.30057811963775
+3890.0,77.31388494996045
+3891.0,77.3271896586381
+3892.0,77.34049224795324
+3893.0,77.35379272018567
+3894.0,77.36709107761236
+3895.0,77.3803873225074
+3896.0,77.39368145714215
+3897.0,77.40697348378512
+3898.0,77.42026340470204
+3899.0,77.4335512221558
+3900.0,77.44683693840658
+3901.0,77.46012055571167
+3902.0,77.4734020763257
+3903.0,77.48668150250046
+3904.0,77.49995883648495
+3905.0,77.51323408052548
+3906.0,77.52650723686554
+3907.0,77.53977830774589
+3908.0,77.55304729540454
+3909.0,77.5663142020768
+3910.0,77.57957902999516
+3911.0,77.59284178138945
+3912.0,77.60610245848676
+3913.0,77.61936106351142
+3914.0,77.63261759868513
+3915.0,77.64587206622677
+3916.0,77.65912446835259
+3917.0,77.67237480727616
+3918.0,77.68562308520826
+3919.0,77.69886930435707
+3920.0,77.71211346692807
+3921.0,77.72535557512404
+3922.0,77.73859563114506
+3923.0,77.75183363718864
+3924.0,77.7650695954495
+3925.0,77.7783035081198
+3926.0,77.79153537738901
+3927.0,77.80476520544397
+3928.0,77.8179929944688
+3929.0,77.83121874664508
+3930.0,77.84444246415173
+3931.0,77.85766414916503
+3932.0,77.87088380385862
+3933.0,77.88410143040355
+3934.0,77.89731703096827
+3935.0,77.91053060771856
+3936.0,77.92374216281766
+3937.0,77.93695169842618
+3938.0,77.95015921670218
+3939.0,77.96336471980105
+3940.0,77.97656820987565
+3941.0,77.98976968907631
+3942.0,78.00296915955067
+3943.0,78.01616662344391
+3944.0,78.02936208289857
+3945.0,78.04255554005469
+3946.0,78.0557469970497
+3947.0,78.06893645601853
+3948.0,78.08212391909355
+3949.0,78.09530938840456
+3950.0,78.10849286607886
+3951.0,78.12167435424124
+3952.0,78.13485385501387
+3953.0,78.14803137051655
+3954.0,78.16120690286641
+3955.0,78.17438045417819
+3956.0,78.18755202656405
+3957.0,78.20072162213367
+3958.0,78.21388924299421
+3959.0,78.22705489125043
+3960.0,78.2402185690045
+3961.0,78.25338027835612
+3962.0,78.26654002140258
+3963.0,78.27969780023861
+3964.0,78.29285361695653
+3965.0,78.30600747364622
+3966.0,78.31915937239502
+3967.0,78.33230931528784
+3968.0,78.3454573044072
+3969.0,78.3586033418331
+3970.0,78.37174742964316
+3971.0,78.38488956991249
+3972.0,78.39802976471385
+3973.0,78.41116801611753
+3974.0,78.42430432619138
+3975.0,78.43743869700087
+3976.0,78.45057113060903
+3977.0,78.46370162907652
+3978.0,78.47683019446156
+3979.0,78.48995682881994
+3980.0,78.50308153420512
+3981.0,78.51620431266815
+3982.0,78.52932516625765
+3983.0,78.54244409701992
+3984.0,78.55556110699887
+3985.0,78.56867619823596
+3986.0,78.5817893727704
+3987.0,78.59490063263897
+3988.0,78.60800997987606
+3989.0,78.62111741651375
+3990.0,78.63422294458181
+3991.0,78.64732656610757
+3992.0,78.66042828311606
+3993.0,78.67352809762998
+3994.0,78.68662601166972
+3995.0,78.69972202725324
+3996.0,78.71281614639632
+3997.0,78.72590837111228
+3998.0,78.73899870341225
+3999.0,78.75208714530491
+4000.0,78.76517369879676
+4001.0,78.77825836589194
+4002.0,78.79134114859227
+4003.0,78.80442204889732
+4004.0,78.81750106880433
+4005.0,78.83057821030827
+4006.0,78.84365347540187
+4007.0,78.85672686607548
+4008.0,78.86979838431725
+4009.0,78.88286803211308
+4010.0,78.89593581144653
+4011.0,78.90900172429897
+4012.0,78.92206577264947
+4013.0,78.93512795847487
+4014.0,78.9481882837497
+4015.0,78.96124675044635
+4016.0,78.97430336053489
+4017.0,78.98735811598318
+4018.0,79.00041101875686
+4019.0,79.0134620708193
+4020.0,79.02651127413169
+4021.0,79.03955863065298
+4022.0,79.05260414233987
+4023.0,79.06564781114695
+4024.0,79.07868963902648
+4025.0,79.09172962792861
+4026.0,79.10476777980122
+4027.0,79.11780409659005
+4028.0,79.13083858023862
+4029.0,79.14387123268826
+4030.0,79.15690205587812
+4031.0,79.16993105174518
+4032.0,79.18295822222422
+4033.0,79.1959835692479
+4034.0,79.20900709474664
+4035.0,79.22202880064876
+4036.0,79.23504868888037
+4037.0,79.24806676136548
+4038.0,79.26108302002588
+4039.0,79.27409746678127
+4040.0,79.28711010354917
+4041.0,79.30012093224498
+4042.0,79.31312995478197
+4043.0,79.32613717307123
+4044.0,79.33914258902178
+4045.0,79.35214620454049
+4046.0,79.3651480215321
+4047.0,79.37814804189924
+4048.0,79.39114626754244
+4049.0,79.40414270036011
+4050.0,79.41713734224855
+4051.0,79.43013019510197
+4052.0,79.44312126081246
+4053.0,79.45611054127004
+4054.0,79.46909803836262
+4055.0,79.48208375397606
+4056.0,79.4950676899941
+4057.0,79.50804984829838
+4058.0,79.52103023076855
+4059.0,79.53400883928211
+4060.0,79.54698567571451
+4061.0,79.55996074193916
+4062.0,79.57293403982739
+4063.0,79.58590557124847
+4064.0,79.59887533806965
+4065.0,79.61184334215608
+4066.0,79.6248095853709
+4067.0,79.6377740695752
+4068.0,79.65073679662804
+4069.0,79.66369776838643
+4070.0,79.67665698670534
+4071.0,79.68961445343774
+4072.0,79.70257017043458
+4073.0,79.7155241395448
+4074.0,79.72847636261524
+4075.0,79.74142684149083
+4076.0,79.75437557801445
+4077.0,79.76732257402693
+4078.0,79.78026783136723
+4079.0,79.79321135187215
+4080.0,79.80615313737658
+4081.0,79.81909318971344
+4082.0,79.83203151071362
+4083.0,79.84496810220605
+4084.0,79.85790296601765
+4085.0,79.87083610397339
+4086.0,79.88376751789629
+4087.0,79.89669720960732
+4088.0,79.90962518092556
+4089.0,79.92255143366813
+4090.0,79.93547596965013
+4091.0,79.94839879068473
+4092.0,79.96131989858318
+4093.0,79.97423929515477
+4094.0,79.98715698220681
+4095.0,80.00007296154472
diff --git a/src/controller/controller/pot_converter.py b/src/controller/controller/pot_converter.py
new file mode 100644
index 0000000..935507e
--- /dev/null
+++ b/src/controller/controller/pot_converter.py
@@ -0,0 +1,122 @@
+import rclpy
+import csv
+from rclpy.node import Node
+from rcl_interfaces.msg import ParameterType, ParameterDescriptor
+from std_msgs.msg import Float32, Int32
+from std_srvs.srv import SetBool
+from controller.helpers.filters import LowPassFilter
+from controller.helpers.joystick import Joystick
+import os
+
+class PotConverter(Node):
+ def __init__(self):
+ super().__init__('pot_converter')
+
+ self.declare_parameter('hand', 'right') # Declare 'hand' parameter
+
+ # Declare parameters
+ self.declare_parameter('alpha', 0.5,
+ ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE, description="Low-pass filter coefficient"))
+ self.declare_parameter('joy_min_input', 9)
+ self.declare_parameter('joy_zero', 40)
+ self.declare_parameter('joy_max_input', 56)
+ self.declare_parameter('joy_min_output', 0.0)
+ self.declare_parameter('joy_max_output', 0.5)
+
+ self.hand = self.get_parameter('hand').value
+
+ # Validate 'hand' parameter
+ if self.hand not in ["right", "left"]:
+ self.get_logger().warn("Invalid 'hand' parameter! Must be 'right' or 'left'. Defaulting to 'right'.")
+ self.hand = "right"
+
+ # Get initial parameter values
+ self.alpha = self.get_parameter('alpha').value
+ joy_min_input = self.get_parameter('joy_min_input').value
+ joy_zero = self.get_parameter('joy_zero').value
+ joy_max_input = self.get_parameter('joy_max_input').value
+ joy_min_output = self.get_parameter('joy_min_output').value
+ joy_max_output = self.get_parameter('joy_max_output').value
+
+ # Initialize objects
+ self.low_pass_filter = LowPassFilter(self.alpha)
+ self.joystick = Joystick(joy_zero, 10, joy_max_input, joy_min_input, joy_max_output)
+
+ # Subscribe to pot topic
+ self.pub = self.create_publisher(Float32, self.hand + '/cmd_vel_vert', 10)
+ self.filter_pub = self.create_publisher(Float32, self.hand + '/pot_filter', 10)
+ self.subscription = self.create_subscription(Int32, 'controller/'+ self.hand + '/pot', self.pot_callback, 10)
+
+
+ # Load calibration data from the same directory as the script
+ script_dir = os.path.dirname(os.path.realpath(__file__))
+ self.calibration_file = os.path.join(script_dir, 'pot_calibration.csv')
+ self.calibration_data = self.load_calibration()
+
+ # Add parameter callback
+ self.add_on_set_parameters_callback(self.param_callback)
+
+ self.center_srv = self.create_service(SetBool, self.hand + "/center_pot", self.center_pot)
+
+ def load_calibration(self):
+ calibration_data = {}
+ try:
+ with open(self.calibration_file, 'r') as file:
+ reader = csv.reader(file)
+ next(reader) # Skip header row
+ for row in reader:
+ y_value = int(float(row[0]))
+ x_value = float(row[1])
+ calibration_data[y_value] = x_value
+ except Exception as e:
+ self.get_logger().error(f"Error loading calibration file: {e}")
+ return calibration_data
+
+ def pot_callback(self, msg):
+ if 0 <= msg.data <= 4095:
+ filtered_reading = self.low_pass_filter.update(msg.data)
+ corresponding_value = self.calibration_data[int(filtered_reading)]
+
+ filtered_msg = Float32()
+ filtered_msg.data = float(corresponding_value)
+ self.filter_pub.publish(filtered_msg)
+
+ cmd_vel_vert = self.joystick.get_output(corresponding_value)
+
+ # Publish vertical command velocity
+ cmd_msg = Float32()
+ cmd_msg.data = float(cmd_vel_vert) # Example scaling
+ # print(f"reading: {filtered_reading}, cali: {corresponding_value}, vel: {cmd_vel_vert}")
+ self.pub.publish(cmd_msg)
+
+ def param_callback(self, params):
+ for param in params:
+ if param.name == 'alpha':
+ self.alpha = param.value
+ self.low_pass_filter = LowPassFilter(self.alpha)
+ elif param.name in ['joy_min_input', 'joy_zero', 'joy_max_input', 'joy_min_output', 'joy_max_output']:
+ joy_min_input = self.get_parameter('joy_min_input').get_parameter_value().integer_value
+ joy_zero = self.get_parameter('joy_zero').get_parameter_value().integer_value
+ joy_max_input = self.get_parameter('joy_max_input').get_parameter_value().integer_value
+ joy_min_output = self.get_parameter('joy_min_output').get_parameter_value().double_value
+ joy_max_output = self.get_parameter('joy_max_output').get_parameter_value().double_value
+ self.joystick = Joystick(joy_min_input, joy_zero, joy_max_input, joy_min_output, joy_max_output)
+
+ return rclpy.parameter.SetParametersResult(successful=True)
+
+ def center_pot(self, request, response):
+ corresponding_value = self.calibration_data[int(self.low_pass_filter.filtered_value)]
+ self.joystick.recenter(center=corresponding_value)
+ response.success = True
+ self.get_logger().info(f"Potentiometer recentered to {corresponding_value}")
+ return response
+
+def main(args=None):
+ rclpy.init(args=args)
+ pot_converter = PotConverter()
+ rclpy.spin(pot_converter)
+ pot_converter.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/controller/launch/controller_launch.py b/src/controller/launch/controller_launch.py
new file mode 100644
index 0000000..4b122fd
--- /dev/null
+++ b/src/controller/launch/controller_launch.py
@@ -0,0 +1,154 @@
+import os
+import subprocess
+import launch
+import launch_ros.actions
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+
+def check_and_launch_drone_movement(context, *args, **kwargs):
+ """Check if 'drone_movement' is running and launch it if not found."""
+ try:
+ # ✅ Run `ros2 node list` to check if 'drone_movement' is already running
+ result = subprocess.run(['ros2', 'node', 'list'], stdout=subprocess.PIPE, text=True)
+ running_nodes = result.stdout.splitlines()
+
+ if '/drone_movement' in running_nodes:
+ print("✅ 'drone_movement' is already running, skipping launch.")
+ return [] # Don't launch another instance
+ else:
+ print("🚀 'drone_movement' is not running, launching now.")
+ return [
+ launch_ros.actions.Node(
+ package='controller',
+ executable='drone_movement',
+ name='drone_movement',
+ output='screen',
+ parameters=[{
+ 'some_param': 'value' # Replace with actual parameters if needed
+ }]
+ )
+ ]
+
+ except Exception as e:
+ print(f"⚠️ Error checking running nodes: {e}")
+ return [] # Fallback to not launching if an error occurs
+
+def generate_launch_description():
+ # ✅ Declare launch arguments correctly
+ serial_port_right_arg = DeclareLaunchArgument(
+ 'serial_port_right',
+ default_value='/dev/ttyUSB1',
+ description='Serial port for the controller'
+ )
+
+ serial_port_left_arg = DeclareLaunchArgument(
+ 'serial_port_left',
+ default_value='/dev/ttyUSB0',
+ description='Serial port for the controller'
+ )
+
+ baud_rate_arg = DeclareLaunchArgument(
+ 'baud_rate',
+ default_value='115200',
+ description='Baud rate for serial communication'
+ )
+
+
+ return launch.LaunchDescription([
+ serial_port_right_arg,
+ serial_port_left_arg,
+ baud_rate_arg,
+
+ # ✅ Controller Publisher Node
+ launch_ros.actions.Node(
+ package='controller',
+ executable='controller_pub',
+ name='controller_pub_right',
+ parameters=[{
+ 'serial_port': LaunchConfiguration('serial_port_right'),
+ 'baud_rate': LaunchConfiguration('baud_rate'),
+ 'hand': 'right'
+ }],
+ output='screen'
+ ),
+
+ # ✅ IMU Converter Node
+ launch_ros.actions.Node(
+ package='controller',
+ executable='imu_converter',
+ name='imu_converter_right',
+ parameters=[{
+ 'hand': 'right'
+ }],
+ output='screen'
+ ),
+
+ # ✅ Potentiometer Converter Node
+ launch_ros.actions.Node(
+ package='controller',
+ executable='pot_converter',
+ name='pot_converter_right',
+ parameters=[{
+ 'hand': 'right'
+ }],
+ output='screen'
+ ),
+
+ launch_ros.actions.Node(
+ package='controller',
+ executable='button_manager',
+ name='button_manager_right',
+ parameters=[{
+ 'hand': 'right'
+ }],
+ output='screen'
+ ),
+
+ # ✅ Controller Publisher Node
+ launch_ros.actions.Node(
+ package='controller',
+ executable='controller_pub',
+ name='controller_pub_left',
+ parameters=[{
+ 'serial_port': LaunchConfiguration('serial_port_left'),
+ 'baud_rate': LaunchConfiguration('baud_rate'),
+ 'hand': 'left'
+ }],
+ output='screen'
+ ),
+
+ # ✅ IMU Converter Node
+ launch_ros.actions.Node(
+ package='controller',
+ executable='imu_converter',
+ name='imu_converter_left',
+ parameters=[{
+ 'hand': 'left'
+ }],
+ output='screen'
+ ),
+
+ # ✅ Potentiometer Converter Node
+ launch_ros.actions.Node(
+ package='controller',
+ executable='pot_converter',
+ name='pot_converter_left',
+ parameters=[{
+ 'hand': 'left'
+ }],
+ output='screen'
+ ),
+
+ launch_ros.actions.Node(
+ package='controller',
+ executable='button_manager',
+ name='button_manager_left',
+ parameters=[{
+ 'hand': 'left'
+ }],
+ output='screen'
+ ),
+
+ # ✅ Check and launch drone_movement only if it's not running
+ OpaqueFunction(function=check_and_launch_drone_movement)
+ ])
diff --git a/src/controller/package.xml b/src/controller/package.xml
new file mode 100644
index 0000000..aece323
--- /dev/null
+++ b/src/controller/package.xml
@@ -0,0 +1,25 @@
+
+
+
+ controller
+ 0.0.0
+ TODO: Package description
+ sleepyyrei
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+ launch
+ launch_ros
+ rclpy
+ std_msgs
+ sensor_msgs
+ geometry_msgs
+ std_srvs
+
+
+ ament_python
+
+
diff --git a/src/controller/resource/controller b/src/controller/resource/controller
new file mode 100644
index 0000000..e69de29
diff --git a/src/controller/setup.cfg b/src/controller/setup.cfg
new file mode 100644
index 0000000..1049f8a
--- /dev/null
+++ b/src/controller/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/controller
+[install]
+install_scripts=$base/lib/controller
diff --git a/src/controller/setup.py b/src/controller/setup.py
new file mode 100644
index 0000000..ca90980
--- /dev/null
+++ b/src/controller/setup.py
@@ -0,0 +1,36 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'controller'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(
+ where='.', # Search for packages in the current directory
+ include=['controller', 'controller.helpers', 'controller.madgwick_py'] # Ensure helpers is included
+ ),
+ data_files=[ # Make sure the resource files and launch files are included
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), # ✅ Add this to include launch files
+ ],
+ install_requires=['setuptools','rclpy', 'std_msgs', 'sensor_msgs', 'geometry_msgs', 'std_srvs'],
+ zip_safe=True,
+ maintainer='sleepyyrei',
+ maintainer_email='sleepyyrei@todo.todo', # Replace with your email
+ description='This package provides control logic for robot systems.', # Provide a real description
+ license='MIT', # Replace with the license you're using
+ tests_require=['pytest'],
+ entry_points={ # Specify ROS 2 node scripts here
+ 'console_scripts': [
+ 'controller_pub = controller.controller_pub:main',
+ 'imu_converter = controller.imu_converter:main',
+ 'pot_converter = controller.pot_converter:main',
+ 'button_manager = controller.button_manager:main',
+ 'drone_movement = controller.drone_movement:main', # Adjust the node entry point accordingly
+ ],
+ },
+)
diff --git a/src/controller/test/test_copyright.py b/src/controller/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/controller/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/src/controller/test/test_flake8.py b/src/controller/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/controller/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/src/controller/test/test_pep257.py b/src/controller/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/controller/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/interfaces/interfaces/CMakeLists.txt b/src/interfaces/interfaces/CMakeLists.txt
index 53b717f..ddeef1f 100644
--- a/src/interfaces/interfaces/CMakeLists.txt
+++ b/src/interfaces/interfaces/CMakeLists.txt
@@ -13,6 +13,8 @@ find_package(rosidl_default_generators REQUIRED)
# find_package( REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/TogglePin.srv"
+ "srv/SetFloat.srv"
+ "srv/ToggleStepper.srv"
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
diff --git a/src/interfaces/interfaces/srv/SetFloat.srv b/src/interfaces/interfaces/srv/SetFloat.srv
new file mode 100644
index 0000000..a8de684
--- /dev/null
+++ b/src/interfaces/interfaces/srv/SetFloat.srv
@@ -0,0 +1,4 @@
+float32 data # GPIO pin number
+---
+bool success # Whether the operation was successful
+string message # Status message
\ No newline at end of file
diff --git a/src/interfaces/interfaces/srv/ToggleStepper.srv b/src/interfaces/interfaces/srv/ToggleStepper.srv
new file mode 100644
index 0000000..ae34b7d
--- /dev/null
+++ b/src/interfaces/interfaces/srv/ToggleStepper.srv
@@ -0,0 +1,5 @@
+int32 stepper_id # List of GPIO pin numbers
+float32 speed # Desired speed (e.g., PWM duty cycle or stepper speed)
+---
+bool success # Whether the operation was successful
+string message # Status message
\ No newline at end of file