progress

documentation/src/ch05-01-installing-software.md

@@ -1,4 +1,57 @@
+# Installing software
+In this chapter we will go over installing and setting up all software required for AQLARP to work. You should already have AQLARP's GitHub repo cloned in your home directory as specified in [chapter 4.1](/ch04-01-raspberry-pi-setup.html#clone-the-github-repo)
+## Installing ros2
+AQLARP is built using ros2 iron, it might work with later versions but this is untested. To install ros2 iron you can follow [their installation instructions](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html). After installing ros2 you must also install colcon, this is ros2's build system. To do this run the command below.
+```console
+$ sudo apt install python3-colcon-common-extensions
+```
+## Sourcing ros2 automatically
+To use ros2 you must source it every time, this can be annoying. To do this automatically run the following command.
+```console
+nano ~/.bashrc
+```
+Then add these 2 lines at the end of the file.
+```bash
+source /opt/ros/iron/setup.bash
+source ~/new_ws/install/setup.bash
+```
+Then press `Ctrl + O` to save and `Ctrl + X` to exit. To make this apply you should log out of the Raspberry Pi and start a new shell session.
+## Installing dependencies
+To install dependencies first go into the workspace directory, to do this run the following command.
+```console
+$ cd ~/AQLARP/ros2_ws
+```
+Then run the following command to install all required dependencies using rosdep.
+```console
+$ rosdep install --from-paths src -y --ignore-src
+```
+Then additionally you have to install pygame using pip, since the version rosdep uses is severally outdated. To do this run the following command.
+```console
+$ python3 -m pip install pygame
+```
 
