Push motor limit switch

arduino/sensors/sensors.ino

@@ -1,14 +1,14 @@
 // Define a struct to hold the sensor data
 struct SensorData {
-  int leftMotorPotentiometer;
-  int rightMotorPotentiometer;
-  bool bottomLimitSwitch;
+  int tiltPotentiometer;
+  bool dumpLimitSwitch;
+  bool extensionLimitSwitch;
 };
 
 // Define the sensor pins here
-#define LEFT_MOTOR_POT_PIN A0
-#define RIGHT_MOTOR_POT_PIN A1
-#define RELAY_PIN 7
+#define TILT_POTENTIOMETER A0
+#define DUMP_LIMIT_SWITCH A1
+#define EXTENSION_LIMIT_SWITCH 7
 
 void setup() {
   // Initialize serial communication
@@ -26,10 +26,9 @@ void loop() {
   SensorData data;
 
   // Read from the analog inputs (potentiometers)
-  data.leftMotorPotentiometer = analogRead(LEFT_MOTOR_POT_PIN);  // Read left motor potentiometer value
-  data.rightMotorPotentiometer = analogRead(RIGHT_MOTOR_POT_PIN); // Read right motor potentiometer value
-
-  data.bottomLimitSwitch  = analogRead(bottom_limit_switch); //bottom limit switch value
+  data.tiltPotentiometer = analogRead(TILT_POTENTIOMETER);  // Read left motor potentiometer value
+  data.dumpLimitSwitch = digitalRead(DUMP_LIMIT_SWITCH);
+  data.extensionLimitSwitch  = digitalRead(EXTENSION_LIMIT_SWITCH);
 
   // Send the struct over the serial bus to the Nvidia Jetson
   Serial.write((byte *)&data, sizeof(SensorData));

config/auger_config.yaml

@@ -9,6 +9,8 @@
     MAX_EXTEND_PUSH_MOTOR_VELOCITY: 600 # in RPM
     push_motor_position: 0
     tilt_actuator_position: 0
+    extension_limit_switch: true
+    STOWED: true
     TILT_ACTUATOR_CURRENT_THRESHOLD: 0
     MAX_PUSH_MOTOR_POSITION: 0
     MIN_PUSH_MOTOR_POSITION: 0

config/motor_control.yaml

@@ -16,7 +16,7 @@
     DIGGER_ACTUATORS_kP_coupling: 0.004 # kP for digger actuator position syncing
     DIGGER_PITCH_kP: 2.5 # kP for tipping while digging
     TIPPING_SPEED_ADJUSTMENT: false # determines if pitch speed adjustment is used
-    DIGGER_SAFETY_ZONE: 120 # Measured in potentiometer units (0 to 1023)
+    EXTENSION_SAFETY_ZONE: 0 # Measured in potentiometer units (0 to 1023)
     CURRENT_SPIKE_THRESHOLD: 1.8 # Threshold for current spike detection (measured in Amps)
     CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds)
     BUCKETS_CURRENT_SPIKE_THRESHOLD: 12.0 # Threshold for current spike detection (measured in Amps)

src/auger/auger/auger_node.py

@@ -20,6 +20,7 @@
 from rovr_interfaces.srv import SetExtension
 from rovr_interfaces.msg import Potentiometers
 from std_srvs.srv import Trigger
+from std_msgs.msg import Bool, Float32
 
 
 class Auger(Node):
@@ -37,6 +38,9 @@ def __init__(self):
         self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get")
 
         # Define parameters here
+        self.declare_parameter("AUGER_STOWED", True)
+        self.declare_parameter("DUMPER_STOWED", True)
+        self.declare_parameter("extension_limit_switch", True)
         self.declare_parameter("POWER_LIMIT", 1)
         self.declare_parameter("SCREW_SPEED", 4000)
         self.declare_parameter(
@@ -80,6 +84,9 @@ def __init__(self):
         self.MAX_SPIN_MOTOR_CURRENT = self.get_parameter("MAX_SPIN_MOTOR_CURRENT").value
         self.push_motor_position = self.get_parameter("push_motor_position").value
         self.tilt_actuator_position = self.get_parameter("tilt_actuator_position").value
+        self.extension_limit_switch = self.get_parameter("extension_limit_switch").value
+        self.auger_stowed = self.get_parameter("AUGER_STOWED").value
+        self.dumper_stowed = self.get_parameter("DUMPER_STOWED").value
         self.TILT_ACTUATOR_CURRENT_THRESHOLD = self.get_parameter(
             "TILT_ACTUATOR_CURRENT_THRESHOLD"
         )
@@ -197,7 +204,18 @@ def __init__(self):
             Potentiometers, "potentiometer", self.push_motor_position_callback, 10
         )
 
+        self.limit_switch_sub = self.create_subscription(
+            Bool, "ExtensionLimitSwitch", self.limit_switch_callback, 10
+        )
+
+        self.dumper_stowed_sub = self.create_subscription(
+            Bool, "dumper_stowed", self.dumper_stowed_callback, 10
+        )
+
         # TODO Define publishers here
+        self.auger_stowed_pub = self.create_publisher(Bool, "auger_stowed", 10)
+
+        self.extension_pos_sub = self.create_publisher(Float32, "extension_pos", 10)
 
     # Define subsystem methods here
 
@@ -243,6 +261,12 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool:
 
         # gets motor current until it is 0 which means it has hit an limit
         # switch
+        if tilt:
+            self.auger_stowed = False
+            msg = Bool()
+            msg.data = self.auger_stowed
+            self.auger_stowed_pub.publish(msg)
+
         while True:
             motor_get_future = self.cli_motor_get.call_async(
                 MotorCommandGet.Request(
@@ -263,6 +287,12 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool:
 
             time.sleep(0.1)
 
+        if not tilt:
+            self.auger_stowed = True
+            msg = Bool()
+            msg.data = self.auger_stowed
+            self.auger_stowed_pub.publish(msg)
+
         return True
 
     def stop_actuator_tilt(self) -> bool:
@@ -353,8 +383,13 @@ def set_motor_push_position(
 
             if motor_get_pos_future.result().success:
                 current_pos = motor_get_pos_future.result().data
-                if (speed <= 0 and current_pos <= desired_position) or (
-                    speed > 0 and current_pos >= desired_position
+                msg = Float32()
+                msg.data = current_pos
+                self.extension_pos_sub.publish(msg)
+                if (
+                    (speed <= 0 and current_pos <= desired_position)
+                    or (speed > 0 and current_pos >= desired_position)
+                    or (self.extension_limit_switch)
                 ):
                     break
             else:
@@ -445,6 +480,8 @@ def stop_auger_spin(self) -> bool:
 
     def extend_digger(self) -> bool:
         """Tilt and extend"""
+        if not self.dumper_stowed:
+            return False
 
         tilt_success = self.set_actuator_tilt_extension(True)
         if not tilt_success:
@@ -526,6 +563,12 @@ def stop_spin_callback(self, request, response):
     def push_motor_position_callback(self, msg):
         self.tilt_actuator_position = msg.data
 
+    def limit_switch_callback(self, msg):
+        self.extension_limit_switch = msg.data
+
+    def dumper_stowed_callback(self, msg):
+        self.dumper_stowed = msg.data
+
     def extend_push_callback(self, request, response):
         """
         This service requests to extend the push motor at full speed.

src/drivetrain/drivetrain/drivetrain_node.py

@@ -30,7 +30,9 @@ def __init__(self):
         self.declare_parameter("GAZEBO_SIMULATION", False)
         self.declare_parameter("MAX_DRIVETRAIN_RPM", 10000)
         self.declare_parameter("DRIVETRAIN_TYPE", "velocity")
-        self.declare_parameter("DIGGER_SAFETY_ZONE", 120)  # Measured in potentiometer units (0 to 1023)
+        self.declare_parameter(
+            "EXTENSION_SAFETY_ZONE", 0
+        )  # Measured in potentiometer units (0 to 1023)
 
         # Assign the ROS Parameters to member variables below #
         self.FRONT_LEFT_DRIVE = self.get_parameter("FRONT_LEFT_DRIVE").value
@@ -40,43 +42,76 @@ def __init__(self):
         self.GAZEBO_SIMULATION = self.get_parameter("GAZEBO_SIMULATION").value
         self.MAX_DRIVETRAIN_RPM = self.get_parameter("MAX_DRIVETRAIN_RPM").value
         self.DRIVETRAIN_TYPE = self.get_parameter("DRIVETRAIN_TYPE").value
-        self.DIGGER_SAFETY_ZONE = self.get_parameter("DIGGER_SAFETY_ZONE").value
+        self.EXTENSION_SAFETY_ZONE = self.get_parameter("EXTENSION_SAFETY_ZONE").value
 
         # Define publishers and subscribers here
-        self.cmd_vel_sub = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10)
-        self.lift_pose_subscription = self.create_subscription(Float32, "lift_pose", self.lift_pose_callback, 10)
+        self.cmd_vel_sub = self.create_subscription(
+            Twist, "cmd_vel", self.cmd_vel_callback, 10
+        )
+        self.extension_sub = self.create_subscription(
+            Float32, "extension_pos", self.extension_callback, 10
+        )
 
         if self.GAZEBO_SIMULATION:
-            self.gazebo_front_left_wheel_pub = self.create_publisher(Float64, "front_left_wheel/cmd_vel", 10)
-            self.gazebo_back_left_wheel_pub = self.create_publisher(Float64, "back_left_wheel/cmd_vel", 10)
-            self.gazebo_front_right_wheel_pub = self.create_publisher(Float64, "front_right_wheel/cmd_vel", 10)
-            self.gazebo_back_right_wheel_pub = self.create_publisher(Float64, "back_right_wheel/cmd_vel", 10)
+            self.gazebo_front_left_wheel_pub = self.create_publisher(
+                Float64, "front_left_wheel/cmd_vel", 10
+            )
+            self.gazebo_back_left_wheel_pub = self.create_publisher(
+                Float64, "back_left_wheel/cmd_vel", 10
+            )
+            self.gazebo_front_right_wheel_pub = self.create_publisher(
+                Float64, "front_right_wheel/cmd_vel", 10
+            )
+            self.gazebo_back_right_wheel_pub = self.create_publisher(
+                Float64, "back_right_wheel/cmd_vel", 10
+            )
 
         # Define service clients here
         self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set")
 
         # Define services (methods callable from the outside) here
-        self.srv_stop = self.create_service(Trigger, "drivetrain/stop", self.stop_callback)
-        self.srv_drive = self.create_service(Drive, "drivetrain/drive", self.drive_callback)
+        self.srv_stop = self.create_service(
+            Trigger, "drivetrain/stop", self.stop_callback
+        )
+        self.srv_drive = self.create_service(
+            Drive, "drivetrain/drive", self.drive_callback
+        )
 
         # Print the ROS Parameters to the terminal below #
-        self.get_logger().info("FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE))
-        self.get_logger().info("FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE))
-        self.get_logger().info("BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE))
-        self.get_logger().info("BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE))
-        self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION))
-        self.get_logger().info("MAX_DRIVETRAIN_RPM has been set to: " + str(self.MAX_DRIVETRAIN_RPM))
-        self.get_logger().info("DIGGER_SAFETY_ZONE has been set to: " + str(self.DIGGER_SAFETY_ZONE))
+        self.get_logger().info(
+            "FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE)
+        )
+        self.get_logger().info(
+            "FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE)
+        )
+        self.get_logger().info(
+            "BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE)
+        )
+        self.get_logger().info(
+            "BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE)
+        )
+        self.get_logger().info(
+            "GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)
+        )
+        self.get_logger().info(
+            "MAX_DRIVETRAIN_RPM has been set to: " + str(self.MAX_DRIVETRAIN_RPM)
+        )
+        self.get_logger().info(
+            "EXTENSION_SAFETY_ZONE has been set to: " + str(self.EXTENSION_SAFETY_ZONE)
+        )
 
         # Current position of the lift motor in potentiometer units (0 to 1023)
-        self.current_lift_position = None  # We don't know the current position yet
+        self.extension_pos = None  # We don't know the current position yet
 
     # Define subsystem methods here
     def drive(self, forward_power: float, turning_power: float) -> None:
         """This method drives the robot with the desired forward and turning power."""
 
         if (
-            (self.current_lift_position is None or self.current_lift_position > self.DIGGER_SAFETY_ZONE)
+            (
+                self.extension_pos is None
+                or self.extension_pos > self.EXTENSION_SAFETY_ZONE
+            )
             and not self.GAZEBO_SIMULATION
             and (forward_power != 0 or turning_power != 0)
         ):
@@ -104,25 +139,45 @@ def drive(self, forward_power: float, turning_power: float) -> None:
 
         # Send velocity (not duty cycle) motor commands to the motor_control_node
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower)
+            MotorCommandSet.Request(
+                can_id=self.FRONT_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower
+            )
         )
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower)
+            MotorCommandSet.Request(
+                can_id=self.BACK_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower
+            )
         )
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type=self.DRIVETRAIN_TYPE, value=rightPower)
+            MotorCommandSet.Request(
+                can_id=self.FRONT_RIGHT_DRIVE,
+                type=self.DRIVETRAIN_TYPE,
+                value=rightPower,
+            )
         )
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type=self.DRIVETRAIN_TYPE, value=rightPower)
+            MotorCommandSet.Request(
+                can_id=self.BACK_RIGHT_DRIVE,
+                type=self.DRIVETRAIN_TYPE,
+                value=rightPower,
+            )
         )
 
         # Publish the wheel speeds to the gazebo simulation
         if self.GAZEBO_SIMULATION:
             if self.DRIVETRAIN_TYPE == "velocity":
-                self.gazebo_front_left_wheel_pub.publish(Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM))
-                self.gazebo_back_left_wheel_pub.publish(Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM))
-                self.gazebo_front_right_wheel_pub.publish(Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM))
-                self.gazebo_back_right_wheel_pub.publish(Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM))
+                self.gazebo_front_left_wheel_pub.publish(
+                    Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM)
+                )
+                self.gazebo_back_left_wheel_pub.publish(
+                    Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM)
+                )
+                self.gazebo_front_right_wheel_pub.publish(
+                    Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM)
+                )
+                self.gazebo_back_right_wheel_pub.publish(
+                    Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM)
+                )
             else:
                 self.gazebo_front_left_wheel_pub.publish(Float64(data=leftPower))
                 self.gazebo_back_left_wheel_pub.publish(Float64(data=leftPower))
@@ -133,16 +188,24 @@ def drive(self, forward_power: float, turning_power: float) -> None:
     def stop(self) -> None:
         """This method stops the drivetrain."""
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=0.0)
+            MotorCommandSet.Request(
+                can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=0.0
+            )
         )
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=0.0)
+            MotorCommandSet.Request(
+                can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=0.0
+            )
         )
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=0.0)
+            MotorCommandSet.Request(
+                can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=0.0
+            )
         )
         self.cli_motor_set.call_async(
-            MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=0.0)
+            MotorCommandSet.Request(
+                can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=0.0
+            )
         )
 
     # Define service callback methods here
@@ -165,9 +228,9 @@ def cmd_vel_callback(self, msg: Twist) -> None:
         self.drive(msg.linear.x, msg.angular.z)
 
     # Define the subscriber callback for the lift pose topic
-    def lift_pose_callback(self, msg: Float32):
+    def extension_callback(self, msg: Float32):
         # Average the two potentiometer values
-        self.current_lift_position = msg.data
+        self.extension_pos = msg.data
 
 
 def main(args=None):