Curated Improvements from my other PR (#62)

* Arduino sketch improvements

* Subsystem logger changes

* comment out motor_control print statements

* Rename 'speed' to 'power'

power more accurately reflects what we are doing

* comment improvements

* Reorder teleop controls

* Autoformat

* added timeouts

---------

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

arduino-sketches/linear_actuator/linear_actuator.ino

@@ -1,8 +1,7 @@
 #include <ezButton.h> // This library makes it easier for us to work with limit switches
 #include <Servo.h>    // Allows us to treat a PWM Talon SRX / Victor SP as a Servo with writeMicroseconds()
 
-Servo small_linear_actuator;
-Servo big_linear_actuator;
+Servo linear_actuator;
 
 ezButton limit_switch_extend(8);  // extend limit switch pin
 ezButton limit_switch_retract(7); // retract limit switch pin
@@ -18,13 +17,11 @@ void setup()
 {
   Serial.begin(9600); // open serial communication with a baud rate of 9600 bps
 
-  small_linear_actuator.attach(3); // Victor SP PWM pin
-  big_linear_actuator.attach(9);   // Victor SP PWM pin
+  linear_actuator.attach(9);   // Victor SP PWM pin
 
   pinMode(LED_BUILTIN, OUTPUT); // This is the built-in LED on the Arduino
 
-  stop_actuator();                    // stop the linear actuator by default
-  victor_stop(small_linear_actuator); // stop the small linear actuator by default
+  stop_actuator(); // stop the linear actuator by default
 }
 
 // put main loop code in this method to run repeatedly:
@@ -41,22 +38,17 @@ void loop()
       extend(buffer[1]); // extend the linear actuator
     if (buffer[0] == 'r')
       retract(buffer[1]); // retract the linear actuator
-
-    if (buffer[0] == 'a')
-      extend_small_actuator(buffer[1]); // extend the small linear actuator
-    if (buffer[0] == 'b')
-      retract_small_actuator(buffer[1]); // retract the small linear actuator
   }
 
   // check if the extend limit switch is pressed and we are currently extending
-  if (limit_switch_extend.isPressed() && extending)
+  if (limit_switch_extend.getState() == 0 && extending)
   {
     stop_actuator();   // stop the linear actuator
     Serial.write('f'); // send a message over serial to the Jetson
   }
 
   // check if the retract limit switch is pressed and we are currently retracting
