docs

documentation/src/SUMMARY.md

@@ -6,14 +6,13 @@
 - [Coordinate System](ch02-01-coordinate-system.md)
 # Building
 - [Required Materials](ch03-01-required-materials.md)
-- [Building](ch04-00-building.md)
-	- [Raspberry Pi Setup](ch04-01-raspberry-pi-setup.md)
-	- [Building The Legs](ch04-02-building-leg.md)
-	- [Building The Main Body](ch04-03-building-body.md)
+- [Raspberry Pi Setup](ch04-01-raspberry-pi-setup.md)
+- [Building](ch05-00-building.md)
+	- [Building The Legs](ch05-02-building-leg.md)
+	- [Building The Main Body](ch05-03-building-body.md)
 - [Installing Software](ch05-01-installing-software.md)
 # API
-- [ROS2 topics](ch06-01-ros2-topics.md)
-- [Code Structure](ch07-01-code-structure.md)
+- [ROS2 Topics](ch06-01-ros2-topics.md)
 # Concepts explained
 
 - [Inverse Kinematics](ch08-00-inverse-kinematics.md)

documentation/src/ch05-02-building-leg.md

@@ -0,0 +1,5 @@
+# Building The Legs
+## Required 3D-printed parts per leg:
+- Left legs:
+	- [Leg-Top-Left]()
+- Right legs:

documentation/src/ch06-01-ros2-topics.md

