Week 3 Ashville (#174)

* diffrent pid slots in align

* practice field fri night

* add align to amp angle

* more teleop bindings

* un-override on preempt

* remove override as function param

* drive and score teleop bindings

* pre qual 1

* remove candle controller

* end of day 1 before drive object auto

---------

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

zebROS_ws/src/behaviors/config/2024_align_speaker_config.yaml

@@ -1,3 +1,3 @@
 tolerance: 0.05
 velocity_tolerance: 0.5
-min_samples: 5
+min_samples: 50 # 0.2 seconds 

zebROS_ws/src/behaviors/config/2024_align_to_trap.yaml

@@ -2,9 +2,15 @@ x_tolerance: 0.03
 y_tolerance: 0.03
 angle_tolerance: 0.03
 
-x_offset: 0.65
+amp_x_tolerance: 0.2
+amp_y_tolerance: 0.05
+amp_angle_tolerance: 0.05
+
+
+x_offset: 0.85
 y_offset: 0.0
 
+# CHECK THESE OFFSETS
 x_offset_amp: 0.0
 y_offset_amp: 0.0
 

zebROS_ws/src/behaviors/config/2024_clawster_config.yaml

@@ -6,7 +6,7 @@ claw:
   switch_name: "claw_limit_switch"
 
 preshooter:
-  intake_speed: 0.90
+  intake_speed: 0.85
   outtake_speed: 0.9
   switch_name: "preshooter_limit_switch"
 

zebROS_ws/src/behaviors/config/2024_climbing_config.yaml

@@ -5,10 +5,10 @@ trap_retract_speed: 20.0
 fast_retract_acceleration: 40.0
 trap_retract_acceleration: 40.0
 extend_height: 55.0
-trap_height: 10.0
-fast_height: 10.0
+trap_height: 15.0
+fast_height: 15.0
 joint_name: climb_leader
 loop_rate: 50.0
 tolerance: 10.0
-pivot_angle: 1.9
-min_pivot_angle: 1.7
+pivot_angle: 1.7
+min_pivot_angle: 1.65

zebROS_ws/src/behaviors/config/2024_rumble_server_config.yaml

@@ -4,4 +4,4 @@ preshooter_limit_switch_name: preshooter_limit_switch
 intake_limit_switch_name: diverter_limit_switch
 claw_limit_switch_name: claw_limit_switch
 
-rumble_on_note: 30000 # out of 65535
+rumble_on_note: 65534 # out of 65535

zebROS_ws/src/behaviors/config/2024_shooting_config.yaml

@@ -22,6 +22,8 @@ amp_top_right_speed: 60.0 # rad/s
 amp_bottom_left_speed: 100.0 # rad/s
 amp_bottom_right_speed: 100.0 # rad/s
 amp_pivot_position: 1.0 # rad
+amp_drive_back_time: 0.5 # sec
+amp_drive_back_speed: 2.0 # m/s
 
 # trap 
 trap_top_left_speed: 325.0 # rad/s

zebROS_ws/src/behaviors/config/auto_mode_config.yaml

@@ -49,7 +49,7 @@ auto_mode_6: ["shoot_center_subwoofer", "start_intake", "drive_1_5_m", "shoot_fr
 auto_mode_7: ["shoot_center_subwoofer", "pause_two", "pause_two", "spin_down_shooter"]
 
 # 8: shoot preloaded note, drive back TESTED AND WORKS
-auto_mode_8: ["shoot_center_subwoofer", "drive_2_5_m"]
+auto_mode_8: ["shoot_center_subwoofer", "drive_2_5_m", "spin_down_shooter"]
 
 # 9: no-op
 auto_mode_9: ["pause_quarter"]

zebROS_ws/src/behaviors/launch/2024_align_to_trap.launch

@@ -73,6 +73,56 @@
             <param name="orient_state_topic" value="/teleop/orient_strafing/state"/>
             <param name="transform_yaw" value="true"/>
         </node>
+
+        <!-- ALIGN TO AMP -->
+
+        <node name="x_position_pid_amp" pkg="pid" type="controller" >
+            <param name="node_name" value="x_position_pid_amp" />
+            <param name="Kp" value="2.0" />
+            <param name="Ki" value="0.0" />
+            <param name="Kd" value="0.0" />
+            <param name="upper_limit" value="2.0" />
+            <param name="lower_limit" value="-2.0" />
+            <param name="windup_limit" value="10" />
+            <param name="max_loop_frequency" value="250.0" />
+            <param name="min_loop_frequency" value="250.0" />
+            <param name="topic_from_controller" value="x_position_pid_amp/x_command" />
+            <param name="setpoint_topic" value="x_position_pid_amp/x_cmd_pub" />
+            <param name="topic_from_plant" value="x_position_pid_amp/x_state_pub" />
+            <remap from="pid_debug" to="x_position_pid_amp/pid_debug" />
+            <remap from="pid_enable" to="x_position_pid_amp/pid_enable" />
+        </node>
+
+        <node name="y_position_pid_amp" pkg="pid" type="controller" >
+            <param name="node_name" value="y_position_pid_amp" />
+            <param name="Kp" value="2.0" />
+            <param name="Ki" value="0.0" />
+            <param name="Kd" value="0.0" />
+            <param name="upper_limit" value="2.0" />
+            <param name="lower_limit" value="-2.0" />
+            <param name="windup_limit" value="10" />
+            <param name="max_loop_frequency" value="250.0" />
+            <param name="min_loop_frequency" value="250.0" />
+            <param name="topic_from_controller" value="y_position_pid_amp/y_command" />
+            <param name="setpoint_topic" value="y_position_pid_amp/y_cmd_pub" />
+            <param name="topic_from_plant" value="y_position_pid_amp/y_state_pub" />
+            <remap from="pid_debug" to="y_position_pid_amp/pid_debug" />
+            <remap from="pid_enable" to="y_position_pid_amp/pid_enable" />
+        </node>
+
+        <node name="align_to_amp_publish_pid_cmd_vel_node" pkg="behaviors" type="publish_pid_cmd_vel_node" output="screen">
+            <param name="orient_topic" value="/teleop/orient_strafing/control_effort" />
+            <param name="x_topic" value="x_position_pid_amp/x_command" />
+            <param name="y_topic" value="y_position_pid_amp/y_command" />
+            <param name="command_timeout" value="0.5" />
+            <param name="enable_topic" value="align_to_amp_pid/pid_enable" />
+            <param name="name" value="align_to_trap_pid" />
+            <param name="orient_state_topic" value="/teleop/orient_strafing/state"/>
+            <param name="transform_yaw" value="true"/>
+        </node>
+        <!-- ALIGN TO AMP END -->
+
+
 
     </group>
 

zebROS_ws/src/behaviors/launch/2024_drive_and_score.launch

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <group ns="align_and_shoot">
+    <group ns="drive_and_score">
         <!-- <rosparam command="load" file="$(find behaviors)/config/2024_align_and_shoot_config.yaml"/> -->
         <node name="drive_and_score_2024" pkg="behaviors" type="2024_drive_and_score.py" output="screen"/>
     </group>

zebROS_ws/src/behaviors/src/2024_align_to_speaker_server.py

@@ -22,6 +22,7 @@ class Aligner:
     _result = behavior_actions.msg.AlignToSpeaker2024Result()
 
     def __init__(self, name):   
+        self.msg = None
         self.valid_samples = 0
         self._action_name = name
         self.tolerance = rospy.get_param("tolerance")
@@ -45,7 +46,7 @@ def __init__(self, name):
         self.pub_dist_and_ang_vel = rospy.Publisher("/speaker_align/dist_and_ang", behavior_actions.msg.AutoAlignSpeaker, queue_size=1) #distance and angle
 
         self._feedback.aligned = False
-
+        
         self.feed_forward = True
 
     def imu_callback(self, imu_msg):
@@ -63,18 +64,22 @@ def imu_callback(self, imu_msg):
             trans = self.tfBuffer.lookup_transform('base_link', offset_point.header.frame_id, rospy.Time())
             destination = tf2_geometry_msgs.do_transform_point(offset_point, trans)
 
-            msg = std_msgs.msg.Float64()
+            self.msg = std_msgs.msg.Float64()
             dist_ang_msg = behavior_actions.msg.AutoAlignSpeaker()
 
             dist_ang_msg.distance = math.sqrt(destination.point.x ** 2 + destination.point.y ** 2)
-            msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x)
+            self.msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x)
             dist_ang_msg.angle = math.atan2(destination.point.y, destination.point.x)
 
             self.pub_dist_and_ang_vel.publish(dist_ang_msg)
         except Exception as e:
             rospy.logwarn_throttle(1, f"align_to_speaker: can't publish distance {e}")
 
