final testing changes (#242)

Co-authored-by: umnrobotics <[email protected]>

config/drivetrain_config.yaml

@@ -3,9 +3,9 @@
     HALF_WHEEL_BASE: 0.55959375 # HALF of the wheelbase in meters
     HALF_TRACK_WIDTH: 0.2746375 # HALF of the trackwidth in meters
     STEERING_MOTOR_GEAR_RATIO: 40 # Gear ratio of the steering motor (40:1)
-    FRONT_LEFT_MAGNET_OFFSET: 828 # (in encoder counts)
-    FRONT_RIGHT_MAGNET_OFFSET: 355 # (in encoder counts)
-    BACK_LEFT_MAGNET_OFFSET: 332 # (in encoder counts)
-    BACK_RIGHT_MAGNET_OFFSET: 388 # (in encoder counts)
+    FRONT_LEFT_MAGNET_OFFSET: 823 # (in encoder counts)
+    FRONT_RIGHT_MAGNET_OFFSET: 378 # (in encoder counts)
+    BACK_LEFT_MAGNET_OFFSET: 906 # (in encoder counts)
+    BACK_RIGHT_MAGNET_OFFSET: 392 # (in encoder counts)
     ABSOLUTE_ENCODER_COUNTS: 1024 # (in encoder counts)
     GAZEBO_SIMULATION: False # Set to true if running in Gazebo

config/nav2_isaac_sim.yaml

@@ -93,12 +93,12 @@ controller_server:
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
       required_movement_radius: 0.5
-      movement_time_allowance: 20.0
+      movement_time_allowance: 50.0
     # Goal checker parameters
     general_goal_checker:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 3.14
+      yaw_goal_tolerance: 0.25
       stateful: True
     # DWB parameters
     FollowPath:

config/nvblox/nvblox_base.yaml

@@ -44,8 +44,8 @@
     # Used for crater detection.
     # one is the floor, two is hole level
     # by going significantly above 'ground', we ignore all the noise from uneven terrain while still detecting obstacles.
-    esdf_above_ground_slice_height: 0.0
-    esdf_underground_detection: -0.8
+    esdf_above_ground_slice_height: 0.4  # TODO: This might need to be adjusted but it works for large obstacles
+    esdf_underground_detection: -2.0  # TODO: This NEEDS to be adjusted
     combine_pointcloud: True
     combine_costmap: True
     invert_below_ground: False

config/rovr_control.yaml

@@ -7,4 +7,4 @@
     skimmer_lift_manual_power: 0.05 # Measured in Duty Cycle (0.0-1.0)
     autonomous_field_type: "top" # The type of field ("top", "bottom", "nasa")
     lift_dumping_position: -1000 # Measured in encoder counts!
-    lift_digging_position: -3050 # Measured in encoder counts!
+    lift_digging_position: -3200 # Measured in encoder counts!

src/drivetrain/drivetrain/drivetrain_node.py

@@ -86,10 +86,10 @@ def __init__(self):
         self.declare_parameter("HALF_WHEEL_BASE", 0.5)
         self.declare_parameter("HALF_TRACK_WIDTH", 0.5)
         self.declare_parameter("STEERING_MOTOR_GEAR_RATIO", 40)
-        self.declare_parameter("FRONT_LEFT_MAGNET_OFFSET", 828)
-        self.declare_parameter("FRONT_RIGHT_MAGNET_OFFSET", 355)
-        self.declare_parameter("BACK_LEFT_MAGNET_OFFSET", 332)
-        self.declare_parameter("BACK_RIGHT_MAGNET_OFFSET", 388)
+        self.declare_parameter("FRONT_LEFT_MAGNET_OFFSET", 823)
+        self.declare_parameter("FRONT_RIGHT_MAGNET_OFFSET", 378)
+        self.declare_parameter("BACK_LEFT_MAGNET_OFFSET", 906)
+        self.declare_parameter("BACK_RIGHT_MAGNET_OFFSET", 392)
         self.declare_parameter("ABSOLUTE_ENCODER_COUNTS", 1024)
         self.declare_parameter("GAZEBO_SIMULATION", False)
 

src/motor_control/src/motor_control_node.cpp

@@ -253,11 +253,11 @@ class MotorControlNode : public rclcpp::Node {
         "motor/get", std::bind(&MotorControlNode::get_callback, this, _1, _2));
 
     // Instantiate all of our PIDControllers here
-    this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()] = new PIDController(42, 0.002, 0.0, 0.0, 0.0, 20, 0.5);
-    this->pid_controllers[this->get_parameter("FRONT_LEFT_TURN").as_int()] = new PIDController(42, 0.002, 0.0, 0.0, 0.0, 20, 0.5);
-    this->pid_controllers[this->get_parameter("BACK_RIGHT_TURN").as_int()] = new PIDController(42, 0.002, 0.0, 0.0, 0.0, 20, 0.5);
-    this->pid_controllers[this->get_parameter("FRONT_RIGHT_TURN").as_int()] = new PIDController(42, 0.002, 0.0, 0.0, 0.0, 20, 0.5);
-    this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.002, 0.0, 0.0, 0.0, 20, 0.05);
+    this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5);
+    this->pid_controllers[this->get_parameter("FRONT_LEFT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5);
+    this->pid_controllers[this->get_parameter("BACK_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5);
+    this->pid_controllers[this->get_parameter("FRONT_RIGHT_TURN").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 10, 0.5);
+    this->pid_controllers[this->get_parameter("SKIMMER_LIFT_MOTOR").as_int()] = new PIDController(42, 0.005, 0.0, 0.0, 0.0, 20, 0.05);
 
     // Enable continuous input for the swerve module PID controllers
     this->pid_controllers[this->get_parameter("BACK_LEFT_TURN").as_int()]->enableContinuousInput(0, 360 * this->get_parameter("STEERING_MOTOR_GEAR_RATIO").as_int());

src/rovr_control/rovr_control/main_control_node.py

@@ -47,7 +47,7 @@ def __init__(self) -> None:
         self.declare_parameter("skimmer_belt_power", -0.3)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("skimmer_lift_manual_power", 0.05)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("lift_dumping_position", -1000)  # Measured in encoder counts
-        self.declare_parameter("lift_digging_position", -3050)  # Measured in encoder counts
+        self.declare_parameter("lift_digging_position", -3200)  # Measured in encoder counts
 
         # Assign the ROS Parameters to member variables below #
         self.autonomous_driving_power = self.get_parameter("autonomous_driving_power").value
@@ -129,20 +129,24 @@ def end_autonomous(self) -> None:
         self.stop_all_subsystems()  # Stop all subsystems
         self.state = states["Teleop"]  # Return to Teleop mode
 
+    # This autonomous routine has been tested and works!
     async def calibrate_field_coordinates(self) -> None:
         """This method rotates until we can see apriltag(s) and then sets the map -> odom tf."""
         if not self.field_calibrated:
             self.get_logger().info("Beginning search for apriltags")
             while not self.cli_drivetrain_drive.wait_for_service():  # Wait for the drivetrain services to be available
                 self.get_logger().warn("Waiting for drivetrain services to become available...")
                 await asyncio.sleep(0.1)
-            await self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=0.0, horizontal_power=0.0, turning_power=0.15))
+            await self.cli_drivetrain_drive.call_async(Drive.Request(forward_power=0.0, horizontal_power=0.0, turning_power=0.3))
         while not self.field_calibrated:
             future = self.cli_set_apriltag_odometry.call_async(ResetOdom.Request())
             future.add_done_callback(self.future_odom_callback)
             await asyncio.sleep(0.05)  # Allows other async tasks to continue running (this is non-blocking)
         self.get_logger().info("Field Coordinates Calibrated!")
         await self.cli_drivetrain_stop.call_async(Stop.Request())
+        self.nav2.spin(3.14)  # Turn around 180 degrees to face the rest of the field
+        while not self.nav2.isTaskComplete():
+            await asyncio.sleep(0.1)  # Allows other async tasks to continue running (this is non-blocking)
         self.apriltag_timer.cancel()
         self.end_autonomous()  # Return to Teleop mode
 
@@ -151,19 +155,22 @@ async def auto_dig_procedure(self) -> None:
         """This method lays out the procedure for autonomously digging!"""
         self.get_logger().info("\nStarting Autonomous Digging Procedure!")
         try:  # Wrap the autonomous procedure in a try-except
-            await self.cli_skimmer_setPower.call_async(SetPower.Request(power=self.skimmer_belt_power))
             await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_digging_position))  # Lower the skimmer into the ground
+            self.skimmer_goal_reached = False
             # Wait for the goal height to be reached
             while not self.skimmer_goal_reached:
                 self.get_logger().info("Moving skimmer to the goal")
                 await asyncio.sleep(0.1)  # Allows other async tasks to continue running (this is non-blocking)
+            await self.cli_skimmer_setPower.call_async(SetPower.Request(power=self.skimmer_belt_power))
             # Drive forward while digging
             self.nav2.driveOnHeading(dist=0.75, speed=0.1)  # TODO: Tune these values
             while not self.nav2.isTaskComplete():  # Wait for the end of the driveOnHeading task
+                self.get_logger().info("Auto Driving")
                 await asyncio.sleep(0.1)  # Allows other async tasks to continue running (this is non-blocking)
             await self.cli_drivetrain_stop.call_async(Stop.Request())
             await self.cli_skimmer_stop.call_async(Stop.Request())
             await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_dumping_position))  # Raise the skimmer back up
+            self.skimmer_goal_reached = False
             # Wait for the lift goal to be reached
             while not self.skimmer_goal_reached:
                 self.get_logger().info("Moving skimmer to the goal")
@@ -174,19 +181,20 @@ async def auto_dig_procedure(self) -> None:
             self.get_logger().warn("Autonomous Digging Procedure Terminated\n")
             self.end_autonomous()  # Return to Teleop mode
 
-    # TODO: This autonomous routine has not been tested yet!
+    # This autonomous routine has been tested and works!
     async def auto_offload_procedure(self) -> None:
         """This method lays out the procedure for autonomously offloading!"""
         self.get_logger().info("\nStarting Autonomous Offload Procedure!")
         try:  # Wrap the autonomous procedure in a try-except
             await self.cli_lift_setPosition.call_async(SetPosition.Request(position=self.lift_dumping_position))  # Raise up the skimmer in preparation for dumping
+            self.skimmer_goal_reached = False
             # Wait for the lift goal to be reached
             while not self.skimmer_goal_reached:
                 self.get_logger().info("Moving skimmer to the goal")
                 await asyncio.sleep(0.1)  # Allows other async tasks to continue running (this is non-blocking)
             self.get_logger().info("Commence Offloading!")
             await self.cli_skimmer_setPower.call_async(SetPower.Request(power=self.skimmer_belt_power))
-            await asyncio.sleep(5 / abs(self.skimmer_belt_power))  # How long to offload for # TODO: Adjust this time factor as needed
+            await asyncio.sleep(8 / abs(self.skimmer_belt_power))  # How long to offload for # TODO: Adjust this time factor as needed
             await self.cli_skimmer_stop.call_async(Stop.Request())  # Stop the skimmer belt
             self.get_logger().info("Autonomous Offload Procedure Complete!\n")
             self.end_autonomous()  # Return to Teleop mode