+## Building the code
+You should be in the workspace directory as specified in the previous step. Then to build the code run the following command and wait for it to complete
+```console
+$ colcon build
+```
+## Calibrating the servos
+Since the servos aren't exactly at 90° they have to be calibrated. To run the calibration script run the following command.
+```console
+$ ros2 run aqlarp_motors calibration
+```
+Then to use the calibration script use the left and right arrow keys to increase or decrease the angle offset and use the up and down arrow keys to switch servo. You should try to set all servos as close to 90 degrees as possible.
+## Running the code
+To run all code of AQLARP, and thus start the robot, run the following command.
+```console
+$ ros2 launch aqlarp_main aqlarp.launch.py
+```
+To control the robot you will have to connect a ps4 controller, you can either do this by using a cable and connecting it using one of the USB ports, or alternatively you can use the instructions in the next part to connect the ps4 controller wirelessly.
+
+The controls for AQLARP are as follows:
+- Playstation button: start and stop the servos
+- Left Joystick: Make the robot strafe in the direction you specify
+- Right Joystick: Make the robot turn by moving the joystick left and right.
 ## Connecting ps4 controller wirelessly
 Open a command prompt on the raspberry pi (with ssh) and run the following command to get into the Bluetooth command line.
 ```console
@@ -20,4 +73,36 @@ Then finally to make it so the controller can connect to AQLARP by just pressing
 ```
 trust <MAC address>
 ```
-Now type `exit` to exit the Bluetooth terminal. If you want to disconnect the controller, hold down the PlayStation button for 10 seconds. To reconnect the controller, press the PlayStation button once.
+Now type `exit` to exit the Bluetooth terminal. If you want to disconnect the controller, hold down the PlayStation button for 10 seconds. To reconnect the controller, press the PlayStation button once.
+
+## Making the code run on startup
+If you want to make AQLARP's code run on startup so you can just connect the controller and use it, you will have to create a file called `aqlarp.service` at `/etc/systemd/system/`.
+To do this run the following command.
+```console
+$ sudo nano /etc/systemd/system/aqlarp.service
+```
+Then paste the following content into it.
+```toml
+[Unit]
+Description="AQLARP daemon"
+
+[Service]
+User=AQLARP
+Restart=always
+RestartSec=1
+
+Environment="LD_LIBRARY_PATH=/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_interfaces/lib:/opt/ros/iron/lib/aarch64-linux-gnu:/opt/ros/iron/lib"
+Environment="PYTHONPATH=/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_sensors/lib/python3.10/site-packages:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_motors/lib/python3.10/site-packages:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_main/lib/python3.10/site-packages:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_interfaces/lib/python3.10/site-packages:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_input/lib/python3.10/site-packages:/opt/ros/iron/lib/python3.10/site-packages"
+Environment="AMENT_PREFIX_PATH=/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_sensors:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_motors:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_main:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_interfaces:/home/AQLARP/AQLARP/ros2_ws/install/aqlarp_input:/opt/ros/iron"
+
+ExecStart=/opt/ros/iron/bin/ros2 launch aqlarp_main aqlarp.launch.py
+
+[Install]
+WantedBy=multi-user.target
+```
+If you use a different user then AQLARP or have cloned the GitHub repo in a different location, you will have to edit this file accordingly. To save the file press `Ctrl + O` and then press `Ctrl + X` to save.
+
+Then finally to enable the service run the following command.
+```console
+$ sudo systemctl enable --now aqlarp.service
+```

ros2_ws/src/aqlarp_main/aqlarp_main/aqlarp_main.py

@@ -14,56 +14,89 @@
 # - 3: Back Right,      Pin: 3-5
 joint_to_pin = [6, 7, 8, 9, 10, 11, 3, 4, 5, 0, 1, 2]
 
+# The class for the main node
 class MainNode(Node):
     def __init__(self):
+        # Initialize the node
         super().__init__('aqlarp_main')
-        # self.pitch_controller = PID(Kp = 0.2, Ki = 0.1, Kd = 0.0, setpoint = 0, output_limits = (-12, 12))
-        # self.roll_controller = PID(Kp = 0.2, Ki = 0.1, Kd = 0.0, setpoint = 0, output_limits = (-10, 10))
+
+        # Create a list with the supported giats, this is currentelly only the crawling giat
+        # But more can be added at a later data.
         self.giats = [CrawlingGiat()]
+
+        # Initialize some default variables
         self.enabled = False
         self.x_heading = self.z_heading = self.rotation_heading = 0
         self.gyro_pitch = self.gyro_roll  = 90.0
 
+        # Create the publisher used to set the joint angles
         self.angle_publisher = self.create_publisher(JointAngles, 'joint_angles', 10)
+        # Create the publisher to disable the servos
         self.disable_joints_publisher = self.create_publisher(Empty, 'disable_joints', 10)
+        # Create a listener for new gyro data
         self.gyro_listener = self.create_subscription(GyroAngles, 'gryo_angles', self.gyro_update, 10)
+        # Create a listener to disable or enable power
         self.power_listener = self.create_subscription(Bool, 'power', self.power_update, 10)
+        # Create a listener to get the heading of the robot
         self.heading_listener = self.create_subscription(Heading, 'heading', self.heading_update, 10)
 
-        # Run 100 times a second
+        # Run the main function 100 times per second
         self.timer = self.create_timer(0.01, self.update)
 
+    # Function that is called when new gyro values are recivied
     def gyro_update(self, msg):
+        # Save the new values
         self.gyro_pitch = msg.pitch
         self.gyro_roll = msg.roll
 
+    # Function that is called when the power state is changed
     def power_update(self, msg):
+        # Save the new power state
         self.enabled = msg.data
 
+    # Function that is called when the heading is changed
     def heading_update(self, msg):
+        # Limit the heading to a circle with a radius of 1 and save it
         self.x_heading, self.z_heading = project_to_circle(msg.x_heading, msg.z_heading)
+        # Limit the rotation between -1 and 1 and save it
         self.rotation_heading = clamp(msg.rotation_heading, -1, 1)
 
+    # Main function that updates all joint angles
     def update(self):
+        # If the robot isn't disable, send the message to disable the joints and return
         if not self.enabled:
             self.disable_joints_publisher.publish(Empty())
             return
 
+        # Get the leg positions from the current giat
         positions = self.giats[0].get_leg_positions(self.x_heading, self.z_heading, self.rotation_heading)
+
+        # Initialize a new empty joint angles object
         jointAngles = JointAngles()
+
+        # Loop over each leg
         for leg in range(4):
+            # Get the positions of the leg
             leg_positions = positions.legs[leg]
+            # Calculate the angles for the leg
             angles = calc_angles(leg, leg_positions[0], leg_positions[1], leg_positions[2], positions.pitch, positions.roll, positions.rotation)
+            # Add the angles to the joint angles object
             for joint in range(3):
                 jointAngles.angles[leg * 3 + joint] = angles[joint]
 
+        # Publish the new angles to the joint angles topic
         self.angle_publisher.publish(jointAngles)
 
+# Main function called on start
 def main(args = None):
+    # Initiliaze the ros library
     rclpy.init(args=args)
 
+    # Create a new instance of the main node
     node = MainNode()
+    # Log that the node has started
     node.get_logger().info('AQLARP core node started')
+    # Run the node
     rclpy.spin(node)
 
     # Cleanup

ros2_ws/src/aqlarp_main/aqlarp_main/src/crawling.py

@@ -2,26 +2,36 @@
 from .utils import project_to_circle, ease, clamp
 from .leg_positions import LegPositions
 
+# The class for the crawling giat
+# In this giat one leg is lifted at a time
 class CrawlingGiat():
     def __init__(self):
+        # Initialize the x value
         self.x = 0
 
     def get_leg_positions(self, moving_x, moving_z, rotation_heading):
+        # Calculate the speed of the robot
+        # This is the maximum of the speed in the x and z direction and the rotation speed
         speed = max(sqrt(moving_x * moving_x + moving_z * moving_z), abs(rotation_heading))
+        # Get the heading by projecting the x and z values to a circle
         heading_x, heading_z = project_to_circle(moving_x, moving_z, False)
 
+        # Intiliaze an empty leg positions value
         positions = LegPositions()
 
+        # If the robot isn't moving, set all legs to the default position and return
         if speed == 0:
             for leg in range(4):
                 positions.legs[leg] = [0, 15, 0]
             return positions
 
+        # Update the x value based on the speed
         self.x += 0.03 * speed
         if self.x >= 4:
             self.x = 0
-
+        # Loop over each leg
         for leg in range(4):
+            # Calculate the lifted leg
             # 0-1: Front left lifted
             if self.x < 1:
                 lifted_leg = 0
@@ -34,7 +44,9 @@ def get_leg_positions(self, moving_x, moving_z, rotation_heading):
             # 3-4: Back left lifted
             else:
                 lifted_leg = 2
-
+
+            # Adjust the x value based on the current leg
+            # The leg should always be lifted if this adjust value is between 0 and 1
             adjusted_x = self.x
             if leg == 1:
                 adjusted_x -= 2
@@ -45,34 +57,46 @@ def get_leg_positions(self, moving_x, moving_z, rotation_heading):
             if adjusted_x < 0:
                 adjusted_x += 4
 
+            # Calculate the leg start and end position
             leg_start_x = 2 * abs(heading_x)
             leg_end_x = -4 * abs(heading_x)
+            # If the heading is negative, swap the start and end position
             if heading_x < 0:
                 leg_start_x, leg_end_x = leg_end_x, leg_start_x
+
+            # Calculate the leg start and end position for the z direction
             leg_start_z = 2 * abs(heading_z)
             leg_end_z = -2 * abs(heading_z)
+            # If the heading is negative, swap the start and end position
             if heading_z < 0:
                 leg_start_z, leg_end_z = leg_end_z, leg_start_z
 
+            # Apply the rotation
+            # The direction of this is different for the front and back legs
             if leg < 2:
                 leg_start_z += 2 * rotation_heading
                 leg_end_z -= 2 * rotation_heading
             else:
                 leg_start_z -= 2 * rotation_heading
                 leg_end_z += 2 * rotation_heading
+            # Limit the z values to -2 and 2
             leg_start_z = clamp(leg_start_z, -2, 2)
             leg_end_z = clamp(leg_end_z, -2, 2)
 
-
-            x_offset = leg_start_x + (leg_end_x - leg_start_x) * ease((adjusted_x - 1.0) / 3.0)
+            # Calculate the offset for the legs
+            x_offset = leg_start_x + (leg_end_x - leg_start_x) * ((adjusted_x - 1.0) / 3.0)
             y_offset = 15
-            z_offset = leg_start_z + (leg_end_z - leg_start_z) * ease((adjusted_x - 1.0) / 3.0)
+            z_offset = leg_start_z + (leg_end_z - leg_start_z) * ((adjusted_x - 1.0) / 3.0)
 
+            # If the current leg is the lifted leg, adjust the x, y and z values
+            # So they move in the oppisite direction of the movement
             if leg == lifted_leg:
                 x_offset = leg_end_x + (leg_start_x - leg_end_x) * ease(adjusted_x)
                 y_offset -= 5 * ease(adjusted_x * 2)
                 z_offset = leg_end_z + (leg_start_z - leg_end_z) * ease(adjusted_x)
 
+            # Calculate the smoothing value, this is used to make the robot move to the side
+            # To help with balance
             raised_leg_x = self.x - floor(self.x)
             ease_point = 0.2
             if raised_leg_x < ease_point:
@@ -82,9 +106,12 @@ def get_leg_positions(self, moving_x, moving_z, rotation_heading):
             else:
                 smoothing = 1
 
+            # make the robot move to the side to help with balance
             x_offset += (-1.3 if lifted_leg < 2 else 1.3) * smoothing
             z_offset += (-1.3 if lifted_leg == 1 or lifted_leg == 2 else 1.3) * smoothing
 
+            # Set the leg position
             positions.legs[leg] = [x_offset, y_offset, z_offset]
 
+        # Return the leg positions
         return positions

ros2_ws/src/aqlarp_main/aqlarp_main/src/leg_positions.py

@@ -1,5 +1,6 @@
 default = [0], [15], [0]
 
+# Class to store the positions for each of the legs, and the transformations of the body
 class LegPositions:
     def __init__(self, legs = [default] * 4, pitch = 0, roll = 0, rotation = 0):
         self.legs = legs