-        if self._feedback.error < self.tolerance and abs(self.current_orient_effort) < self.velocity_tolerance:
+        if self.msg is not None:
+            self._feedback.error = abs(angles.shortest_angular_distance(self.msg.data, self.current_yaw))
+
+        #rospy.loginfo(f"errr {self._feedback.error} tolerance {self.tolerance}")
+        if self._feedback.error < self.tolerance: # and abs(self.current_orient_effort) < self.velocity_tolerance
             self.valid_samples += 1
         else:
             self.valid_samples = 0
@@ -117,29 +122,30 @@ def aligner_callback(self, goal: behavior_actions.msg.AlignToSpeaker2024Goal):
                 rate.sleep()
                 continue
 
-            msg = std_msgs.msg.Float64()
+            self.msg = std_msgs.msg.Float64()
             dist_ang_msg = behavior_actions.msg.AutoAlignSpeaker()
 
             dist_ang_msg.distance = math.sqrt(destination.point.x ** 2 + destination.point.y ** 2)
-            msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x)
+            self.msg.data = math.pi + self.current_yaw + math.atan2(destination.point.y, destination.point.x)
             dist_ang_msg.angle = math.atan2(destination.point.y, destination.point.x)
 
-            self._feedback.error = abs(angles.shortest_angular_distance(msg.data, self.current_yaw))
             self._feedback.aligned = False
 