-  if (limit_switch_retract.isPressed() && retracting)
+  if (limit_switch_retract.getState() == 0 && retracting)
   {
     stop_actuator();   // stop the linear actuator
     Serial.write('s'); // send a message over serial to the Jetson
@@ -68,41 +60,26 @@ void extend(char speed)
 {
   extending = true;
   retracting = false;
-  victor_set_forward_power(big_linear_actuator, speed);
+  victor_set_forward_power(linear_actuator, speed);
   digitalWrite(LED_BUILTIN, HIGH); // Turn on the built-in LED
 }
 // retract the linear actuator by running the motor in reverse
 void retract(char speed)
 {
   retracting = true;
   extending = false;
-  victor_set_reverse_power(big_linear_actuator, speed);
+  victor_set_reverse_power(linear_actuator, speed);
   digitalWrite(LED_BUILTIN, HIGH); // Turn on the built-in LED
 }
 // stop the linear actuator motor
 void stop_actuator()
 {
-  victor_stop(big_linear_actuator);
+  victor_stop(linear_actuator);
   digitalWrite(LED_BUILTIN, LOW); // Turn off the built-in LED
   extending = false;
   retracting = false;
 }
 
-// extend the small linear actuator by running the motor forwards
-void extend_small_actuator(char speed)
-{ // input speed is 0-100
-  victor_set_forward_power(small_linear_actuator, speed);
-  delay(4000);                        // wait for 4 seconds before stopping
-  victor_stop(small_linear_actuator); // stop the small linear actuator
-}
-// retract the small linear actuator by running the motor in reverse
-void retract_small_actuator(char speed)
-{ // input speed is 0-100
-  victor_set_reverse_power(small_linear_actuator, speed);
-  delay(4000);                        // wait for 4 seconds before stopping
-  victor_stop(small_linear_actuator); // stop the small linear actuator
-}
-
 void victor_set_forward_power(Servo victor, char speed)
 { // input speed is 0-100
   long speed_long = speed;

config/rovr_control.yaml

@@ -3,8 +3,8 @@ main_control_node:
     autonomous_driving_power: 0.25   # Measured in Duty Cycle (0.0-1.0)
     max_drive_power: 1.0             # Measured in Duty Cycle (0.0-1.0)
     max_turn_power: 1.0              # Measured in Duty Cycle (0.0-1.0)
-    linear_actuator_speed: 8         # Duty Cycle value between 0-100 (not 0.0-1.0)
-    linear_actuator_up_speed: 40     # Duty Cycle value between 0-100 (not 0.0-1.0)
+    linear_actuator_power: 8         # Duty Cycle value between 0-100 (not 0.0-1.0)
+    linear_actuator_up_power: 40     # Duty Cycle value between 0-100 (not 0.0-1.0)
     digger_rotation_power: 0.4       # Measured in Duty Cycle (0.0-1.0)
     drum_belt_power: 0.2             # Measured in Duty Cycle (0.0-1.0)
     conveyor_belt_power: 0.35        # Measured in Duty Cycle (0.0-1.0)

src/conveyor/conveyor/conveyor_node.py

@@ -83,9 +83,9 @@ def toggle_callback(self, request, response) -> None:
 def main(args=None):
     """The main function."""
     rclpy.init(args=args)
-    print("Initializing the Conveyor subsystem!")
 
     node = ConveyorNode()
+    node.get_logger().info("Initializing the Conveyor subsystem!")
     rclpy.spin(node)
 
     node.destroy_node()

src/digger/digger/digger_node.py

@@ -72,9 +72,9 @@ def toggle_callback(self, request, response) -> None:
 def main(args=None):
     """The main function."""
     rclpy.init(args=args)
-    print("Initializing the Digger subsystem!")
 
     node = DiggerNode()
+    node.get_logger().info("Initializing the Digger subsystem!")
     rclpy.spin(node)
 
     node.destroy_node()

src/drivetrain/drivetrain/drivetrain_node.py

@@ -88,9 +88,9 @@ def cmd_vel_callback(self, msg):
 def main(args=None):
     """The main function."""
     rclpy.init(args=args)
-    print("Initializing the Drivetrain subsystem!")
 
     node = DrivetrainNode()
+    node.get_logger().info("Initializing the Drivetrain subsystem!")
     rclpy.spin(node)
 
     node.destroy_node()

src/motor_control/src/motor_control_node.cpp

@@ -70,7 +70,7 @@ class MotorControlNode : public rclcpp::Node {
 
     send_can(id + 0x00000000, data); // ID does NOT need to be modified to signify this is a duty cycle command
     this->current_msg[id] = std::make_tuple(id + 0x00000000, data); // update the hashmap
-    RCLCPP_INFO(this->get_logger(), "Setting the duty cycle of CAN ID: %u to %f", id, percentPower);
+    // RCLCPP_INFO(this->get_logger(), "Setting the duty cycle of CAN ID: %u to %f", id, percentPower);
   }
 
   // Set the velocity of the motor in RPM (Rotations Per Minute)
@@ -79,7 +79,7 @@ class MotorControlNode : public rclcpp::Node {
 
     send_can(id + 0x00000300, data); // ID must be modified to signify this is an RPM command
     this->current_msg[id] = std::make_tuple(id + 0x00000300, data);                  // update the hashmap
-    RCLCPP_INFO(this->get_logger(), "Setting the RPM of CAN ID: %u to %d", id, rpm); // Print to the terminal
+    // RCLCPP_INFO(this->get_logger(), "Setting the RPM of CAN ID: %u to %d", id, rpm); // Print to the terminal
   }
 
   // Set the position of the motor in _____ (degrees? encoder counts?)

src/offloader/offloader/offloader_node.py

@@ -74,9 +74,9 @@ def toggle_callback(self, request, response) -> None:
 def main(args=None):
     """The main function."""
     rclpy.init(args=args)
-    print("Initializing the Offloader subsystem!")
 
     node = OffloaderNode()
+    node.get_logger().info("Initializing the Offloader subsystem!")
     rclpy.spin(node)
 
     node.destroy_node()

src/rovr_control/rovr_control/main_control_node.py

@@ -61,43 +61,43 @@ def __init__(self) -> None:
         self.declare_parameter("autonomous_driving_power", 0.25)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("max_drive_power", 1.0)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("max_turn_power", 1.0)  # Measured in Duty Cycle (0.0-1.0)
-        self.declare_parameter("linear_actuator_speed", 8)  # Duty Cycle value between 0-100 (not 0.0-1.0)
-        self.declare_parameter("linear_actuator_up_speed", 40)  # Duty Cycle value between 0-100 (not 0.0-1.0)
+        self.declare_parameter("linear_actuator_power", 8)  # Duty Cycle value between 0-100 (not 0.0-1.0)
+        self.declare_parameter("linear_actuator_up_power", 40)  # Duty Cycle value between 0-100 (not 0.0-1.0)
         self.declare_parameter("digger_rotation_power", 0.4)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("drum_belt_power", 0.2)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("conveyor_belt_power", 0.35)  # Measured in Duty Cycle (0.0-1.0)
         self.declare_parameter("offload_belt_power", 0.35)  # Measured in Duty Cycle (0.0-1.0)
 
-        # Assign the ROS Parameters
+        # Assign the ROS Parameters to member variables below #
         self.autonomous_driving_power = self.get_parameter("autonomous_driving_power").value
         self.max_drive_power = self.get_parameter("max_drive_power").value
         self.max_turn_power = self.get_parameter("max_turn_power").value
         self.digger_rotation_power = self.get_parameter("digger_rotation_power").value
         self.drum_belt_power = self.get_parameter("drum_belt_power").value
         self.conveyor_belt_power = self.get_parameter("conveyor_belt_power").value
         self.offload_belt_power = self.get_parameter("offload_belt_power").value
-        self.linear_actuator_speed = self.get_parameter("linear_actuator_speed").value
-        self.linear_actuator_up_speed = self.get_parameter("linear_actuator_up_speed").value
+        self.linear_actuator_power = self.get_parameter("linear_actuator_power").value
+        self.linear_actuator_up_power = self.get_parameter("linear_actuator_up_power").value
 
-        # Print the ROS Parameters to the terminal
+        # Print the ROS Parameters to the terminal below #
         print("autonomous_driving_power has been set to:", self.autonomous_driving_power)
         print("max_drive_power has been set to:", self.max_drive_power)
         print("max_turn_power has been set to:", self.max_turn_power)
-        print("linear_actuator_speed has been set to:", self.linear_actuator_speed)
-        print("linear_actuator_up_speed has been set to:", self.linear_actuator_up_speed)
+        print("linear_actuator_power has been set to:", self.linear_actuator_power)
+        print("linear_actuator_up_power has been set to:", self.linear_actuator_up_power)
         print("digger_rotation_power has been set to:", self.digger_rotation_power)
         print("drum_belt_power has been set to:", self.drum_belt_power)
         print("conveyor_belt_power has been set to:", self.conveyor_belt_power)
         print("offload_belt_power has been set to:", self.offload_belt_power)
 
-        # NOTE: The code commented out below is for dynamic ip address asignment, but we haven't gotten it to work yet
+        # NOTE: The code commented out below is for dynamic ip address asignment, but we haven't gotten it to work consistantly yet
         # self.target_ip = get_target_ip('blixt-G14', '192.168.1.110', self.get_logger().info)
         # self.get_logger().info(f'set camera stream target ip to {self.target_ip}')
 
         # Try connecting to the Arduino over Serial
         try:
             # Set this as a static Serial port!
-            self.arduino = serial.Serial("/dev/Arduino_Uno", 9600)
+            self.arduino = serial.Serial("/dev/Arduino_Uno", 9600, timeout=0.01)
         except Exception as e:
             print(e)  # If an exception is raised, print it, and then move on
 
@@ -164,22 +164,20 @@ async def auto_dig_procedure(self) -> None:
             self.arduino.read_all()  # Read all messages from the serial buffer to clear them out
             await asyncio.sleep(2)  # Wait a bit for the drum motor to get up to speed
             # Tell the Arduino to extend the linear actuator
-            self.arduino.write(f"e{chr(self.linear_actuator_speed)}".encode("ascii"))
-            while True:  # Wait for a confirmation message from the Arduino
-                reading = self.arduino.read()
-                print(reading)
-                if reading == b"f":
-                    break
-                await asyncio.sleep(0)  # Trick to allow other tasks to run ;)
+            self.arduino.write(f"e{chr(self.linear_actuator_power)}".encode("ascii"))
+            # Wait for a confirmation message from the Arduino
+            reading = self.arduino.read().decode("ascii")
+            while reading != "f":
+                reading = self.arduino.read().decode("ascii")
+                await asyncio.sleep(0.01)  # Trick to allow other tasks to run ;)
             await asyncio.sleep(5)  # Wait for 5 seconds
             # Tell the Arduino to retract the linear actuator
-            self.arduino.write(f"r{chr(self.linear_actuator_up_speed)}".encode("ascii"))
-            while True:  # Wait for a confirmation message from the Arduino
-                reading = self.arduino.read()
-                print(reading)
-                if reading == b"s":
-                    break
-                await asyncio.sleep(0)  # Trick to allow other tasks to run ;)
+            self.arduino.write(f"r{chr(self.linear_actuator_up_power)}".encode("ascii"))
+            # Wait for a confirmation message from the Arduino
+            reading = self.arduino.read().decode("ascii")
+            while reading != "s":
+                reading = self.arduino.read().decode("ascii")
+                await asyncio.sleep(0.01)  # Trick to allow other tasks to run ;)
             # Reverse the digging drum
             await self.cli_digger_stop.call_async(Stop.Request())
             await asyncio.sleep(0.5)  # Let the digger slow down
@@ -275,49 +273,48 @@ def apriltags_callback(self, msg: TFMessage) -> None:
     def joystick_callback(self, msg: Joy) -> None:
         """This method is called whenever a joystick message is received."""
 
-        # TELEOP CONTROLS BELOW #
+        # PUT TELEOP CONTROLS BELOW #
+
         if self.state == states["Teleop"]:
             # Drive the robot using joystick input during Teleop
             drive_power = msg.axes[RIGHT_JOYSTICK_VERTICAL_AXIS] * self.max_drive_power  # Forward power
             turn_power = msg.axes[LEFT_JOYSTICK_HORIZONTAL_AXIS] * self.max_turn_power  # Turning power
             self.drive_power_publisher.publish(Twist(linear=Vector3(x=drive_power), angular=Vector3(z=turn_power)))
 
-            # Check if the digger button is pressed
+            # Check if the digger button is pressed #
             if msg.buttons[X_BUTTON] == 1 and buttons[X_BUTTON] == 0:
                 self.cli_digger_toggle.call_async(SetPower.Request(power=self.digger_rotation_power))
                 self.cli_conveyor_toggle.call_async(
                     ConveyorSetPower.Request(
                         drum_belt_power=self.drum_belt_power, conveyor_belt_power=self.conveyor_belt_power
                     )
                 )
-            # Check if the offloader button is pressed
+            # Reverse the digging drum (set negative power) #
+            if msg.buttons[RIGHT_BUMPER] == 1 and buttons[RIGHT_BUMPER] == 0:
+                self.cli_digger_setPower.call_async(SetPower.Request(power=-1 * self.digger_rotation_power))
+                self.cli_conveyor_toggle.call_async(
+                    ConveyorSetPower.Request(
+                        drum_belt_power=self.drum_belt_power, conveyor_belt_power=self.conveyor_belt_power
+                    )
+                )
+            # Check if the offloader button is pressed #
             if msg.buttons[B_BUTTON] == 1 and buttons[B_BUTTON] == 0:
                 self.cli_offloader_toggle.call_async(SetPower.Request(power=self.offload_belt_power))
 
-            # Check if the digger_extend button is pressed
+            # Check if the digger_extend button is pressed #
             if msg.buttons[A_BUTTON] == 1 and buttons[A_BUTTON] == 0:
                 self.digger_extend_toggled = not self.digger_extend_toggled
                 if self.digger_extend_toggled:
                     # Tell the Arduino to extend the linear actuator
-                    self.arduino.write(f"e{chr(self.linear_actuator_speed)}".encode("ascii"))
+                    self.arduino.write(f"e{chr(self.linear_actuator_power)}".encode("ascii"))
                 else:
                     # Tell the Arduino to retract the linear actuator
-                    self.arduino.write(f"r{chr(self.linear_actuator_up_speed)}".encode("ascii"))
-
-            # Stop the linear actuator
+                    self.arduino.write(f"r{chr(self.linear_actuator_up_power)}".encode("ascii"))
+            # Stop the linear actuator #
             if msg.buttons[Y_BUTTON] == 1 and buttons[Y_BUTTON] == 0:
                 # Send stop command to the Arduino
                 self.arduino.write(f"e{chr(0)}".encode("ascii"))
 
-            if msg.buttons[RIGHT_BUMPER] == 1 and buttons[RIGHT_BUMPER] == 0:
-                # Reverse the digging drum (set negative power)
-                self.cli_digger_setPower.call_async(SetPower.Request(power=-1 * self.digger_rotation_power))
-                self.cli_conveyor_toggle.call_async(
-                    ConveyorSetPower.Request(
-                        drum_belt_power=self.drum_belt_power, conveyor_belt_power=self.conveyor_belt_power
-                    )
-                )
-
         # THE CONTROLS BELOW ALWAYS WORK #
 
         # Check if the autonomous digging button is pressed