@@ -0,0 +1,60 @@
+# Ros2 Topics
+This chapter outlines the various ros2 topics AQLARP has that you can interact with to adapt the robot to your use case.
+
+## Heading topic
+Topic name: `heading`
+
+Package: `aqlarp_main`
+
+Description: with this topic you can specify where the robot should go to and at what speed. The heading parameters will be limited to a circle of radius one and the rotation parameter will be limited between -1 and 1.
+
+Data type: [Heading](https://github.com/DeDiamondPro/AQLARP/blob/master/ros2_ws/src/aqlarp_interfaces/msg/Heading.msg)
+
+Values:
+- `float32 x_heading`
+- `float32 z_heading`
+- `float32 rotation`
+## Power topic
+Topic name: `power`
+
+Package: `aqlarp_main`
+
+Description: with this topic you can set the power to the servos on or off.
+
+Data type: Boolean
+
+## Gyro topic
+Topic name: `gryo_angles`
+
+Package: `aqlarp_sensors`
+
+Description: all gyro related data is published on this topic, this is published 100 times per second.
+
+Data type: [GyroAngles](https://github.com/DeDiamondPro/AQLARP/blob/master/ros2_ws/src/aqlarp_interfaces/msg/GyroAngles.msg)
+
+Values:
+- `float32 pitch`
+- `float32 roll`
+- `float32[3] angular_velocity`
+- `float32[3] acceleration`
+
+## Joint angles topic
+Topic name: `joint_angles`
+
+Package: `aqlarp_motors`
+
+Description: with this topic you can specify the angles that all servos should be set to.
+
+Data type: [JointAngles](https://github.com/DeDiamondPro/AQLARP/blob/master/ros2_ws/src/aqlarp_interfaces/msg/JointAngles.msg)
+
+Values:
+- `float32[12] angles`
+
+## Disable joints topic
+Topic name: `disable_joints`
+
+Package: `aqlarp_motors`
+
+Description: power off all servos
+
+Data type: Empty

ros2_ws/src/aqlarp_input/aqlarp_input/controller_input.py

@@ -7,54 +7,76 @@
 class PS4ControllerNode(Node):
     def __init__(self):
         super().__init__('controller_input')
+
+        # Initialize pygame and the joysticks
         pygame.init()
         for i in range(pygame.joystick.get_count()):
             pygame.joystick.Joystick(i).init()
         self.get_logger().info('Controller input node started')
 
+        # Initialize variables
         self.enabled = False
 
+        # Create publishers
         self.power_publisher = self.create_publisher(Bool, 'power', 10)
         self.heading_publisher = self.create_publisher(Heading, 'heading', 10)
 
+        # Create a timer to update the heading 100 times per second
         self.timer = self.create_timer(0.01, self.update)
 
-    def convert_state(self, state):
-        if abs(state) < 0.2: # Deadzone
+    # Function to apply a dead zone to the controller input, 
+    # This prevents the robot from moving when the joystick is not being touched due to light stick drift
+    def apply_dead_zone(self, state):
+        if abs(state) < 0.2:
             return 0.0
         return state
 
+    # Function to update the heading of the robot
     def update(self):
         try:
+            # Process events
             for event in pygame.event.get():
                 self.process_event(event)
-            joystick_count = pygame.joystick.get_count()
-            if joystick_count < 1:
+
+            # If there are no joysticks, return
+            if pygame.joystick.get_count() < 1:
                 return
+
+            # Initialize the heading message
             heading = Heading()
-            heading.x_heading = -self.convert_state(pygame.joystick.Joystick(0).get_axis(1))
-            heading.z_heading = self.convert_state(pygame.joystick.Joystick(0).get_axis(0))
-            heading.rotation_heading = -self.convert_state(pygame.joystick.Joystick(0).get_axis(3))
+
+            # Set the heading values
+            heading.x_heading = -self.apply_dead_zone(pygame.joystick.Joystick(0).get_axis(1))
+            heading.z_heading = self.apply_dead_zone(pygame.joystick.Joystick(0).get_axis(0))
+            heading.rotation_heading = -self.apply_dead_zone(pygame.joystick.Joystick(0).get_axis(3))
+
+            # Log the heading values to the debug log
             self.get_logger().debug(f"Heading: x={heading.x_heading}, z={heading.z_heading}, rotation={heading.rotation_heading}")
+            # Publish the heading message
             self.heading_publisher.publish(heading)
         except:
             pass
 
-
+    # Process a controller event
     def process_event(self, event):
+        # If the PS button is pressed, toggle the enabled state
         if event.type == pygame.JOYBUTTONDOWN and event.button == 10:
             self.enabled = not self.enabled
             self.power_publisher.publish(Bool(data=self.enabled))
+        # If a controller is disconnected, make sure the robot stops moving
         elif event.type == pygame.JOYDEVICEREMOVED:
            self.heading_publisher.publish(Heading())
 
 def main(args = None):
+    # Initialize rclpy
     rclpy.init(args=args)
 
+    # Create the node
     node = PS4ControllerNode()
-
+    # Run the node
     rclpy.spin(node)
 
+    # Cleanup
     node.destroy_node()
     rclpy.shutdown()
 

ros2_ws/src/aqlarp_motors/aqlarp_motors/aqlarp_motors.py

@@ -8,15 +8,17 @@
 
 pca = ServoKit(channels=16)
 
-
+# Clamp a value between a minimum and maximum
 def clamp(value, min, max):
     return max if value > max else min if value < min else value
 
 
 def load_offsets():
+    # Find the servo offsets file
     user_dir = os.path.expanduser("~")
     offsets_file = os.path.join(user_dir, '.aqlarp/servo_offsets.yaml')
 
+    # Load the offsets file if it exists
     if os.path.exists(offsets_file):
         with open(offsets_file, 'r', encoding='utf8') as infile:
             offsets = yaml.safe_load(infile)
@@ -29,6 +31,7 @@ class AngleListener(Node):
     def __init__(self):
         super().__init__('angle_listener')
 
+        # Set update frequency and servo pulse width range
         pca.frequency = 100
         for servo in range(12):
             pca.servo[servo].set_pulse_width_range(500, 2500)

ros2_ws/src/aqlarp_motors/aqlarp_motors/calibration.py

@@ -7,17 +7,22 @@
 import os
 
 def save_offsets(offsets):
+    # Find the offsets file
     user_dir = os.path.expanduser("~")
     config_dir = os.path.join(user_dir, '.aqlarp')
+    # Create the config directory if it doesn't exist
     os.makedirs(config_dir, exist_ok=True)
 
+    # Save the offsets to the file
     with open(os.path.join(config_dir, 'servo_offsets.yaml'), 'w+', encoding='utf8') as outfile:
         yaml.dump(offsets, outfile)
 
 def load_offsets():
+    # Find the offsets file
     user_dir = os.path.expanduser("~")
     offsets_file = os.path.join(user_dir, '.aqlarp/servo_offsets.yaml')
 
+    # Load the offsets file if it exists
     if os.path.exists(offsets_file):
         with open(offsets_file, 'r', encoding='utf8') as infile:
             offsets = yaml.safe_load(infile)
@@ -28,39 +33,48 @@ def load_offsets():
 class ServoCalibrationNode(Node):
     def __init__(self):
         super().__init__('servo_calibration_node')
+        # Initialize variables
         self.kit = ServoKit(channels=16)
         self.current_joint = 0
         self.offsets = load_offsets()
         self.joint_to_pin = [6, 7, 8, 9, 10, 11, 3, 4, 5, 0, 1, 2]
 
+    # Clamp a value between a minimum and maximum
     def clamp(self, value, min, max):
         return max if value > max else min if value < min else value
 
     def calibrate(self, stdscr):
+        # Set up curses
         curses.curs_set(0)
         stdscr.nodelay(True)
         stdscr.timeout(100)
 
+        # Set the initial servo angles
         for i in range(12):
             self.kit.servo[i].angle = 90 + self.offsets[i]
 
         while True:
+            # Clear the screen
             stdscr.clear()
+            # Print the current joint and offset to the screen
             leg_name = ('top ' if self.current_joint < 6 else 'bottom ') + ('left' if math.floor(self.current_joint / 3) % 2 == 0 else 'right')
             joint_name = ('top' if self.current_joint % 3 == 0 else 'bottom' if self.current_joint % 3 == 1 else 'side')
             stdscr.addstr(0, 0, f'Calibrating {leg_name} leg, {joint_name} joint. Press esc or q to exit and save.')
             stdscr.addstr(1, 0, f'Current offset: < {self.offsets[self.joint_to_pin[self.current_joint]]} >')
 
+            # Get the key press
             c = stdscr.getch()
+            # Get the pin number of the current joint
             pin = self.joint_to_pin[self.current_joint]
 
+            # If escape or q is pressed, exit and save   
             if c == 27 or c == 113:
-                # If escape or q is pressed, exit and save
                 for i in range(12):
                     self.kit.servo[i].angle = None
                 save_offsets(self.offsets)
                 print('Calibration complete, settings saved.')
                 break
+            # If the arrow keys are pressed, change the current joint or offset
             if c == curses.KEY_UP and self.current_joint < 11:
                 self.current_joint += 1
             elif c == curses.KEY_DOWN and self.current_joint > 0:
@@ -70,15 +84,19 @@ def calibrate(self, stdscr):
             elif c == curses.KEY_LEFT:
                 self.offsets[pin] = self.clamp(self.offsets[pin] - 1, -90, 90)
 
+            # Set the servo angle
             self.kit.servo[pin].angle = 90 + self.offsets[pin]
 
 def main(args=None):
+    # Initialize the rclpy
     rclpy.init(args=args)
-
+    # Create the node
     node = ServoCalibrationNode()
 
+    # Run the calibration function
     curses.wrapper(node.calibrate)
 
+    # Cleanup
     node.destroy_node()
     rclpy.shutdown()
 

ros2_ws/src/aqlarp_sensors/aqlarp_sensors/gyro.py

@@ -6,6 +6,7 @@
 import adafruit_mpu6050
 from math import *
 
+# Function to convert a 2D vector to degrees
 def vector_2_degrees(x, y):
     angle = degrees(atan2(y, x))
     if angle < 0:
@@ -15,25 +16,45 @@ def vector_2_degrees(x, y):
 class GyroAccelNode(Node):
     def __init__(self):
         super().__init__('gyro')
+
+        # Create a publisher on the 'gyro_angles' topic
         self.publisher = self.create_publisher(GyroAngles, 'gryo_angles', 10)
+
+        # Initialize the I2C bus and the MPU6050 sensor
         i2c = busio.I2C(board.SCL, board.SDA)
         self.mpu = adafruit_mpu6050.MPU6050(i2c)
+
+        # Create a timer to read the sensor data 100 times per second
         self.timer = self.create_timer(0.01, self.timer_callback)
 
     def timer_callback(self):
+        # Read the sensor data
         acceleration = self.mpu.acceleration
+
+        # Initialize the GyroAngles message
         msg = GyroAngles()
+        # Set the roll and pitch angles
         msg.roll = vector_2_degrees(acceleration[0], acceleration[2])
         msg.pitch = vector_2_degrees(acceleration[1], acceleration[2])
+        # Set the acceleration and angular velocity
         msg.acceleration = acceleration
         msg.angular_velocity = self.mpu.gyro
+
+        # Log the roll, pitch, acceleration, and angular velocity to the debug log
         self.get_logger().debug(f'Roll: {msg.roll}, Pitch: {msg.pitch}, Acceleration: {msg.acceleration}, Angular Velocity: {msg.angular_velocity}', throttle_duration_sec = 0.1)
+        # Publish the message
         self.publisher.publish(msg)
 
 def main(args=None):
+    # Initialize rclpy
     rclpy.init(args=args)
+
+    # Create the node
     gyro_accel_node = GyroAccelNode()
+    # Run the node
     rclpy.spin(gyro_accel_node)
+
+    # Cleanup
     gyro_accel_node.destroy_node()
     rclpy.shutdown()