From 918c5211a55c2999f88f2da2fb82b48bd86cd365 Mon Sep 17 00:00:00 2001 From: DeDiamondPro <67508414+DeDiamondPro@users.noreply.github.com> Date: Wed, 20 Mar 2024 09:56:48 +0100 Subject: [PATCH] progress --- aqlarp.service | 16 ---- .../src/ch05-01-installing-software.md | 87 ++++++++++++++++++- .../aqlarp_main/aqlarp_main/aqlarp_main.py | 39 ++++++++- .../aqlarp_main/aqlarp_main/src/crawling.py | 37 ++++++-- .../aqlarp_main/src/leg_positions.py | 1 + 5 files changed, 155 insertions(+), 25 deletions(-) delete mode 100644 aqlarp.service diff --git a/aqlarp.service b/aqlarp.service deleted file mode 100644 index 76130a3..0000000 --- a/aqlarp.service +++ /dev/null @@ -1,16 +0,0 @@ -[Unit] -Description="AQLARP daemon" - -[Service] -User=AQLARP -Restart=always -RestartSec=1 - -Environment="LD_LIBRARY_PATH=/home/AQLARP/new_ws/install/aqlarp_interfaces/lib:/opt/ros/iron/lib/aarch64-linux-gnu:/opt/ros/iron/lib" -Environment="PYTHONPATH=/home/AQLARP/new_ws/install/aqlarp_sensors/lib/python3.10/site-packages:/home/AQLARP/new_ws/install/aqlarp_motors/lib/python3.10/site-packages:/home/AQLARP/new_ws/install/aqlarp_main/lib/python3.10/site-packages:/home/AQLARP/new_ws/install/aqlarp_interfaces/lib/python3.10/site-packages:/home/AQLARP/new_ws/install/aqlarp_input/lib/python3.10/site-packages:/opt/ros/iron/lib/python3.10/site-packages" -Environment="AMENT_PREFIX_PATH=/home/AQLARP/new_ws/install/aqlarp_sensors:/home/AQLARP/new_ws/install/aqlarp_motors:/home/AQLARP/new_ws/install/aqlarp_main:/home/AQLARP/new_ws/install/aqlarp_interfaces:/home/AQLARP/new_ws/install/aqlarp_input:/opt/ros/iron" - -ExecStart=/opt/ros/iron/bin/ros2 launch aqlarp_main aqlarp.launch.py - -[Install] -WantedBy=multi-user.target diff --git a/documentation/src/ch05-01-installing-software.md b/documentation/src/ch05-01-installing-software.md index ed31a58..f987bf2 100644 --- a/documentation/src/ch05-01-installing-software.md +++ b/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 ``` -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. \ No newline at end of file +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 +``` \ No newline at end of file diff --git a/ros2_ws/src/aqlarp_main/aqlarp_main/aqlarp_main.py b/ros2_ws/src/aqlarp_main/aqlarp_main/aqlarp_main.py index 2ae7146..e2af6f9 100644 --- a/ros2_ws/src/aqlarp_main/aqlarp_main/aqlarp_main.py +++ b/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 diff --git a/ros2_ws/src/aqlarp_main/aqlarp_main/src/crawling.py b/ros2_ws/src/aqlarp_main/aqlarp_main/src/crawling.py index 75db8e5..a28c0e3 100644 --- a/ros2_ws/src/aqlarp_main/aqlarp_main/src/crawling.py +++ b/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 diff --git a/ros2_ws/src/aqlarp_main/aqlarp_main/src/leg_positions.py b/ros2_ws/src/aqlarp_main/aqlarp_main/src/leg_positions.py index 6ef4bd3..c3f07c8 100644 --- a/ros2_ws/src/aqlarp_main/aqlarp_main/src/leg_positions.py +++ b/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