-            self.object_publish.publish(msg) 
+            self.object_publish.publish(self.msg) 
             self.pub_dist_and_ang_vel.publish(dist_ang_msg)
 
             cmd_vel_msg = geometry_msgs.msg.Twist()
             cmd_vel_msg.angular.x = 0
             cmd_vel_msg.angular.y = 0
-            cmd_vel_msg.angular.z = self.current_orient_effort + 1.0 * numpy.sign(self.current_orient_effort) * int(self.feed_forward)
+            cmd_vel_msg.angular.z = self.current_orient_effort 
+            if abs(self._feedback.error) > 0.1: 
+                cmd_vel_msg.angular.z += 1.0 * numpy.sign(self.current_orient_effort) * int(self.feed_forward)
             cmd_vel_msg.linear.x = 0
             cmd_vel_msg.linear.y = 0
             cmd_vel_msg.linear.z = 0
             self.pub_cmd_vel.publish(cmd_vel_msg)
 
-            rospy.loginfo_throttle(0.5, f"Align to speaker {abs(angles.shortest_angular_distance(msg.data, self.current_yaw))}")
+            rospy.loginfo_throttle(0.5, f"Align to speaker {abs(angles.shortest_angular_distance(self.msg.data, self.current_yaw))}")
 
             if self.valid_samples >= self.min_samples:
                 self._feedback.aligned = True

zebROS_ws/src/behaviors/src/2024_align_to_trap.py

@@ -36,9 +36,13 @@ class Aligner:
 
     def __init__(self, name):   
         self._action_name = name
-        self.x_tolerance = rospy.get_param("x_tolerance")
-        self.y_tolerance = rospy.get_param("y_tolerance")
-        self.angle_tolerance = rospy.get_param("angle_tolerance")
+        self.trap_x_tolerance = rospy.get_param("x_tolerance")
+        self.trap_y_tolerance = rospy.get_param("y_tolerance")
+        self.trap_angle_tolerance = rospy.get_param("angle_tolerance")
+
+        self.amp_x_tolerance = rospy.get_param("amp_x_tolerance")
+        self.amp_y_tolerance = rospy.get_param("amp_y_tolerance")
+        self.amp_angle_tolerance = rospy.get_param("amp_angle_tolerance")
 
         self.x_offset = rospy.get_param("x_offset")
         self.y_offset = rospy.get_param("y_offset")
@@ -59,21 +63,36 @@ def __init__(self, name):
         self.orientation_command_pub = rospy.Publisher("/teleop/orientation_command", std_msgs.msg.Float64, queue_size=1)
         self.object_subscribe = rospy.Subscriber("/imu/zeroed_imu", sensor_msgs.msg.Imu, self.imu_callback)
 
-        self.x_state_pub = rospy.Publisher("x_position_pid/x_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
-        self.x_cmd_pub = rospy.Publisher("x_position_pid/x_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
-        self.x_enable_pub = rospy.Publisher("x_position_pid/x_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
-        self.x_command = 0
+        self.trap_x_state_pub = rospy.Publisher("x_position_pid/x_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.trap_x_cmd_pub = rospy.Publisher("x_position_pid/x_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.trap_x_enable_pub = rospy.Publisher("x_position_pid/x_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+        self.trap_x_command = 0
+        # self.x_command_sub = rospy.Publisher("x_position_pid/x_command", std_msgs.msg.Float64, self.x_command_callback, tcp_nodelay=True)
+
+        self.trap_y_state_pub = rospy.Publisher("y_position_pid/y_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.trap_y_cmd_pub = rospy.Publisher("y_position_pid/y_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.trap_y_enable_pub = rospy.Publisher("y_position_pid/y_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+        self.trap_y_command = 0
+
+
+
+        self.amp_x_state_pub = rospy.Publisher("x_position_pid_amp/x_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.amp_x_cmd_pub = rospy.Publisher("x_position_pid_amp/x_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.amp_x_enable_pub = rospy.Publisher("x_position_pid_amp/x_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+        self.amp_x_command = 0
         # self.x_command_sub = rospy.Publisher("x_position_pid/x_command", std_msgs.msg.Float64, self.x_command_callback, tcp_nodelay=True)
 
-        self.y_state_pub = rospy.Publisher("y_position_pid/y_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
-        self.y_cmd_pub = rospy.Publisher("y_position_pid/y_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
-        self.y_enable_pub = rospy.Publisher("y_position_pid/y_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
-        self.y_command = 0
+        self.amp_y_state_pub = rospy.Publisher("y_position_pid_amp/y_state_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.amp_y_cmd_pub = rospy.Publisher("y_position_pid_amp/y_cmd_pub", std_msgs.msg.Float64, queue_size=1, tcp_nodelay=True)
+        self.amp_y_enable_pub = rospy.Publisher("y_position_pid_amp/y_enable_pub", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+        self.amp_y_command = 0
         # self.y_command_sub = rospy.Publisher("y_position_pid/y_command", std_msgs.msg.Float64, self.y_command_callback, tcp_nodelay=True)
 
         self.team_subscribe = rospy.Subscriber("/frcrobot_rio/match_data", MatchSpecificData, self.match_data_callback)
 
-        self.enable_pub = rospy.Publisher("align_to_trap_pid/pid_enable", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+        self.trap_enable_pub = rospy.Publisher("align_to_trap_pid/pid_enable", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+        self.amp_enable_pub = rospy.Publisher("align_to_amp_pid/pid_enable", std_msgs.msg.Bool, queue_size=1, tcp_nodelay=True)
+
 
         self.sub_effort = rospy.Subscriber("/teleop/orient_strafing/control_effort", std_msgs.msg.Float64, self.robot_orientation_effort_callback)
 
@@ -105,9 +124,39 @@ def aligner_callback(self, goal: behavior_actions.msg.AlignToTrap2024Goal):
         if goal.destination == goal.AMP:
             rospy.loginfo("2024_align_to_trap: Aligning to amp")
             closest_frame = self.RED_AMP if self.color == MatchSpecificData.ALLIANCE_COLOR_RED else self.BLUE_AMP
+            self.x_enable_pub = self.amp_x_enable_pub
+            self.y_enable_pub = self.amp_y_enable_pub
+            self.enable_pub = self.amp_enable_pub
+            self.x_state_pub = self.amp_x_state_pub 
+            self.x_cmd_pub = self.amp_x_cmd_pub 
+            self.x_enable_pub = self.amp_x_enable_pub 
+            self.x_command = self.amp_x_command 
+            self.y_state_pub = self.amp_y_state_pub 
+            self.y_cmd_pub = self.amp_y_cmd_pub 
+            self.y_enable_pub = self.amp_y_enable_pub 
+            self.y_command = self.amp_y_command 
+            self.x_tolerance = self.amp_x_tolerance
+            self.y_tolerance = self.amp_y_tolerance
+            self.angle_tolerance = self.amp_angle_tolerance
+
         elif goal.destination == goal.SUBWOOFER:
             rospy.loginfo("2024_align_to_trap: Aligning to subwoofer")
             closest_frame = self.RED_SUBWOOFER if self.color == MatchSpecificData.ALLIANCE_COLOR_RED else self.BLUE_SUBWOOFER
+            self.x_enable_pub = self.amp_x_enable_pub
+            self.y_enable_pub = self.amp_y_enable_pub
+            self.enable_pub = self.amp_enable_pub
+            self.x_state_pub = self.amp_x_state_pub 
+            self.x_cmd_pub = self.amp_x_cmd_pub 
+            self.x_enable_pub = self.amp_x_enable_pub 
+            self.x_command = self.amp_x_command 
+            self.y_state_pub = self.amp_y_state_pub 
+            self.y_cmd_pub = self.amp_y_cmd_pub 
+            self.y_enable_pub = self.amp_y_enable_pub 
+            self.y_command = self.amp_y_command 
+            self.x_tolerance = self.amp_x_tolerance
+            self.y_tolerance = self.amp_y_tolerance
+            self.angle_tolerance = self.amp_angle_tolerance
+
         else:
             rospy.loginfo("2024_align_to_trap: Aligning to trap")
             for frame in (self.RED_TAGS if self.color == MatchSpecificData.ALLIANCE_COLOR_RED else self.BLUE_TAGS):
@@ -116,6 +165,21 @@ def aligner_callback(self, goal: behavior_actions.msg.AlignToTrap2024Goal):
                 if distance < closest_distance:
                     closest_frame = frame
                     closest_distance = distance
+            self.x_enable_pub = self.trap_x_enable_pub
+            self.y_enable_pub = self.trap_y_enable_pub
+            self.enable_pub = self.trap_enable_pub
+            self.x_state_pub = self.trap_x_state_pub 
+            self.x_cmd_pub = self.trap_x_cmd_pub 
+            self.x_enable_pub = self.trap_x_enable_pub 
+            self.x_command = self.trap_x_command 
+            self.y_state_pub = self.trap_y_state_pub 
+            self.y_cmd_pub = self.trap_y_cmd_pub 
+            self.y_enable_pub = self.trap_y_enable_pub 
+            self.y_command = self.trap_y_command 
+            self.x_tolerance = self.trap_x_tolerance
+            self.y_tolerance = self.trap_y_tolerance
+            self.angle_tolerance = self.trap_angle_tolerance
+
 
         trans = self.tfBuffer.lookup_transform(closest_frame, "map", rospy.Time())
         if goal.destination == goal.TRAP: