diff --git a/.rosinstall b/.rosinstall index 4ca3608ae..ae77b2bc4 100644 --- a/.rosinstall +++ b/.rosinstall @@ -1,41 +1,22 @@ -- git: - local-name: src/external_pkgs/nmea_navsat_driver - uri: https://github.com/UBC-Snowbots/nmea_navsat_driver.git - version: noetic-snowbots-mods -- git: - local-name: src/external_pkgs/realsense-ros - uri: https://github.com/IntelRealSense/realsense-ros.git -- git: - local-name: external_libs/librealsense - uri: https://github.com/IntelRealsense/librealsense.git -- git: - local-name: src/external_pkgs/ros_arduino_bridge - uri: https://github.com/UBC-Snowbots/ros_arduino_bridge.git - version: noetic-snowbots-mods -- git: - local-name: src/external_pkgs/sicktoolbox - uri: https://github.com/UBC-Snowbots/sicktoolbox.git -- git: - local-name: src/external_pkgs/sicktoolbox_wrapper - uri: https://github.com/ros-drivers/sicktoolbox_wrapper.git -- git: - local-name: src/external_pkgs/zed-ros-wrapper - uri: https://github.com/UBC-Snowbots/zed-ros-wrapper.git -- git: - local-name: src/mapping_urc/multi_resolution_graph - uri: 'https://github.com/garethellis0/multi_resolution_graph.git' - version: 0.0.3 -- git: - local-name: external_libs/dkms-hid-nintendo - uri: 'https://github.com/nicman23/dkms-hid-nintendo' -- git: - local-name: external_libs/joycond - uri: https://github.com/DanielOgorchock/joycond --git: - local-name: src/external_pkgs/qt_ros - uri: https://github.com/stonier/qt_ros.git -- git: - local-name: src/external_pkgs/phidgets_drivers - uri: https://github.com/ros-drivers/phidgets_drivers - version: noetic - +# IT IS UNLIKELY YOU WANT TO EDIT THIS FILE BY HAND, +# UNLESS FOR REMOVING ENTRIES. +# IF YOU WANT TO CHANGE THE ROS ENVIRONMENT VARIABLES +# USE THE rosinstall TOOL INSTEAD. +# IF YOU CHANGE IT, USE rosinstall FOR THE CHANGES TO TAKE EFFECT +- git: {local-name: src/external_pkgs/nmea_navsat_driver, uri: 'https://github.com/UBC-Snowbots/nmea_navsat_driver.git', + version: noetic-snowbots-mods} +- git: {local-name: src/external_pkgs/realsense-ros, uri: 'https://github.com/IntelRealSense/realsense-ros.git'} +- git: {local-name: external_libs/librealsense, uri: 'https://github.com/IntelRealsense/librealsense.git'} +- git: {local-name: src/external_pkgs/ros_arduino_bridge, uri: 'https://github.com/UBC-Snowbots/ros_arduino_bridge.git', + version: noetic-snowbots-mods} +- git: {local-name: src/external_pkgs/sicktoolbox, uri: 'https://github.com/UBC-Snowbots/sicktoolbox.git'} +- git: {local-name: src/external_pkgs/sicktoolbox_wrapper, uri: 'https://github.com/ros-drivers/sicktoolbox_wrapper.git'} +- git: {local-name: src/external_pkgs/zed-ros-wrapper, uri: 'https://github.com/UBC-Snowbots/zed-ros-wrapper.git'} +- git: {local-name: src/mapping_urc/multi_resolution_graph, uri: 'https://github.com/garethellis0/multi_resolution_graph.git', + version: 0.0.3} +- git: {local-name: external_libs/dkms-hid-nintendo, uri: 'https://github.com/nicman23/dkms-hid-nintendo'} +- git: {local-name: external_libs/joycond, uri: 'https://github.com/DanielOgorchock/joycond'} +- git: {local-name: src/external_pkgs/qt_ros, uri: 'https://github.com/stonier/qt_ros.git'} +- git: {local-name: src/external_pkgs/phidgets_drivers, uri: 'https://github.com/ros-drivers/phidgets_drivers', + version: noetic} +- git: {local-name: src/external_pkgs/serial, uri: 'https://github.com/wjwwood/serial.git'} diff --git a/setup_scripts/install_dependencies.sh b/setup_scripts/install_dependencies.sh index a6ae1a291..cc4bba138 100755 --- a/setup_scripts/install_dependencies.sh +++ b/setup_scripts/install_dependencies.sh @@ -79,7 +79,7 @@ sudo apt-get install -y\ sudo apt-get update -y sudo apt-get install -y libalglib-dev - +sudo apt-get install -y libdlib-dev sudo apt-get install -y libgsl-dev sudo apt-get install -y libserial-dev diff --git a/src/arm_2023_moveit_config/.setup_assistant b/src/arm_2023_moveit_config/.setup_assistant new file mode 100644 index 000000000..f45c37cb3 --- /dev/null +++ b/src/arm_2023_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: snowbots_arm_urdf_simplified_v5_12 + relative_path: urdf/snowbots_arm_urdf_simplified_v5_12.urdf + xacro_args: "" + SRDF: + relative_path: config/snowbots_arm_urdf_simplified_v5_12.srdf + CONFIG: + author_name: Rowan Zawadzki + author_email: zawadzkirowan@gmail.com + generated_timestamp: 1675661163 \ No newline at end of file diff --git a/src/arm_2023_moveit_config/CMakeLists.txt b/src/arm_2023_moveit_config/CMakeLists.txt new file mode 100644 index 000000000..1423a86cc --- /dev/null +++ b/src/arm_2023_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(arm_2023_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/arm_2023_moveit_config/config/cartesian_limits.yaml b/src/arm_2023_moveit_config/config/cartesian_limits.yaml new file mode 100644 index 000000000..7df72f693 --- /dev/null +++ b/src/arm_2023_moveit_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/src/arm_2023_moveit_config/config/chomp_planning.yaml b/src/arm_2023_moveit_config/config/chomp_planning.yaml new file mode 100644 index 000000000..eb9c91225 --- /dev/null +++ b/src/arm_2023_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/src/arm_2023_moveit_config/config/fake_controllers.yaml b/src/arm_2023_moveit_config/config/fake_controllers.yaml new file mode 100644 index 000000000..561535a7b --- /dev/null +++ b/src/arm_2023_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,17 @@ +controller_list: + - name: fake_arm_controller + type: $(arg fake_execution_type) + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 + - name: fake_manipulator_controller + type: $(arg fake_execution_type) + joints: + [] +initial: # Define initial robot poses per group + - group: arm + pose: home diff --git a/src/arm_2023_moveit_config/config/gazebo_controllers.yaml b/src/arm_2023_moveit_config/config/gazebo_controllers.yaml new file mode 100644 index 000000000..e4d2eb00c --- /dev/null +++ b/src/arm_2023_moveit_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/src/arm_2023_moveit_config/config/joint_limits.yaml b/src/arm_2023_moveit_config/config/joint_limits.yaml new file mode 100644 index 000000000..77435ed0b --- /dev/null +++ b/src/arm_2023_moveit_config/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + j1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + j2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + j3: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + j4: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + j5: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + j6: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/src/arm_2023_moveit_config/config/kinematics.yaml b/src/arm_2023_moveit_config/config/kinematics.yaml new file mode 100644 index 000000000..6a217a811 --- /dev/null +++ b/src/arm_2023_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +sb_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/src/arm_2023_moveit_config/config/ompl_planning.yaml b/src/arm_2023_moveit_config/config/ompl_planning.yaml new file mode 100644 index 000000000..94ea7d3cf --- /dev/null +++ b/src/arm_2023_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,183 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +sb_arm: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(j1,j2) + longest_valid_segment_fraction: 0.005 +manipulator: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/src/arm_2023_moveit_config/config/ros_controllers.yaml b/src/arm_2023_moveit_config/config/ros_controllers.yaml new file mode 100644 index 000000000..3e03dd6a4 --- /dev/null +++ b/src/arm_2023_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,41 @@ + +arm_position_controller: + type: position_controllers/JointGroupPositionController + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 + gains: + j1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + j2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + j3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + j4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + j5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + j6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/src/arm_2023_moveit_config/config/sensors_3d.yaml b/src/arm_2023_moveit_config/config/sensors_3d.yaml new file mode 100644 index 000000000..51010a367 --- /dev/null +++ b/src/arm_2023_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/src/arm_2023_moveit_config/config/simple_moveit_controllers.yaml b/src/arm_2023_moveit_config/config/simple_moveit_controllers.yaml new file mode 100644 index 000000000..4118c3b48 --- /dev/null +++ b/src/arm_2023_moveit_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,2 @@ +controller_list: + [] \ No newline at end of file diff --git a/src/arm_2023_moveit_config/config/snowbots_arm_urdf_simplified_v5_12.srdf b/src/arm_2023_moveit_config/config/snowbots_arm_urdf_simplified_v5_12.srdf new file mode 100644 index 000000000..2beca6ca7 --- /dev/null +++ b/src/arm_2023_moveit_config/config/snowbots_arm_urdf_simplified_v5_12.srdf @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/config/stomp_planning.yaml b/src/arm_2023_moveit_config/config/stomp_planning.yaml new file mode 100644 index 000000000..5a6d0193a --- /dev/null +++ b/src/arm_2023_moveit_config/config/stomp_planning.yaml @@ -0,0 +1,78 @@ +stomp/sb_arm: + group_name: sb_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/manipulator: + group_name: manipulator + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/src/arm_2023_moveit_config/launch/chomp_planning_pipeline.launch.xml b/src/arm_2023_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..13004a9c1 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/default_warehouse_db.launch b/src/arm_2023_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 000000000..b1cfa7f1f --- /dev/null +++ b/src/arm_2023_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/demo.launch b/src/arm_2023_moveit_config/launch/demo.launch new file mode 100644 index 000000000..c3ffdd80c --- /dev/null +++ b/src/arm_2023_moveit_config/launch/demo.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/demo_gazebo.launch b/src/arm_2023_moveit_config/launch/demo_gazebo.launch new file mode 100644 index 000000000..0ef8f9540 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/src/arm_2023_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..9ef9f2545 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/gazebo.launch b/src/arm_2023_moveit_config/launch/gazebo.launch new file mode 100644 index 000000000..92b025991 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/joystick_control.launch b/src/arm_2023_moveit_config/launch/joystick_control.launch new file mode 100644 index 000000000..9411f6e60 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/move_group.launch b/src/arm_2023_moveit_config/launch/move_group.launch new file mode 100644 index 000000000..3cf6108a4 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/move_group.launch @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/moveit.rviz b/src/arm_2023_moveit_config/launch/moveit.rviz new file mode 100644 index 000000000..6d8744450 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/moveit.rviz @@ -0,0 +1,131 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 226 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Name: MotionPlanning + Planned Path: + Links: ~ + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: ~ + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.0 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.5 + Target Frame: world + Yaw: -0.6232355833053589 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 848 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1291 + X: 454 + Y: 25 diff --git a/src/arm_2023_moveit_config/launch/moveit_rviz.launch b/src/arm_2023_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 000000000..a4605c037 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml b/src/arm_2023_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..ba64870cb --- /dev/null +++ b/src/arm_2023_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/ompl_planning_pipeline.launch.xml b/src/arm_2023_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 000000000..6a802a5a1 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/src/arm_2023_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 000000000..c7c4cf500 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/planning_context.launch b/src/arm_2023_moveit_config/launch/planning_context.launch new file mode 100644 index 000000000..fd6450f9b --- /dev/null +++ b/src/arm_2023_moveit_config/launch/planning_context.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/planning_pipeline.launch.xml b/src/arm_2023_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 000000000..4b4d0d663 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml b/src/arm_2023_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..9ebc91c10 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/arm_2023_moveit_config/launch/ros_controllers.launch b/src/arm_2023_moveit_config/launch/ros_controllers.launch new file mode 100644 index 000000000..73fd33b0c --- /dev/null +++ b/src/arm_2023_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/run_benchmark_ompl.launch b/src/arm_2023_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 000000000..d1cc56ce0 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/sensor_manager.launch.xml b/src/arm_2023_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 000000000..94da2f8d0 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/setup_assistant.launch b/src/arm_2023_moveit_config/launch/setup_assistant.launch new file mode 100644 index 000000000..f64aa5278 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/simple_moveit_controller_manager.launch.xml b/src/arm_2023_moveit_config/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..06e9462bf --- /dev/null +++ b/src/arm_2023_moveit_config/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/snowbots_arm_urdf_simplified_v5_12_moveit_sensor_manager.launch.xml b/src/arm_2023_moveit_config/launch/snowbots_arm_urdf_simplified_v5_12_moveit_sensor_manager.launch.xml new file mode 100644 index 000000000..5d02698d7 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/snowbots_arm_urdf_simplified_v5_12_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/src/arm_2023_moveit_config/launch/stomp_planning_pipeline.launch.xml b/src/arm_2023_moveit_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..7b8a10435 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/trajectory_execution.launch.xml b/src/arm_2023_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 000000000..20c3dfc45 --- /dev/null +++ b/src/arm_2023_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/warehouse.launch b/src/arm_2023_moveit_config/launch/warehouse.launch new file mode 100644 index 000000000..0712e670f --- /dev/null +++ b/src/arm_2023_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/launch/warehouse_settings.launch.xml b/src/arm_2023_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 000000000..e473b083b --- /dev/null +++ b/src/arm_2023_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/arm_2023_moveit_config/package.xml b/src/arm_2023_moveit_config/package.xml new file mode 100644 index 000000000..c3aa1f538 --- /dev/null +++ b/src/arm_2023_moveit_config/package.xml @@ -0,0 +1,41 @@ + + + arm_2023_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the snowbots_arm_urdf_simplified_v5_12 with the MoveIt Motion Planning Framework + + Rowan Zawadzki + Rowan Zawadzki + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + snowbots_arm_urdf_simplified_v5_12 + + + diff --git a/src/arm_hardware_driver/CMakeLists.txt b/src/arm_hardware_driver/CMakeLists.txt index 5a0a09267..93817ee95 100644 --- a/src/arm_hardware_driver/CMakeLists.txt +++ b/src/arm_hardware_driver/CMakeLists.txt @@ -17,9 +17,11 @@ find_package(catkin REQUIRED COMPONENTS std_msgs sb_utils sb_msgs -) -find_library(libserial_LIBRARIES libserial.so.0) + serial + ) +#set(serial_SRCS src/serial.cc include/serial/serial.h include/serial/v8stdint.h src/) +#find_library(libserial_LIBRARIES libserial.so.0) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -111,8 +113,8 @@ find_library(libserial_LIBRARIES libserial.so.0) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include -# LIBRARIES sample_package -# CATKIN_DEPENDS roscpp std_msgs + LIBRARIES serial + CATKIN_DEPENDS roscpp std_msgs serial # DEPENDS system_lib ) @@ -130,9 +132,9 @@ include_directories( ) ## Declare a C++ library -# add_library(sample_package -# src/${PROJECT_NAME}/sample_package.cpp -# ) + # add_library(serial + # src/${PROJECT_NAME}/sample_package.cpp + #) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -142,6 +144,7 @@ include_directories( ## Declare a C++ executable # add_executable(sample_package_node src/sample_package_node.cpp) add_executable(arm_hardware_driver src/arm_hardware_driver.cpp src/armHardwareDriver.cpp include/armHardwareDriver.h) +#add_executable(arm_hardware_driver_ros_direct src/ROSDIRECTarm_hardware_driver.cpp src/ROSDIRECTarmHardwareDriver.cpp include/ROSDIRECTarmHardwareDriver.h) ## Add cmake target dependencies of the executable @@ -152,7 +155,7 @@ add_executable(arm_hardware_driver src/arm_hardware_driver.cpp src/armHardwareDr # target_link_libraries(sample_package_node # ${catkin_LIBRARIES} # ) -target_link_libraries(arm_hardware_driver ${catkin_LIBRARIES} ${libserial_LIBRARIES} ${sb_utils_LIBRARIES}) +target_link_libraries(arm_hardware_driver ${catkin_LIBRARIES} ${sb_utils_LIBRARIES}) ############# ## Install ## diff --git a/src/arm_hardware_driver/include/ROSDIRECTarmHardwareDriver.h b/src/arm_hardware_driver/include/ROSDIRECTarmHardwareDriver.h new file mode 100644 index 000000000..83f8f4dd2 --- /dev/null +++ b/src/arm_hardware_driver/include/ROSDIRECTarmHardwareDriver.h @@ -0,0 +1,165 @@ +/* + * Created By: Tate Kolton and Ihsan Olawale + * Created On: July 4, 2022 + * Description: Header file for recieving messages from pro controller and + * relaying them to arm hardware driver module + */ + +#ifndef ARM_HARDWARE_DRIVER_MYNODE_H +#define ARM_HARDWARE_DRIVER_MYNODE_H + +// STD Includes +#include +#include + +// ROS Includes +#include +#include +#include +#include +#include + +// Snowbots Includes +#include +#include +#include + +// Other +#define debug 1 + +class ArmHardwareDriver { + public: + ArmHardwareDriver(ros::NodeHandle& nh); + void teensyPosCallback(const sb_msgs::ArmPosition inMsg); + //void obsPosCallback(const sb_); + void parseInput(std::string inMsg); + void joint_space_motion(std::string inMsg); + void drill_motion(std::string inMsg); + void endEffector(const char dir); + void endEffectorRel(); + void prepareDrilling(); + void collectSample(); + void depositSample(); + void manualDrill(const char dir); + void releaseDrill(); + void homeArm(); + void cartesian_motion(std::string inMsg); + void cartesian_moveit_move(std::vector& pos_commands, + std::vector& joint_positions); + void updateEncoderSteps(std::string msg); + void encStepsToJointPos(std::vector& enc_steps, + std::vector& joint_positions); + void jointPosToEncSteps(std::vector& joint_positions, + std::vector& enc_steps); + void sendMsg(std::string outMsg); + void recieveMsg(); + void requestArmPosition(); + void updateHWInterface(); + void requestEEFeedback(); + void requestJPFeedback(); + void homeEE(); + void axisRelease(const char axis); + void axisMove(const char axis, const char dir); + void newInput(); + //void joyparse(std::int32_t buttons[], std::float_t axes[]); //maybe return a time? so we know how long since the last input + + // character representations of buttons for arm communication + const char leftJSU = 'A'; + const char leftJSD = 'B'; + const char rightJSU = 'C'; + const char rightJSD = 'D'; + const char buttonA = 'E'; + const char buttonB = 'F'; + const char buttonX = 'G'; + const char buttonY = 'H'; + const char triggerL = 'I'; + const char triggerR = 'J'; + const char bumperL = 'K'; + const char bumperR = 'L'; + const char buttonARel = 'M'; + const char buttonBRel = 'N'; + const char buttonXRel = 'O'; + const char buttonYRel = 'P'; + const char triggerLRel = 'Q'; + const char triggerRRel = 'R'; + const char bumperLRel = 'S'; + const char bumperRRel = 'T'; + const char arrowL = 'U'; + const char arrowR = 'V'; + const char arrowU = 'W'; + const char arrowD = 'X'; + const char arrowRLRel = '0'; + const char leftJSRel = 'Y'; + const char rightJSRel = 'Z'; + const char rightJSPress = '7'; + const char rightJSPressRel = '8'; + const char homeVal = '4'; + const char homeValEE = '6'; + + const char J1 = '1'; + const char J2 = '2'; + const char J3 = '3'; + const char J4 = '4'; + const char J5 = '5'; + const char J6 = '6'; + // arm modes + const char jointMode = '1'; + const char IKMode = '2'; + const char drillMode = '3'; + + // joystick direction characters + const char left = 'L'; + const char right = 'R'; + const char up = 'U'; + const char down = 'D'; + const char wrist = 'W'; + const char garbage = 'G'; + const char open = 'O'; + const char close = 'C'; + + int num_joints_ = 6; + double ppr = 400.0; + double encppr = 512.0; + + bool homeFlag = false; + //char mode = jointMode + + // hardware interface communication variables + std::vector encPos, encCmd; + std::vector armCmd, armPos, encStepsPerDeg; + std::vector reductions{50, 161, 93.07, 44.8, 28, 14}; + + + + // timer variables + double refresh_rate_hz = 10.0; + ros::Timer arm_pos_timer; + + private: + ros::NodeHandle nh; + sb_msgs::armFirmware Arm; //most recent arm information + + + void armCMDPositionCallBack(const sb_msgs::ArmPosition::ConstPtr& cmd_msg); + void microComCallBack(const sb_msgs::armFirmware::ConstPtr& micro_msg); + void joyCallBack(const sensor_msgs::Joy::ConstPtr& joy_msg); + + + //void teensyFeedback(const ros::TimerEvent& e); + + ros::Subscriber sub_command_pos; + ros::Subscriber sub_obs_pos; //observed position + ros::Subscriber sub_joy; + ros::Publisher pub_cmd; + ros::Publisher pub_joint_cmd; + + + //user input + sensor_msgs::Joy joy; //controller + //input flags + //bool mode + + // ros::Timer feedbackLoop; + +}; +#endif // ARM_HARDWARE_DRIVER_MYNODE_H diff --git a/src/arm_hardware_driver/include/armHardwareDriver.h b/src/arm_hardware_driver/include/armHardwareDriver.h index bca2832ef..50afc1d07 100644 --- a/src/arm_hardware_driver/include/armHardwareDriver.h +++ b/src/arm_hardware_driver/include/armHardwareDriver.h @@ -10,7 +10,9 @@ // STD Includes #include -#include +#include +#include +#include // ROS Includes #include @@ -23,27 +25,24 @@ #include // Other -#include +#include +using std::string; +using std::exception; +using std::cout; +using std::cerr; +using std::endl; +using std::vector; + class ArmHardwareDriver { public: ArmHardwareDriver(ros::NodeHandle& nh); - void teensySerialCallback(const std_msgs::String::ConstPtr& inMsg); + void allControllerCallback(const std_msgs::String::ConstPtr& inMsg); void parseInput(std::string inMsg); void joint_space_motion(std::string inMsg); - void drill_motion(std::string inMsg); - void jointSpaceMove(const char joystick, const char dir); - void changeSpeed(const char dir); - void changeAxis(const char joystick); - void releaseAxis(const char joystick, const char dir); void endEffector(const char dir); void endEffectorRel(); - void prepareDrilling(); - void collectSample(); - void depositSample(); - void manualDrill(const char dir); - void releaseDrill(); void homeArm(); void cartesian_motion(std::string inMsg); void cartesian_moveit_move(std::vector& pos_commands, @@ -57,12 +56,16 @@ class ArmHardwareDriver { void recieveMsg(); void requestArmPosition(); void updateHWInterface(); - void requestEEFeedback(); - void requestJPFeedback(); void homeEE(); void axisRelease(const char axis); void axisMove(const char axis, const char dir); + //new serial + unsigned long baud = 9600; + string port = "/dev/ttyUSB0 Serial"; + + + // character representations of buttons for arm communication const char leftJSU = 'A'; const char leftJSD = 'B'; @@ -102,10 +105,10 @@ class ArmHardwareDriver { const char J4 = '4'; const char J5 = '5'; const char J6 = '6'; + // arm modes const char jointMode = '1'; const char IKMode = '2'; - const char drillMode = '3'; // joystick direction characters const char left = 'L'; @@ -121,34 +124,24 @@ class ArmHardwareDriver { double ppr = 400.0; double encppr = 512.0; - bool serialOpen = true; - bool dataInTransit = false; bool homeFlag = false; char mode = jointMode; // hardware interface communication variables std::vector encPos, encCmd; - std::vector armCmd, armPos, encStepsPerDeg; - std::vector reductions{50, 161, 93.07, 44.8, 57.34, 57.34}; - - // timer variables - double refresh_rate_hz = 10.0; - ros::Timer arm_pos_timer; + std::vector armCmd, armPos, poseCmd, encStepsPerDeg; + std::vector reductions{50.0, 160.0, 92.3077, 43.936, 57, 14}; private: ros::NodeHandle nh; void armPositionCallBack(const sb_msgs::ArmPosition::ConstPtr& cmd_msg); - void teensyFeedback(const ros::TimerEvent& e); + void poseSelectCallback(const sb_msgs::ArmPosition::ConstPtr& poseAngles); ros::Subscriber subPro; - ros::Subscriber sub_command_pos; - ros::Publisher pub_observed_pos; - ros::Timer feedbackLoop; - - // The SerialStream to/from the teensy - LibSerial::SerialPort teensy; + ros::Subscriber subPose; + ros::Subscriber subCmdPos; + ros::Publisher pubObservedPos; - // The Port the teensy is connected to - std::string port; + serial::Serial teensy; }; #endif // ARM_HARDWARE_DRIVER_MYNODE_H diff --git a/src/arm_hardware_driver/package.xml b/src/arm_hardware_driver/package.xml index 79f713ecf..1cb84d3eb 100644 --- a/src/arm_hardware_driver/package.xml +++ b/src/arm_hardware_driver/package.xml @@ -44,12 +44,14 @@ std_msgs sb_utils sb_msgs - libserial-dev - roscpp + serial + + serial + roscpp--> std_msgs sb_utils sb_msgs - libserial-dev + diff --git a/src/arm_hardware_driver/src/ROSDIRECTarmHardwareDriver.cpp b/src/arm_hardware_driver/src/ROSDIRECTarmHardwareDriver.cpp new file mode 100644 index 000000000..e1f9b0b58 --- /dev/null +++ b/src/arm_hardware_driver/src/ROSDIRECTarmHardwareDriver.cpp @@ -0,0 +1,356 @@ +/* + * Created By: Tate Kolton + * Created On: July 4, 2022 + * Description: This package receives info from /cmd_arm topic and publishes + * serial data via callback to be recieved by the arm MCU (Teensy 4.1) + */ + +#include "../include/ROSDIRECTarmHardwareDriver.h" + +ArmHardwareDriver::ArmHardwareDriver(ros::NodeHandle& nh) : nh(nh) { + // Setup NodeHandles + + ros::NodeHandle private_nh("~"); + ros::Rate loop_rate(10); //10 hz + + // Setup Subscribers + int queue_size_firm = 5; //firmware + //int queue_size_joy = 5; + int queue_size_joy = 5; + + + // subPro = nh.subscribe( + // "/cmd_arm", queue_size, &ArmHardwareDriver::teensySerialCallback, this); + // sub_command_pos = nh.subscribe( + //"/cmd_pos_arm", queue_size, &ArmHardwareDriver::armCMDPositionCallBack, this); + sub_obs_pos = nh.subscribe( + "/obs_pos_arm", queue_size_firm, &ArmHardwareDriver::microComCallBack, this); + sub_joy = nh.subscribe( + "/joy", queue_size_joy, &ArmHardwareDriver::joyCallBack, this); + + pub_cmd = + private_nh.advertise("cmd_pos_arm", queue_size_firm ); + + pub_joint_cmd = + private_nh.advertise("cmd_pos_arm", queue_size_firm ); + // Get Params + //SB_getParam( + // private_nh, "/hardware_driver/port", port, (std::string) "/dev/ttyACM0"); + + encCmd.resize(num_joints_); + armCmd.resize(num_joints_); + encStepsPerDeg.resize(num_joints_); + armPos.resize(num_joints_); + encPos.resize(num_joints_); + armCmd.resize(num_joints_); + + for (int i = 0; i < num_joints_; i++) { + encStepsPerDeg[i] = reductions[i] * ppr * 5.12 / 360.0; + } + +// while (ros::ok()) +// { +// /* code */ +// } + + //float feed_freq = 10.131; // not exactly 5 to ensure that this doesn't regularly interfere with HW interface callback + // ros::Duration feedbackFreq = ros::Duration(1.0/feed_freq); + //feedbackLoop = nh.createTimer(feedbackFreq, &ArmHardwareDriver::teensyFeedback, this); +loop_rate.sleep(); +ros::spinOnce(); +} + +// Callback for xbox360/ps4/pro controller messages via joy_node +void ArmHardwareDriver::joyCallBack(const sensor_msgs::Joy::ConstPtr& joy_msg){ + joy = *joy_msg; + + joy.buttons[3]; + + + newInput(); //this feels weird to call another function in a callback and store the input values as global variables. + +if(debug){ + ROS_INFO("controller input success"); +} +} + + + +// rosserial listener + void ArmHardwareDriver::microComCallBack(const sb_msgs::armFirmware::ConstPtr& micro_msg) { +Arm = *micro_msg; + +// parseInput(inMsg->data); + } + +void ArmHardwareDriver::newInput(){ +if(joy.buttons[3]){ + ROS_INFO("meep meep"); +} +} + +// void ArmHardwareDriver::parseInput(std::string inMsg) { +// mode = inMsg[0]; + +// if (mode == jointMode) { +// joint_space_motion(inMsg); +// } else if (mode == IKMode) { +// cartesian_motion(inMsg); +// } else if (mode == drillMode) { +// drill_motion(inMsg); +// } +// } + +// Sends joint space motion related commands to teensy +void ArmHardwareDriver::joint_space_motion(std::string inMsg) { + char action = inMsg[1]; + + // if(action == homeVal) { + // homeArm(); + // } else if(action == leftJSU) { + // axisMove(J3,up); + // } else if (action == leftJSD) { + // axisMove(J3, down); + // } else if (action == rightJSU) { + // axisMove(J2, up); + // } else if (action == rightJSD) { + // axisMove(J2, down); + // } else if (action == leftJSRel) { + // axisRelease(J3); + // } else if (action == rightJSRel) { + // axisRelease(J2); + // } else if (action == buttonY) { + // axisMove(J6, left); + // } else if (action == buttonA) { + // axisMove(J6, right); + // } else if (action == buttonB) { + // axisMove(J5, down); + // } else if (action == buttonX) { + // axisMove(J5, up); + // } else if (action == triggerL) { + // axisMove(J1, left); + // } else if (action == triggerR) { + // axisMove(J1, right); + // } else if ((action == triggerLRel) || (action == triggerRRel)) { + // axisRelease(J1); + // } else if (action == buttonARel) { + // axisRelease(J6); + // } else if (action == buttonBRel) { + // axisRelease(J5); + // } else if (action == buttonXRel) { + // axisRelease(J5); + // } else if (action == buttonYRel) { + // axisRelease(J6); + // } else if(action == bumperL) { + // axisMove(J4, left); + // } else if(action == bumperR) { + // axisMove(J4, right); + // } else if((action == bumperLRel) || (action == bumperRRel)) { + // axisRelease(J4); + // } else if (action == arrowL) { + // endEffector(open); + // } else if (action == arrowR) { + // endEffector(close); + // } else if (action == arrowRLRel) { + // endEffectorRel(); + // } else if(action == homeValEE) { + // homeEE(); + // } +} + +// void ArmHardwareDriver::cartesian_motion(std::string inMsg) { +// char action = inMsg[1]; + +// if (action == arrowL) { +// endEffector(open); +// } else if (action == arrowR) { +// endEffector(close); +// } else if (action == arrowRLRel) { +// endEffectorRel(); +// } else if(action == homeValEE) { +// homeEE(); +// } +// } + +// Sends drilling mode related commands to teensy +// void ArmHardwareDriver::drill_motion(std::string inMsg) { +// char action = inMsg.at(1); + +// if (action == buttonARel) { +// prepareDrilling(); +// } else if (action == buttonBRel) { +// collectSample(); +// } else if (action == buttonX) { +// depositSample(); +// } else if (action == triggerL) { +// manualDrill(left); +// } else if (action == triggerR) { +// manualDrill(right); +// } else if ((action == triggerLRel) || (action == triggerRRel)) { +// releaseDrill(); +// } +// // below two lines to be implemented once cartesian mode is sorted +// // case rightJSU: moveDrillUp(); break; +// // case rightJSD: moveDrillDown(); break; +// } + +// void ArmHardwareDriver::axisMove(const char axis, const char dir) +// { +// std::string outMsg = "JMM"; +// outMsg += axis; +// outMsg += dir; +// outMsg += "\n"; +// sendMsg(outMsg); +// } + +// void ArmHardwareDriver::axisRelease(const char axis) +// { +// std::string outMsg = "JMR"; +// outMsg += axis; +// outMsg += "\n"; +// sendMsg(outMsg); +// } + +// // End Effector Hardware Driver Functions +// void ArmHardwareDriver::endEffector(const char dir) { +// std::string outMsg = "EE"; +// outMsg += dir; +// outMsg += "\n"; +// sendMsg(outMsg); +// } + +// void ArmHardwareDriver::endEffectorRel() { +// std::string outMsg = "EER\n"; +// sendMsg(outMsg); +// } + +// // Drilling Mode Hardware Driver Functions +// void ArmHardwareDriver::prepareDrilling() { +// std::string outMsg = "DMP\n"; +// sendMsg(outMsg); +// } + +// void ArmHardwareDriver::collectSample() { +// std::string outMsg = "DMC\n"; +// sendMsg(outMsg); +// } + +// void ArmHardwareDriver::depositSample() { +// std::string outMsg = "DMD\n"; +// sendMsg(outMsg); +// } + +// void ArmHardwareDriver::manualDrill(const char dir) { +// std::string outMsg = "DMM"; +// outMsg += dir; +// outMsg += "\n"; +// sendMsg(outMsg); +// } + +// void ArmHardwareDriver::releaseDrill() { +// std::string outMsg = "DMMX"; +// outMsg += "\n"; +// sendMsg(outMsg); +// } + + void ArmHardwareDriver::homeArm() { + pub_cmd.pub(); + } + +// void ArmHardwareDriver::homeEE() { +// std::string outMsg = "EEH\n"; +// sendMsg(outMsg); +// } + +void ArmHardwareDriver::armCMDPositionCallBack(const sb_msgs::ArmPosition::ConstPtr& cmd_msg) { + + // armCmd.assign(commanded_msg->positions.begin(), + // commanded_msg->positions.end()); + // jointPosToEncSteps(armCmd, encCmd); + + // std::string outMsg = "MT"; + // for (int i = 0; i < num_joints_; ++i) { + // outMsg += 'A' + i; + // outMsg += std::to_string(encCmd[i]); + // } + // outMsg += "\n"; + // sendMsg(outMsg); + // recieveMsg(); +} + +// void ArmHardwareDriver::updateEncoderSteps(std::string msg) { +// size_t idx1 = msg.find("A", 2) + 1; +// size_t idx2 = msg.find("B", 2) + 1; +// size_t idx3 = msg.find("C", 2) + 1; +// size_t idx4 = msg.find("D", 2) + 1; +// size_t idx5 = msg.find("E", 2) + 1; +// size_t idx6 = msg.find("F", 2) + 1; +// size_t idx7 = msg.find("Z", 2) + 1; +// encPos[0] = std::stoi(msg.substr(idx1, idx2 - idx1)); +// encPos[1] = std::stoi(msg.substr(idx2, idx3 - idx2)); +// encPos[2] = std::stoi(msg.substr(idx3, idx4 - idx3)); +// encPos[3] = std::stoi(msg.substr(idx4, idx5 - idx4)); +// encPos[4] = std::stoi(msg.substr(idx5, idx6 - idx5)); +// encPos[5] = std::stoi(msg.substr(idx6, idx7 - idx6)); +// } + +// void ArmHardwareDriver::encStepsToJointPos( +// std::vector& enc_steps, std::vector& joint_positions) { +// for (int i = 0; i < enc_steps.size(); ++i) { +// // convert enc steps to joint deg +// joint_positions[i] = +// static_cast(enc_steps[i]) / encStepsPerDeg[i]; +// } +// } + +// void ArmHardwareDriver::jointPosToEncSteps(std::vector& joint_positions, +// std::vector& enc_steps) { +// for (int i = 0; i < joint_positions.size(); ++i) { +// // convert joint deg to enc steps +// enc_steps[i] = static_cast(joint_positions[i] * encStepsPerDeg[i]); +// } +// } + +// Libserial Implementation + + void ArmHardwareDriver::sendMsg(std::string outMsg) { +// // serialOpen = false; +// //dataInTransit = true; +// //teensy.Write(outMsg); + std_msgs::String out; + out.data = outMsg; + pub_joint_cmd.pub(out); +// //ROS_INFO("Sent via serial: %s", outMsg.c_str()); + } + +void ArmHardwareDriver::recieveMsg() { +// std::stringstream buffer; +// char next_char; +// do { +// teensy.ReadByte(next_char); //WAS teensy >> next_char; +// // ROS_INFO("next_char: %c", next_char); +// buffer << next_char; +// } while (next_char != 'Z'); +// std::string inMsg = buffer.str(); + +// // check if joint state is available +// if (inMsg.substr(0, 2) == "JP") { +// updateEncoderSteps(inMsg); +// encStepsToJointPos(encPos, armPos); +// // updateHWInterface(); +// // check if end effector force feedback is available +// } else if (inMsg.substr(0, 2) == "EE") +// ROS_INFO("%s", inMsg.c_str()); +// // check if homing is completed +// else if(inMsg.substr(0, 2) == "HC") +// { +// ROS_INFO("ARM CALIBRATION COMPLETE, NOW ACCEPTING CONTROLLER COMMANDS!"); +// } +} + + void ArmHardwareDriver::updateHWInterface() { +// // TODO: Ihsan fill in correct message implementation +// sb_msgs::ArmPosition outMsg; +// outMsg.positions.assign(armPos.begin(), armPos.end()); +// pub_observed_pos.publish(outMsg); + } diff --git a/src/arm_hardware_driver/src/ROSDIRECTarm_hardware_driver.cpp b/src/arm_hardware_driver/src/ROSDIRECTarm_hardware_driver.cpp new file mode 100644 index 000000000..5b36f73f6 --- /dev/null +++ b/src/arm_hardware_driver/src/ROSDIRECTarm_hardware_driver.cpp @@ -0,0 +1,29 @@ +/* + * Created By: Tate Kolton + * Created On: July 4, 2022 + * Description: Node for recieving messages from pro controller and relaying them to arm hardware driver module + */ + +#include "../include/armHardwareDriver.h" +#include + +int main(int argc, char** argv) { + + + // Setup your ROS node + std::string node_name = "arm_hardware_driver"; + ros::CallbackQueue ros_queue; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + nh.setCallbackQueue(&ros_queue); + + // Create an instance of your class + ArmHardwareDriver teensyComm(nh); + + // Start up ros. This will continue to run until the node is killed + ros::MultiThreadedSpinner spinner(0); + spinner.spin(&ros_queue); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/arm_hardware_driver/src/armHardwareDriver.cpp b/src/arm_hardware_driver/src/armHardwareDriver.cpp index dddd0f364..5a09f3456 100644 --- a/src/arm_hardware_driver/src/armHardwareDriver.cpp +++ b/src/arm_hardware_driver/src/armHardwareDriver.cpp @@ -13,22 +13,25 @@ ArmHardwareDriver::ArmHardwareDriver(ros::NodeHandle& nh) : nh(nh) { ros::NodeHandle private_nh("~"); // Setup Subscribers - int queue_size = 10; - - subPro = nh.subscribe( - "/cmd_arm", queue_size, &ArmHardwareDriver::teensySerialCallback, this); - sub_command_pos = nh.subscribe( - "/cmd_pos_arm", queue_size, &ArmHardwareDriver::armPositionCallBack, this); - pub_observed_pos = - private_nh.advertise("/observed_pos_arm", 1); - - // Get Params - SB_getParam( - private_nh, "/hardware_driver/port", port, (std::string) "/dev/ttyACM0"); - // Open the given serial port - teensy.Open(port); - teensy.SetBaudRate(LibSerial::BaudRate::BAUD_9600); - teensy.SetCharacterSize(LibSerial::CharacterSize::CHAR_SIZE_8); + int queue_size = 55; + + subPro = nh.subscribe( + "/cmd_arm", queue_size, &ArmHardwareDriver::allControllerCallback, this); + + subCmdPos = nh.subscribe( + "/cmd_pos_arm", queue_size, &ArmHardwareDriver::armPositionCallBack, this); + + subPose = private_nh.subscribe("/cmd_pose", 1, &ArmHardwareDriver::poseSelectCallback, this); + + pubObservedPos = private_nh.advertise("/observed_pos_arm", 1); + + + teensy.setBaudrate(115200); + teensy.setPort("/dev/ttyACM0"); + teensy.open(); + teensy.setDTR(false); + teensy.setRTS(false); + encCmd.resize(num_joints_); armCmd.resize(num_joints_); @@ -36,53 +39,19 @@ ArmHardwareDriver::ArmHardwareDriver(ros::NodeHandle& nh) : nh(nh) { armPos.resize(num_joints_); encPos.resize(num_joints_); armCmd.resize(num_joints_); + poseCmd.resize(num_joints_); for (int i = 0; i < num_joints_; i++) { encStepsPerDeg[i] = reductions[i] * ppr * 5.12 / 360.0; } - - float feed_freq = 10.131; // not exactly 5 to ensure that this doesn't regularly interfere with HW interface callback - ros::Duration feedbackFreq = ros::Duration(1.0/feed_freq); - feedbackLoop = nh.createTimer(feedbackFreq, &ArmHardwareDriver::teensyFeedback, this); - -} - -//Timer initiated event to request joint feedback -void ArmHardwareDriver::teensyFeedback(const ros::TimerEvent& e) -{ - - //ROS_INFO("timer working"); - /* - if(homeFlag) - { - */ - //requestEEFeedback(); - //if(mode == jointMode) - //{ - //requestJPFeedback(); - //} - // } -} - -void ArmHardwareDriver::requestEEFeedback() -{ - std::string outMsg = "FBE\n"; - sendMsg(outMsg); - recieveMsg(); -} - -void ArmHardwareDriver::requestJPFeedback() -{ - std::string outMsg = "FBJ\n"; - sendMsg(outMsg); - recieveMsg(); + } // Callback function to relay pro controller messages to teensy MCU on arm via -// rosserial -void ArmHardwareDriver::teensySerialCallback( -const std_msgs::String::ConstPtr& inMsg) { - parseInput(inMsg->data); +void ArmHardwareDriver::allControllerCallback(const std_msgs::String::ConstPtr& inMsg) { + std::string tmp = inMsg->data; + //ROS_INFO("%s", tmp.c_str()); + parseInput(tmp); } void ArmHardwareDriver::parseInput(std::string inMsg) { @@ -92,9 +61,7 @@ void ArmHardwareDriver::parseInput(std::string inMsg) { joint_space_motion(inMsg); } else if (mode == IKMode) { cartesian_motion(inMsg); - } else if (mode == drillMode) { - drill_motion(inMsg); - } + } } // Sends joint space motion related commands to teensy @@ -103,54 +70,80 @@ void ArmHardwareDriver::joint_space_motion(std::string inMsg) { if(action == homeVal) { homeArm(); + recieveMsg(); } else if(action == leftJSU) { axisMove(J3,up); + recieveMsg(); } else if (action == leftJSD) { axisMove(J3, down); + recieveMsg(); } else if (action == rightJSU) { axisMove(J2, up); + recieveMsg(); } else if (action == rightJSD) { axisMove(J2, down); + recieveMsg(); } else if (action == leftJSRel) { axisRelease(J3); + recieveMsg(); } else if (action == rightJSRel) { axisRelease(J2); + recieveMsg(); + } else if (action == buttonY) { + axisMove(J6, left); + recieveMsg(); } else if (action == buttonA) { - jointSpaceMove(wrist, up); + axisMove(J6, right); + recieveMsg(); } else if (action == buttonB) { - jointSpaceMove(wrist, left); + axisMove(J5, down); + recieveMsg(); } else if (action == buttonX) { - jointSpaceMove(wrist, right); - } else if (action == buttonY) { - jointSpaceMove(wrist, down); + axisMove(J5, up); + recieveMsg(); } else if (action == triggerL) { axisMove(J1, left); + recieveMsg(); } else if (action == triggerR) { axisMove(J1, right); + recieveMsg(); } else if ((action == triggerLRel) || (action == triggerRRel)) { axisRelease(J1); - } else if (action == buttonARel) { - releaseAxis(wrist, up); + recieveMsg(); + } else if (action == buttonARel) { + axisRelease(J6); + recieveMsg(); } else if (action == buttonBRel) { - releaseAxis(wrist, left); + axisRelease(J5); + recieveMsg(); } else if (action == buttonXRel) { - releaseAxis(wrist, right); + axisRelease(J5); + recieveMsg(); } else if (action == buttonYRel) { - releaseAxis(wrist, down); + axisRelease(J6); + recieveMsg(); } else if(action == bumperL) { axisMove(J4, left); + recieveMsg(); } else if(action == bumperR) { axisMove(J4, right); + recieveMsg(); } else if((action == bumperLRel) || (action == bumperRRel)) { axisRelease(J4); + recieveMsg(); } else if (action == arrowL) { endEffector(open); + recieveMsg(); } else if (action == arrowR) { endEffector(close); + recieveMsg(); } else if (action == arrowRLRel) { endEffectorRel(); + recieveMsg(); } else if(action == homeValEE) { homeEE(); + recieveMsg(); + } } @@ -166,42 +159,13 @@ void ArmHardwareDriver::cartesian_motion(std::string inMsg) { } else if(action == homeValEE) { homeEE(); } -} -// Sends drilling mode related commands to teensy -void ArmHardwareDriver::drill_motion(std::string inMsg) { - char action = inMsg.at(1); - - if (action == buttonARel) { - prepareDrilling(); - } else if (action == buttonBRel) { - collectSample(); - } else if (action == buttonX) { - depositSample(); - } else if (action == triggerL) { - manualDrill(left); - } else if (action == triggerR) { - manualDrill(right); - } else if ((action == triggerLRel) || (action == triggerRRel)) { - releaseDrill(); - } - // below two lines to be implemented once cartesian mode is sorted - // case rightJSU: moveDrillUp(); break; - // case rightJSD: moveDrillDown(); break; -} - -void ArmHardwareDriver::jointSpaceMove(const char joystick, const char dir) { - std::string outMsg = "JM"; - outMsg += "M"; - outMsg += joystick; - outMsg += dir; - outMsg += "\n"; - sendMsg(outMsg); + recieveMsg(); } void ArmHardwareDriver::axisMove(const char axis, const char dir) { - std::string outMsg = "JMT"; + std::string outMsg = "JMM"; outMsg += axis; outMsg += dir; outMsg += "\n"; @@ -210,37 +174,12 @@ void ArmHardwareDriver::axisMove(const char axis, const char dir) void ArmHardwareDriver::axisRelease(const char axis) { - std::string outMsg = "JMW"; + std::string outMsg = "JMR"; outMsg += axis; outMsg += "\n"; sendMsg(outMsg); } -void ArmHardwareDriver::changeSpeed(const char dir) { - //std::string outMsg = "JM"; - //outMsg = "S"; - //outMsg += dir; - //outMsg += "\n"; - //sendMsg(outMsg); -} - -void ArmHardwareDriver::changeAxis(const char joystick) { - std::string outMsg = "JM"; - outMsg += "A"; - outMsg += joystick; - outMsg += "\n"; - sendMsg(outMsg); -} - -void ArmHardwareDriver::releaseAxis(const char joystick, const char dir) { - std::string outMsg = "JM"; - outMsg += "R"; - outMsg += joystick; - outMsg += dir; - outMsg += "\n"; - sendMsg(outMsg); -} - // End Effector Hardware Driver Functions void ArmHardwareDriver::endEffector(const char dir) { std::string outMsg = "EE"; @@ -254,51 +193,36 @@ void ArmHardwareDriver::endEffectorRel() { sendMsg(outMsg); } -// Drilling Mode Hardware Driver Functions -void ArmHardwareDriver::prepareDrilling() { - std::string outMsg = "DMP\n"; - sendMsg(outMsg); -} - -void ArmHardwareDriver::collectSample() { - std::string outMsg = "DMC\n"; +void ArmHardwareDriver::homeArm() { + std::string outMsg = "HM\n"; sendMsg(outMsg); -} -void ArmHardwareDriver::depositSample() { - std::string outMsg = "DMD\n"; - sendMsg(outMsg); } -void ArmHardwareDriver::manualDrill(const char dir) { - std::string outMsg = "DMM"; - outMsg += dir; - outMsg += "\n"; +void ArmHardwareDriver::homeEE() { + std::string outMsg = "EEH\n"; sendMsg(outMsg); } -void ArmHardwareDriver::releaseDrill() { - std::string outMsg = "DMMX"; - outMsg += "\n"; - sendMsg(outMsg); -} +void ArmHardwareDriver::poseSelectCallback( +const sb_msgs::ArmPosition::ConstPtr& poseAngles) { + poseCmd.assign(poseAngles->positions.begin(), poseAngles->positions.end()); + jointPosToEncSteps(poseCmd, encCmd); + + std::string outMsg = "PM"; + for(int i=0; i < num_joints_; i++) { + outMsg += 'A' + i; + outMsg += std::to_string(encCmd[i]); + } -void ArmHardwareDriver::homeArm() { - std::string outMsg = "HM\n"; - homeFlag = false; + outMsg += "/n"; sendMsg(outMsg); recieveMsg(); - homeFlag = true; -} - -void ArmHardwareDriver::homeEE() { - std::string outMsg = "EEH\n"; - sendMsg(outMsg); } void ArmHardwareDriver::armPositionCallBack( const sb_msgs::ArmPosition::ConstPtr& commanded_msg) { - // TODO: ihsan fill std::vector type with sb_msgs values + armCmd.assign(commanded_msg->positions.begin(), commanded_msg->positions.end()); jointPosToEncSteps(armCmd, encCmd); @@ -350,57 +274,58 @@ void ArmHardwareDriver::jointPosToEncSteps(std::vector& joint_positions, void ArmHardwareDriver::sendMsg(std::string outMsg) { // Send everything in outMsg through serial port - /* - if(serialOpen) - { - */ - // close serial port to other processes - serialOpen = false; - dataInTransit = true; - teensy.Write(outMsg); - // } + //ROS_INFO("attempting send"); + teensy.write(outMsg); ROS_INFO("Sent via serial: %s", outMsg.c_str()); } void ArmHardwareDriver::recieveMsg() { - // fill inMsg string with whatever comes through serial port until \n - /* - if(dataInTransit) - { - */ - std::stringstream buffer; - char next_char; - do { - teensy.WriteByte(next_char); - // ROS_INFO("next_char: %c", next_char); - buffer << next_char; - } while (next_char != 'Z'); - std::string inMsg = buffer.str(); - - // check if joint state is available - if (inMsg.substr(0, 2) == "JP") { - updateEncoderSteps(inMsg); - encStepsToJointPos(encPos, armPos); - // updateHWInterface(); - // check if end effector force feedback is available - } else if (inMsg.substr(0, 2) == "EE") - ROS_INFO("%s", inMsg.c_str()); - // check if homing is completed - else if(inMsg.substr(0, 2) == "HC") - { - ROS_INFO("ARM CALIBRATION COMPLETE, NOW ACCEPTING CONTROLLER COMMANDS!"); - } - // open serial port to other processes - serialOpen = true; - dataInTransit = false; - /* - } - */ + + std::string next_char = ""; + std::string buffer = ""; + int timeoutCounter = 0; + do { + // teensy.ReadByte(next_char); //WAS teensy >> next_char; + // // ROS_INFO("next_char: %c", next_char); + // buffer << next_char; + // } while (next_char != 'Z'); + // std::string inMsg = buffer.str(); + + // // check if joint state is available + // if (inMsg.substr(0, 2) == "JP") { + // updateEncoderSteps(inMsg); + // encStepsToJointPos(encPos, armPos); + // // updateHWInterface(); + // // check if end effector force feedback is available + // } else if (inMsg.substr(0, 2) == "EE") + // ROS_INFO("%s", inMsg.c_str()); + // // check if homing is completed + // else if(inMsg.substr(0, 2) == "HC") + // { + // ROS_INFO("ARM CALIBRATION COMPLETE, NOW ACCEPTING CONTROLLER COMMANDS!"); + // } + timeoutCounter ++; + next_char = teensy.read(); + buffer += next_char; + // if(timeoutCounter > 50){ + // ROS_INFO("timed out"); + // next_char = "Z"; + // } + } while (next_char != "Z"); + + ROS_INFO("buffer: %s", buffer.c_str()); + + + // // Update parameters based on feedback + updateEncoderSteps(buffer); + encStepsToJointPos(encPos, armPos); + updateHWInterface(); + } void ArmHardwareDriver::updateHWInterface() { - // TODO: Ihsan fill in correct message implementation sb_msgs::ArmPosition outMsg; outMsg.positions.assign(armPos.begin(), armPos.end()); - pub_observed_pos.publish(outMsg); + pubObservedPos.publish(outMsg); } + diff --git a/src/arm_hardware_driver/src/arm_hardware_driver.cpp b/src/arm_hardware_driver/src/arm_hardware_driver.cpp index 5b36f73f6..d509b19d7 100644 --- a/src/arm_hardware_driver/src/arm_hardware_driver.cpp +++ b/src/arm_hardware_driver/src/arm_hardware_driver.cpp @@ -21,6 +21,7 @@ int main(int argc, char** argv) { ArmHardwareDriver teensyComm(nh); // Start up ros. This will continue to run until the node is killed + ros::MultiThreadedSpinner spinner(0); spinner.spin(&ros_queue); diff --git a/src/arm_hardware_interface/launch/arm_hardware_bringup_ros_direct.launch b/src/arm_hardware_interface/launch/arm_hardware_bringup_ros_direct.launch new file mode 100644 index 000000000..9ba24c296 --- /dev/null +++ b/src/arm_hardware_interface/launch/arm_hardware_bringup_ros_direct.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/firmware/ROSarmFirmware/ROSarmFirmware.h b/src/firmware/ROSarmFirmware/ROSarmFirmware.h new file mode 100644 index 000000000..a5470ce62 --- /dev/null +++ b/src/firmware/ROSarmFirmware/ROSarmFirmware.h @@ -0,0 +1,283 @@ +//#include "sb_msgs/ArmPosition.h" +//#include "ros/subscriber.h" +/* +Created By: Tate Kolton, Graeme Dockrill, Rowan Zawadzki +Created On: November 11, 2022 +Updated On: January 10, 2023 +Updated On: January 23, 2023 for direct ROS integration and xbox/ps4/nintendo controllers +Description: Header file for firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU with direct ros integration +*/ + +#include +#include +#include +#include +#include +#include +#include //may cause errors, if this file exhists, keep spammig upload/verify until arduino finds it +//#include 32 would be more efficient, if performance drops, change arm msg to float32 +// general parameters +#define SIM 1 //firmware simulation, disables encoders and feeds back only stepper positions, works with only a microcontroller connected. + +#define NUM_AXES 6 +#define NUM_PARAMS 7 +#define ON 0 +#define OFF 1 +#define SW_ON 0 +#define SW_OFF 1 +#define FWD 1 +#define REV 0 + + +//ros declarations +ros::NodeHandle nh; +std_msgs::Int16 beat; +sb_msgs::armFirmware CMDangles; +sb_msgs::armFirmware OBSangles; +//std_msgs::Float64MultiArray OBSangles; + + int spinTEMP = millis(); +int spinINTERVAL = 20; //base speed of ros +int beatTEMP = millis(); +int beatINTERVAL = 1000; //ms +int posTEMP = millis(); +int posINTERVAL = 100; //ms + +ros::Publisher heart("/heartbeat", &beat); +ros::Publisher posPub("/obs_arm_pos", &OBSangles); + + + + +static const char release = 'R'; +static const char move = 'M'; +static const char change = 'A'; +static const char speed = 'S'; +static const char right = 'R'; +static const char left = 'L'; +static const char up = 'U'; +static const char down = 'D'; +static const char wrist = 'W'; +static const char garbage = 'G'; +static const char faster = 'U'; +static const char slower = 'D'; +static const char prepare = 'P'; +static const char collect = 'C'; +static const char deposit = 'D'; +static const char manual = 'M'; +static const char drillRelease = 'X'; +static const char open = 'O'; +static const char close = 'C'; +static const char joint = 'J'; +static const char EEval = 'E'; +static const char homeValEE = 'H'; + +// Motor pins +int stepPins[6] = {6, 8, 2, 10, 12, 25}; +int dirPins[6] = {5, 7, 1, 9, 11, 24}; + +// Encoder pins +int encPinA[6] = {17, 38, 36, 40, 15, 13}; +int encPinB[6] = {16, 37, 35, 39, 14, 41}; + +// limit switch pins +int limPins[6] = {18, 19, 20, 21, 22, 23}; + +// pulses per revolution for motors +long ppr[6] = {400, 400, 400, 400, 400, 400}; + +// Gear Reductions +float red[6] = {50.0, 161.0, 44.8, 93.07, 28, 14}; + +// End effector variables +const float calibrationFactor = -111.25; +float force; +HX711 scale; +const int dataPin = 34; +const int clkPin = 33; +int calPos = 0; +int closePos = 0; +int openPos = 500; +int EEstepPin = 4; +int EEdirPin = 3; +int speedEE = 100; +int accEE = 500; +int speedDrill = 3000; +int accDrill = 1000; +const int MOTOR_DIR_EE = 1; +const int openButton = 5; +const int closeButton = 4; +const float calForce = 0.3; +const float maxForce = 10.0; +float EEforce; +int forcePct = 0; + +// Encoder Variables +int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; +int pprEnc = 512; +int ENC_DIR[6] = {-1, -1, -1, -1, 1, 1}; +const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12}; +float ENC_STEPS_PER_DEG[NUM_AXES]; + +// Motor speeds and accelerations +int maxSpeed[8] = {1200, 1800, 3000, 2500, 2200, 2200}; +int maxAccel[8] = {1300, 3500, 4600, 3300, 5000, 5000}; +int homeSpeed[8] = {300, 1000, 1000, 400, 2000, 2000}; +int homeAccel[8] = {500, 2000, 1500, 1000, 1500, 1500}; + +// Time variables +const unsigned long readInterval = 10; +unsigned long currentTime; +unsigned long currentTimeJP; +unsigned long currentTimeEE; +unsigned long previousTime = 0; +unsigned long previousTimeEE = 0; +const unsigned long timeIntervalEE = 500; +const unsigned long timeIntervalJP = 250; +unsigned long previousTimeJP = 0; + +// Stepper motor objects for AccelStepper library +AccelStepper steppers[6]; +AccelStepper endEff(1, EEstepPin, EEdirPin); +AccelStepper steppersIK[6]; + +// Encoder objects for Encoder library +Encoder enc1(encPinA[0], encPinB[0]); +Encoder enc2(encPinA[1], encPinB[1]); +Encoder enc3(encPinA[2], encPinB[2]); +Encoder enc4(encPinA[3], encPinB[3]); +Encoder enc5(encPinA[4], encPinB[4]); +Encoder enc6(encPinA[5], encPinB[5]); + +// General Global Variable declarations + +int axisDir[6] = {1, -1, 1, -1, 1, 1}; +int axisDirIK[6] = {-1, -1, -1, 1, -1, -1}; +int runFlags[] = {0, 0, 0, 0, 0, 0}; +int currentAxis = 1; +int i; +bool initFlag = false; +bool jointFlag = false; +bool IKFlag = false; +bool resetEE = false; +bool vertFlag = false; +bool horizFlag = false; + +// Variables for homing / arm calibration +long homePosConst = -99000; +long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst}; +long homeCompAngles[] = {54, 10, 90, 10, 90, 170}; +long homeCompConst[] = {500, 2000, 1000, 500, 500, 500}; +long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5]}; +long homeCompSteps[] = {axisDir[0]*homeCompAngles[0]*red[0]*ppr[0]/360.0, axisDir[1]*homeCompAngles[1]*red[1]*ppr[1]/360.0, axisDir[2]*homeCompAngles[2]*red[2]*ppr[2]/360.0, axisDir[3]*homeCompAngles[3]*red[3]*ppr[3]/360.0, axisDir[4]*homeCompAngles[4]*red[4]*ppr[4]/360.0, axisDir[5]*homeCompAngles[5]*red[5]*ppr[5]/360.0}; + +// Range of motion (degrees) for each axis +int maxAngles[6] = {160, 160, 180, 120, 180, 340}; +long max_steps[] = {axisDir[0]*red[0]*maxAngles[0]/360.0*ppr[0], axisDir[1]*red[1]*maxAngles[1]/360.0*ppr[1], axisDir[2]*red[2]*maxAngles[2]/360.0*ppr[2], axisDir[3]*red[3]*maxAngles[3]/360.0*ppr[3], red[4]*maxAngles[4]/360.0*ppr[4], red[5]*maxAngles[5]/360.0*ppr[5]}; +long min_steps[NUM_AXES]; +char value; + +// Values for changing speed +const int maxSpeedIndex = 2; +int speedVals[maxSpeedIndex+1][NUM_AXES] = {{600, 900, 1500, 1250, 1050, 1050}, {900, 1200, 2000, 1665, 1460, 1460}, {900, 1600, 2500, 2200, 2000, 2000}}; +int speedIndex = maxSpeedIndex; + +// Cartesian mode speed settings +float IKspeeds[] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2}; +float IKaccs[] = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; + + +//****// FUNCTION PROTOTYPES //****// + +// Waits for a message to be published to the serial port. +void recieveCommand(); + +// Parses the message passed to it and determines which subfunction to call for control of arm. +void parseMessage(String inMsg); + +// Takes a character and prints it to the serial port. +void sendMessage(char outChar); + +// If the arm is in joint mode, the encoder positions are read and sent over serial. +void sendFeedback(String inMsg); + + //For closed loop control. Reads and sends current position. Sets new position based on feedback while the arm is in cartesian mode. +void cartesianCommands(String inMsg); + +// Sets the max speed and acceleration for each joint and motor to its Inverse Kinematic speed. +void setCartesianSpeed(); + +// Parses which commands to execute when in joint space mode. +void jointCommands(String inMsg); + +// Sets the max speed and acceleration for each joint and motor to its Joint Mode speed. +void setJointSpeed(); + +// Takes in the axis to move and the direction, and calls moveAxis in FWD or REV. +void moveArm(char axis, char dir); + +// Stops the passed axis and sets its run flag to 0. +void relArm(char axis); + +// Changes the axis target position and sets run flag to 1 or -1 depending on dir. +void moveAxis(int axis, int dir); + +// Opens, closes, releases, or homes the end effector depending on the data in inMsg [2]. +void endEffectorCommands(String inMsg); + +// Measures the end effector force. +void getEEForce(); + +// Prints end effector force to serial. +void sendEEForce(); + +// Selects which drill function to execute based on inMsg[2]. +void drillCommands(String inMsg); + +// Manually spins drill based on user input. +void manualDrill(char dir); + +// Stops the drill. +void stopDrill(); + +// Moves drill to max travel. +void spinDrill(); + +// Not yet implemented +void collectSample(); + +// Not yet implemented. +void depositSample(); + +// Prints the current encoder position for each axis to serial. +void sendCurrentPosition(); + +// Reads each encoder's position and stores them in encPos[]. +void readEncPos(int* encPos); + +// Zeroes each encoder at its current position. +void zeroEncoders(); + +// For closed loop movement (Inverse Kin Mode) +void cmdArm(); + +// changes speed of all axes based on user input. +void changeSpeed(char speedVal); + +// Main function for homing the entire arm. +void homeArm(); + +// Runs through and checks if each axis has reached its limit switch, then runs it a couple steps away from switch for 0 position. +void homeAxes(); + +// sets homing speed and acceleration for axes 1-6 and sets target homing position. +void initializeHomingMotion(); + +// sets main program speeds for each axis. +void initializeMotion(); + +// Runs all stepper motors to target position (if no target defined, motors will not move). +void runSteppers(); + +// Waits for the user to press the home button before continuing with other functions. Reads serial port and homes the arm if "HM___" is read. +void waitForHome(); \ No newline at end of file diff --git a/src/firmware/ROSarmFirmware/ROSarmFirmware.ino b/src/firmware/ROSarmFirmware/ROSarmFirmware.ino new file mode 100644 index 000000000..5b4d6f132 --- /dev/null +++ b/src/firmware/ROSarmFirmware/ROSarmFirmware.ino @@ -0,0 +1,546 @@ +/* +Created By: Tate Kolton, Graeme Dockrill +Created On: December 21, 2021 +Updated On: January 10, 2023 +Description: Main firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU +Major Edits on January 23, 2023 to make combatible with joy_node and direct ros integration +*/ + +// header file with all constants defined and libraries included +#include "ROSarmFirmware.h" + +void CmdCallback(const sb_msgs::ArmFirmware& msg) { + OBScmd = msg; +} + + +// setup function to initialize pins and provide initial homing to the arm. +void setup() { + + Serial.begin(115200); + nh.getHardware()->setBaud(115200); + nh.initNode(); + nh.advertise(heart); + nh.advertise(posPub); + ros::Subscriber cmdSub("cmd_pos_arm", &CmdCallback); + //ros::Rate loop_rate(10); + nh.subscribe(cmdSub); + nh.negotiateTopics(); + + OBSangles.positions_length = 6; + + + for (int i = 0; i < NUM_AXES; i++) { + ENC_STEPS_PER_DEG[i] = ppr[i] * red[i] * (ENC_MULT[i] / 360.0); + + min_steps[i] = -homeCompSteps[i]; + max_steps[i] = max_steps[i] - homeCompSteps[i]; + } + + min_steps[0] = -max_steps[0]; + + // initializes end effector motor + pinMode(EEstepPin, OUTPUT); + pinMode(EEdirPin, OUTPUT); + endEff.setMinPulseWidth(200); + endEff.setMaxSpeed(speedEE); + endEff.setAcceleration(accEE); + endEff.setCurrentPosition(1000); + + // initializes step pins, direction pins, limit switch pins, and stepper motor objects for accelStepper library + for (i = 0; i < NUM_AXES; i++) { + pinMode(dirPins[i], OUTPUT); + pinMode(stepPins[i], OUTPUT); + pinMode(limPins[i], INPUT_PULLUP); + steppers[i] = AccelStepper(1, stepPins[i], dirPins[i]); + steppers[i].setMinPulseWidth(200); + } + + // waits for user to press "home" button before rest of functions are available + + //waitForHome(); + //nh.spinOnce(); +} +// main program loop +void loop() { + // receives command from serial and executes accoringly + //recieveCommand(); + + + // run steppers to target position + runSteppers(); + + //heartbeat for ros + if (millis() - beatTEMP > beatINTERVAL) { + beat.data += 1; + beatTEMP = millis(); //would it be better to only call millis() once and add the interval onto spinTEMP? + heart.publish(&beat); + sendCurrentPosition(); + } + if (millis() - spinTEMP > spinINTERVAL) { + spinTEMP = millis(); + + nh.spinOnce(); + } +} + +void recieveCommand() { + String inData = ""; + char recieved = '\0'; + + // if a message is present, copy it to inData + if (Serial.available() > 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); + } + + // parse the received data + if (recieved == '\n') { + + parseMessage(inData); + } +} + +void CmdCallback(sb_msgs::ArmPosition msg) { //recieve command + CMDangles = msg; +} +void sendCurrentPosition() { + int posdata; + if (!SIM) { + + } else { + float data[6] = { 1.9, 2.4, 33.4, 4.87, 5.45, 6.34 }; + for (int i = 0; i < NUM_AXES; i++) { + //OBSangles.positions = data;//steppers[i].currentPosition(); + } + posPub.publish(&OBSangles); + } +} + +void sendFeedback(String inMsg) { + char function = inMsg[2]; + + // if in joint mode, send feedback over serial + if (function == joint) { + readEncPos(curEncSteps); + sendCurrentPosition(); + } +} + +void runSteppers() { + + for (i = 0; i < NUM_AXES; i++) { + steppers[i].run(); + } + + // endEff.run(); +} + +//****//JOINT MODE FUNCTIONS//****// + +// Parses which commands to execute when in joint space mode +void jointCommands(String inMsg) { + char function = inMsg[2]; + char detail1 = inMsg[3]; + + if (!jointFlag) // Check if we are switching from cartesian mode + { + IKFlag = false; + jointFlag = true; + cartesianToJoint(); + setJointSpeed(); + } + + if (function == move) + moveArm(detail1, inMsg[4]); + + else if (function == release) + relArm(detail1); +} + +void moveArm(char axis, char dir) { + int axisNum = String(axis).toInt(); // String to int for math + + if ((dir == left) || (dir == up)) { + moveAxis((axisNum - 1), FWD); + } else if ((dir == right) || (dir == down)) { + moveAxis((axisNum - 1), REV); + } +} + +void relArm(char axis) { + int axisNum = String(axis).toInt(); // String to int for math + steppers[axisNum - 1].stop(); + runFlags[axis] = 0; +} + +void moveAxis(int axis, int dir) { + if ((axis == 0) || (axis == 1)) { // switching direction if axis 1 or 2 (arm moves in intuitive dir) + dir = !dir; + } + + if (dir == FWD && runFlags[axis] != 1) { // sets stepper to move to max steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(max_steps[axis]); + runFlags[axis] = 1; + } else if (dir == REV && runFlags[axis] != -1) { // sets stepper to move to min steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(min_steps[axis]); + runFlags[axis] = -1; + } +} + +// sets Joint Mode speeds after switching out of cartesian mode +void setJointSpeed() { + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setMaxSpeed(speedVals[maxSpeedIndex][i]); + steppers[i].setAcceleration(maxAccel[i]); + } +} + +void cartesianToJoint() { + readEncPos(curEncSteps); + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setCurrentPosition(curEncSteps[i] / ENC_MULT[i]); + } +} + +//***//CARTESIAN MODE FUNCTIONS//***// + +void cartesianCommands(String inMsg) { + + if (jointFlag) { + IKFlag = true; + jointFlag = false; + setCartesianSpeed(); + } + // read current joint positions + readEncPos(curEncSteps); + + // update host with joint positions + sendCurrentPosition(); + + // get new position commands + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt(); + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt(); + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt(); + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt(); + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt(); + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt(); + + // update target joint positions + readEncPos(curEncSteps); + cmdArm(); +} + +void setCartesianSpeed() { + float JOINT_MAX_SPEED[NUM_AXES]; + float MOTOR_MAX_SPEED[NUM_AXES]; + float JOINT_MAX_ACCEL[NUM_AXES]; + float MOTOR_MAX_ACCEL[NUM_AXES]; + + for (int i = 0; i < NUM_AXES; i++) { + JOINT_MAX_SPEED[i] = IKspeeds[i] * (180.0 / 3.14159); + JOINT_MAX_ACCEL[i] = IKaccs[i] * (180.0 / 3.14159); + MOTOR_MAX_SPEED[i] = JOINT_MAX_SPEED[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + MOTOR_MAX_ACCEL[i] = JOINT_MAX_ACCEL[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + steppers[i].setAcceleration(MOTOR_MAX_ACCEL[i]); + steppers[i].setMaxSpeed(MOTOR_MAX_SPEED[i]); + } +} + + +//***// END EFFECTOR RELATED FUNCTIONS //***// + +void endEffectorCommands(String inMsg) { + // char data = inMsg[2]; + + // //opening code + // if (data == open) { //check if open button pressed and if force is less than max + // endEff.moveTo(openPos * MOTOR_DIR_EE); //continue to move to open position + // } + + // //closing code + // else if (data == close) { //check if open button pressed and if force is less than max + // endEff.moveTo(closePos * MOTOR_DIR_EE); //continue to move to closed position + // } + + // else if (data == release) { //else check if release button pressed + // endEff.stop(); // stop when above condition reached + // } + + // else if (data == homeValEE) { + // if (resetEE) { + // endEff.setCurrentPosition(1000); + // resetEE = false; + // } + + // else { + // endEff.setCurrentPosition(0); + // resetEE = true; + // } + // } +} + +void getEEForce() { + // if (scale.wait_ready_timeout(1)) { + // float force = scale.get_units() / 1000 * 9.81; + // forcePct = force * 100.0 / maxForce; + // } +} + +void sendEEForce() { + // String force_value = String(forcePct); + // String force_message = String("EE: Gripper Force: ") + String(force_value) + String(" Z"); + // Serial.print(force_message); +} + +//***// DRILL RELATED FUNCTIONS //***// + +void drillCommands(String inMsg) { + // char function = inMsg[2]; + + // if (function == manual) + // manualDrill(inMsg[3]); + // else if (function == drillRelease) + // stopDrill(); + // else if (function == prepare) + // spinDrill(); + // else if (function == collect) + // stopDrill(); + // else if (function == deposit) + // depositSample(); +} + +void manualDrill(char dir) { + if (dir == left) { + endEff.move(99000); + } + + else { + endEff.move(-99000); + } +} + +void stopDrill() { + endEff.stop(); +} + +void spinDrill() { + endEff.move(99000); +} + +void collectSample() { + // not currently implemented +} + +void depositSample() { + // not currently implemented +} + +// Set target position for cartesian movements of arm +void cmdArm() { + for (int i = 0; i < NUM_AXES; i++) { + int diffEncSteps = cmdEncSteps[i] - curEncSteps[i]; + if (abs(diffEncSteps) > 5) { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < ppr[i]) { + diffMotSteps = diffMotSteps / 2; + } + + steppers[i].move(diffMotSteps * axisDirIK[i]); + } + } +} + +//***//ENCODER RELATED FUNCTIONS//***// + +void readEncPos(int* encPos) { + encPos[0] = enc1.read() * ENC_DIR[0]; + encPos[1] = enc2.read() * ENC_DIR[1]; + encPos[2] = enc3.read() * ENC_DIR[2]; + encPos[3] = enc4.read() * ENC_DIR[3]; + encPos[4] = enc5.read() * ENC_DIR[4]; + encPos[5] = enc6.read() * ENC_DIR[5]; +} + +void zeroEncoders() { + enc1.write(0); + enc2.write(0); + enc3.write(0); + enc4.write(0); + enc5.write(0); + enc6.write(0); + + readEncPos(curEncSteps); +} + +//****// ARM CALIBRATION FUNCTIONS//****// + +void homeArm() { // main function for full arm homing + initializeHomingMotion(); + homeAxes(); + initializeMotion(); + zeroEncoders(); + + // clear serial port in case of garbage values + while (Serial.available() != 0) { + Serial.read(); + } + + // notify hardware driver that homing has completed + Serial.print("HCZ"); +} + +// Runs through and checks if each axis has reached its limit switch, then runs it to specified home position +void homeAxes() { + bool stopFlags[6] = { false, false, false, false, false, false }; + bool finishFlags[6] = { false, false, false, false, false, false }; + bool completeFlag = false; + int count = 0; + + while (!completeFlag) { + if (!SIM) { + for (i = 0; i < NUM_AXES; i++) { + + if (!finishFlags[i]) { + + if (!digitalRead(limPins[i]) && !stopFlags[i]) { + steppers[i].run(); + } + + else if (!stopFlags[i]) { + steppers[i].setCurrentPosition(-homeComp[i]); + steppers[i].stop(); + steppers[i].setMaxSpeed(maxSpeed[i] / 2); + steppers[i].setAcceleration(homeAccel[i]); + steppers[i].moveTo(homeCompSteps[i]); + stopFlags[i] = true; + } + + else if (steppers[i].distanceToGo() != 0) { + steppers[i].run(); + } + + else { + finishFlags[i] = true; + count++; + } + } + } + } else { + completeFlag = true; + } + if (count == NUM_AXES) { + completeFlag = true; + } + } +} + +void initializeHomingMotion() { // sets homing speed and acceleration and sets target homing position + + for (i = 0; i < NUM_AXES; i++) { + + steppers[i].setMaxSpeed(homeSpeed[i]); + steppers[i].setAcceleration(homeAccel[i]); + steppers[i].setCurrentPosition(0); + + if (i == 0) // Special case for axis 1 + { + if ((axisDir[0] * steppers[0].currentPosition() < -homeCompSteps[0])) { + steppers[0].move(-homePos[i]); + } else { + steppers[0].move(homePos[i]); + } + } + + else { + steppers[i].move(homePos[i]); + } + } +} + + +void initializeMotion() { // sets main program speeds for each axis and zeros position + + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setMaxSpeed(speedVals[maxSpeedIndex][i]); + steppers[i].setAcceleration(maxAccel[i]); + steppers[i].setCurrentPosition(0); + } +} + +void waitForHome() { + String inData; + char recieved = '\0'; + + while (!initFlag) { + inData = ""; + if (Serial.available() > 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); + } + + if (recieved == '\n') { + if (inData.substring(0, 2) == "HM") + homeArm(); + initFlag = true; + } + } +} + + +// OLD FUNCTIONS THAT MAY BE USED IN THE FIRMWARE // + +//void homeEE() +//{ +// EEforce=scale.get_units()/1000*9.81; +// +// // target position for end effector in closed direction +// endEff.move(-99000*MOTOR_DIR_EE); +// +// while(abs(EEforce) < calForce) +// { +// if (scale.wait_ready_timeout(1)) { +// EEforce=scale.get_units()/1000*9.81; //converting mass to force +// // close end effector +// } +// endEff.run(); +// } +// +// endEff.setCurrentPosition(-30); +// endEff.moveTo(openPos*MOTOR_DIR_EE); +// while(endEff.distanceToGo() != 0) +// { +// endEff.run(); +// } +//} + +// void changeSpeed(char speedVal) { // changes speed of all axes based on user input + +// if(speedVal == faster){ +// if(speedIndex < maxSpeedIndex) { +// speedIndex++; +// for(i=0;i 0) { +// speedIndex--; +// for(i=0;i +#include +#include +#include +#include +#include +#include + +//for if no arm is connected, simulation only needed +#define simOnly 1 +// general parameters +#define NUM_AXES 6 +#define NUM_AXES_EX_WRIST 4 +#define NUM_AXES_EFF 8 +#define NUM_PARAMS 7 +#define ON 0 +#define OFF 1 +#define SW_ON 0 +#define SW_OFF 1 +#define FWD 1 +#define REV 0 + +//ros stuff +ros::NodeHandle nh; +std_msgs::Int16 beat; +sb_msgs::ArmPosition INangles; +sb_msgs::ArmPosition OBSangles; +//node slowdowns, to relieve serial processing +int beatTEMP = millis(); +int beatINTERVAL = 1000; //ms +int posTEMP = millis(); +int posINTERVAL = 100; //ms + + +ros::Publisher heart("/heartbeat", &beat, 100); +ros::Publisher observer("/observed_arm_pos", &OBSangles, 100); + + + + + +static const char release = 'R'; +static const char move = 'M'; +static const char change = 'A'; +static const char speed = 'S'; +static const char right = 'R'; +static const char left = 'L'; +static const char up = 'U'; +static const char down = 'D'; +static const char wrist = 'W'; +static const char garbage = 'G'; +static const char faster = 'U'; +static const char slower = 'D'; +static const char prepare = 'P'; +static const char collect = 'C'; +static const char deposit = 'D'; +static const char manual = 'M'; +static const char drillRelease = 'X'; +static const char open = 'O'; +static const char close = 'C'; +static const char joint = 'J'; +static const char EEval = 'E'; +static const char homeValEE = 'H'; +static const char moveBase = 'T'; +static const char moveWrist = 'M'; +static const char relWrist = 'R'; +static const char relBase = 'W'; + +// Motor variables +int stepPins[8] = { 6, 8, 10, 2, 12, 25, 12, 25 }; +int dirPins[8] = { 5, 7, 9, 1, 11, 24, 11, 24 }; +int stepPinsIK[6] = { 6, 8, 2, 10, 25, 12 }; +int dirPinsIK[6] = { 5, 7, 1, 9, 24, 11 }; +int encPinA[6] = { 17, 38, 40, 36, 15, 13 }; +int encPinB[6] = { 16, 37, 39, 35, 14, 41 }; + +// end effector variables +const float calibrationFactor = -111.25; +float force; +HX711 scale; +const int dataPin = 34; +const int clkPin = 33; +int calPos = 0; +int closePos = 0; +int openPos = 500; // needs to be calibrated +int EEstepPin = 4; +int EEdirPin = 3; +int speedEE = 100; +int accEE = 500; +int speedDrill = 3000; +int accDrill = 1000; +const int MOTOR_DIR_EE = 1; +const int openButton = 5; +const int closeButton = 4; +const float calForce = 0.3; +const float maxForce = 10.0; +float EEforce; +int forcePct = 0; + +// limit switch pins +int limPins[6] = { 18, 19, 21, 20, 23, 22 }; + +// pulses per revolution for motors +long ppr[6] = { 400, 400, 400, 400, 400, 400 }; + +// Gear Reduction Ratios +float red[6] = { 50.0, 161.0, 44.8, 93.07, 57.34, 57.34 }; +float redIK[6] = { 50.0, 161.0, 93.07, 44.8, 57.34, 57.34 }; + + +// Encoder Variables +int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; +int pprEnc = 512; +int ENC_DIR[6] = { -1, -1, -1, -1, 1, 1 }; +const float ENC_MULT[] = { 5.12, 5.12, 5.12, 5.12, 5.12, 5.12 }; +float ENC_STEPS_PER_DEG[NUM_AXES]; + +// Motor speeds and accelerations +int maxSpeed[8] = { 1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200 }; +int maxAccel[8] = { 1300, 3500, 4600, 3300, 5000, 5000, 5000, 5000 }; +int homeSpeed[8] = { 300, 1000, 1000, 400, 2000, 2000, 2000, 2000 }; // {500, 1200, 600, 400, 2000, 2000, 2000, 2000}; +int homeAccel[8] = { 500, 2000, 1500, 1000, 1500, 1500, 1500, 1500 }; //{500, 2000, 1000, 1500, 1500, 1500, 1500, 1500}; + + +// Time variables +const unsigned long readInterval = 10; +unsigned long currentTime; +unsigned long currentTimeJP; +unsigned long currentTimeEE; +unsigned long previousTime = 0; +unsigned long previousTimeEE = 0; +const unsigned long timeIntervalEE = 500; +const unsigned long timeIntervalJP = 250; +unsigned long previousTimeJP = 0; + +// stepper motor objects for AccelStepper library +AccelStepper steppers[8]; +AccelStepper endEff(1, EEstepPin, EEdirPin); +AccelStepper steppersIK[8]; + + +Encoder enc1(encPinA[0], encPinB[0]); +Encoder enc2(encPinA[1], encPinB[1]); +Encoder enc3(encPinA[2], encPinB[2]); +Encoder enc4(encPinA[3], encPinB[3]); +Encoder enc5(encPinA[4], encPinB[4]); +Encoder enc6(encPinA[5], encPinB[5]); + +Encoder encoders[] = { enc1, enc2, enc3, enc4, enc5, enc6 }; + +// variable declarations + +int axisDir[8] = { 1, -1, 1, -1, 1, 1, -1, 1 }; +int axisDirIK[6] = { -1, -1, -1, 1, -1, -1 }; +int currentAxis = 1; +int runFlags[] = { 0, 0, 0, 0, 0, 0 }; +int i; +bool initFlag = false; +bool jointFlag = true; +bool IKFlag = false; +bool J1Flag = false; +bool resetEE = false; +bool vertFlag = false; +bool horizFlag = false; + +// variables for homing / arm calibration +long homePosConst = -99000; +long homePos[] = { axisDir[0] * homePosConst, axisDir[1] * homePosConst, axisDir[2] * homePosConst, axisDir[3] * homePosConst, axisDir[4] * homePosConst, axisDir[5] * homePosConst, axisDir[6] * homePosConst, axisDir[7] * homePosConst }; +long homeCompAngles[] = { axisDir[0] * 54, axisDir[1] * 10, axisDir[2] * 90, axisDir[3] * 1, axisDir[4] * 85, axisDir[5] * 85, axisDir[6] * 170, axisDir[7] * 170 }; +long homeCompConst[] = { 500, 2000, 1000, 500, 500, 500, 500, 500 }; +long homeComp[] = { axisDir[0] * homeCompConst[0], axisDir[1] * homeCompConst[1], axisDir[2] * homeCompConst[2], axisDir[3] * homeCompConst[3], axisDir[4] * homeCompConst[4], axisDir[5] * homeCompConst[5], axisDir[6] * homeCompConst[6], axisDir[7] * homeCompConst[7] }; +long homeCompSteps[] = { homeCompAngles[0] * red[0] * ppr[0] / 360.0, homeCompAngles[1] * red[1] * ppr[1] / 360.0, homeCompAngles[2] * red[2] * ppr[2] / 360.0, homeCompAngles[3] * red[3] * ppr[3] / 360.0, homeCompAngles[4] * red[4] * ppr[4] / 360.0, homeCompAngles[5] * red[5] * ppr[5] / 360.0, homeCompAngles[6] * red[4] * ppr[4] / 360.0, homeCompAngles[7] * red[5] * ppr[5] / 360.0 }; +// Range of motion (degrees) for each axis +int maxAngles[6] = { 190, 160, 180, 120, 160, 180 }; +long max_steps[] = { axisDir[0] * red[0] * maxAngles[0] / 360.0 * ppr[0], axisDir[1] * red[1] * maxAngles[1] / 360.0 * ppr[1], axisDir[2] * red[2] * maxAngles[2] / 360.0 * ppr[2], axisDir[3] * red[3] * maxAngles[3] / 360.0 * ppr[3], red[4] * maxAngles[4] / 360.0 * ppr[4], red[5] * maxAngles[5] / 360.0 * ppr[5] }; +long min_steps[NUM_AXES]; +char value; + +// values for changing speed +const int maxSpeedIndex = 2; +int speedVals[maxSpeedIndex + 1][NUM_AXES_EFF] = { { 600, 900, 1500, 1250, 1050, 1050, 1050, 1050 }, { 900, 1200, 2000, 1665, 1460, 1460, 1460, 1460 }, { 900, 1600, 2500, 2200, 2000, 2000, 2000, 2000 } }; +int speedIndex = maxSpeedIndex; + +// Cartesian mode speed settings +float IKspeeds[] = { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }; +float IKaccs[] = { 0.3, 0.3, 0.3, 0.3, 0.3, 0.3 }; + +void runSteppers(); +void runSteppersIK(); +void homeArm(); +void readEncPos(int* encPos); +void cmdArmBase(); +void cmdArmWrist(); +void cartesianToJointSpace(); +void runWrist(int dir, int axis); +void sendCurrentPosition(); + + +void setup() { // setup function to initialize pins and provide initial homing to the arm + // nh.getHardware()->setBaud(57600); + nh.initNode(); + nh.advertise(heart); + nh.advertise(observer); + nh.negotiateTopics(); + //Serial.begin(9600); + + + + // for (int i = 0; i < NUM_AXES; i++) { + // ENC_STEPS_PER_DEG[i] = ppr[i] * redIK[i] * (ENC_MULT[i] / 360.0); + // } + // for (int i = 0; i < NUM_AXES_EX_WRIST; i++) { + // min_steps[i] = -homeCompSteps[i]; + // max_steps[i] = max_steps[i] - homeCompSteps[i]; + // } + // min_steps[0] = -(homeCompSteps[0] * axisDir[0] * 180) / homeCompAngles[0]; + // min_steps[4] = -homeCompSteps[4] / axisDir[4]; + // min_steps[5] = -homeCompSteps[6] / axisDir[6]; + // max_steps[4] = max_steps[4] - homeCompSteps[4] / axisDir[4]; + // max_steps[5] = max_steps[5] - homeCompSteps[6] / axisDir[6]; + + + // initializes end effector motor + // pinMode(EEstepPin, OUTPUT); + // pinMode(EEdirPin, OUTPUT); + // endEff.setMinPulseWidth(200); + // endEff.setMaxSpeed(speedEE); + // endEff.setAcceleration(accEE); + // endEff.setCurrentPosition(1000); + + // initializes step pins, direction pins, limit switch pins, and stepper motor objects for accelStepper library + for (i = 0; i < NUM_AXES; i++) { + pinMode(stepPinsIK[i], OUTPUT); + pinMode(dirPinsIK[i], OUTPUT); + steppersIK[i] = AccelStepper(1, stepPinsIK[i], dirPinsIK[i]); + steppersIK[i].setMinPulseWidth(200); + } + + for (i = 0; i < NUM_AXES_EFF; i++) { + pinMode(dirPins[i], OUTPUT); + pinMode(stepPins[i], OUTPUT); + pinMode(limPins[i], INPUT_PULLUP); + steppers[i] = AccelStepper(1, stepPins[i], dirPins[i]); + steppers[i].setMinPulseWidth(200); + } + // waits for user to press "home" button before rest of functions are available + //Serial.println("diagnosticing"); + // waitForHome(); + delay(90); + +//homeArm(); + nh.spinOnce(); +} + +void loop() { + if (millis() - posTEMP > posINTERVAL) { + //sendCurrentPosition(); + posTEMP = 0; + } +if(millis() - beatTEMP > beatINTERVAL){ + beat.data += 1; + heart.publish(&beat); + beatTEMP = millis(); + } + + // if(!IKFlag) + // runSteppers(); + + // else + // runSteppersIK(); + + + nh.spinOnce(); +} + +// void recieveCommand() +// { +// String inData = ""; +// char recieved = '\0'; + +// if(Serial.available()>0) +// { +// do { +// recieved = Serial.read(); +// inData += String(recieved); +// } while(recieved != '\n'); +// } + +// if(recieved == '\n') +// { + +// parseMessage(inData); +// } +// } + +// void parseMessage(String inMsg) +// { +// String function = inMsg.substring(0, 2); + +// if(function == "MT") +// { +// cartesianCommands(inMsg); +// } + +// else if(function == "JM") +// { +// jointCommands(inMsg); +// } + +// else if(function == "EE") +// { +// endEffectorCommands(inMsg); +// } + +// else if(function == "DM") +// { +// drillCommands(inMsg); +// } + +// else if(function == "FB") +// { +// sendFeedback(inMsg); +// } + +// else if(function == "HM") +// { +// homeArm(); +// } +// } + +// void sendMessage(char outChar) +// { +// String outMsg = String(outChar); +// Serial.print(outMsg); +// } + +// void sendFeedback(String inMsg) +// { +// char function = inMsg[2]; + +// if(function == joint) +// { +// readEncPos(curEncSteps); +// sendCurrentPosition(); +// } +// } + +//****//CARTESIAN MODE FUNCTIONS//****// + +void cartesianCommands(String inMsg) { + // read current joint positions + readEncPos(curEncSteps); + + // update host with joint positions + sendCurrentPosition(); + + if (!IKFlag) { + setCartesianSpeed(); + IKFlag = true; + jointFlag = false; + } + + // get new position commands + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt(); + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt(); + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt(); + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt(); + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt(); + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt(); + + // update target joint positions + readEncPos(curEncSteps); + cmdArmBase(); + cmdArmWrist(); +} + +void setCartesianSpeed() { + float JOINT_MAX_SPEED[NUM_AXES]; + float MOTOR_MAX_SPEED[NUM_AXES]; + float JOINT_MAX_ACCEL[NUM_AXES]; + float MOTOR_MAX_ACCEL[NUM_AXES]; + + for (int i = 0; i < NUM_AXES; i++) { + JOINT_MAX_SPEED[i] = IKspeeds[i] * (180.0 / 3.14159); + JOINT_MAX_ACCEL[i] = IKaccs[i] * (180.0 / 3.14159); + MOTOR_MAX_SPEED[i] = JOINT_MAX_SPEED[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + MOTOR_MAX_ACCEL[i] = JOINT_MAX_ACCEL[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + steppersIK[i].setAcceleration(MOTOR_MAX_ACCEL[i]); + steppersIK[i].setMaxSpeed(MOTOR_MAX_SPEED[i]); + } +} + +// parses which commands to execute when in joint space mode +void jointCommands(String inMsg) { + char function = 3; //inMsg[2]; + char detail1 = 4; //inMsg[3]; + + if (!jointFlag) { + cartesianToJointSpace(); + IKFlag = false; + jointFlag = true; + } + + if (function == moveBase) + moveArmBase(detail1, inMsg[4]); + else if (function == moveWrist) { + char dir = inMsg[4]; + if (dir == up) + runWrist(FWD, 6); + else if (dir == down) + runWrist(REV, 6); + else if (dir == left) + runWrist(FWD, 5); + else if (dir == right) + runWrist(REV, 5); + } else if (function == relBase) + relArmBase(detail1); + else if (function == relWrist) + releaseEvent(detail1, inMsg[4]); +} + +void moveArmBase(char axis, char dir) { + int axisNum = String(axis).toInt(); + + if (axisNum == 3) + axisNum = 4; + + else if (axisNum == 4) + axisNum = 3; + + if ((dir == left) || (dir == up)) { + moveBaseAxis((axisNum - 1), FWD); + } else if ((dir == right) || (dir == down)) { + moveBaseAxis((axisNum - 1), REV); + } +} + +void relArmBase(char axis) { + int axisNum = String(axis).toInt(); + + if (axisNum == 3) + axisNum = 4; + + else if (axisNum == 4) + axisNum = 3; + + steppers[axisNum - 1].stop(); +} + +void moveBaseAxis(int axis, int dir) { + if ((axis == 0) || (axis == 1)) { + dir = !dir; + } + + if (dir == FWD) { + steppers[axis].moveTo(max_steps[axis]); + runFlags[axis] = 1; + } else if (dir == REV) { + steppers[axis].moveTo(min_steps[axis]); + runFlags[axis] = -1; + } +} + +void endEffectorCommands(String inMsg) { + char data = inMsg[2]; + + //opening code + if (data == open) { //check if open button pressed and if force is less than max + endEff.moveTo(openPos * MOTOR_DIR_EE); //continue to move to open position + } + + //closing code + else if (data == close) { //check if open button pressed and if force is less than max + endEff.moveTo(closePos * MOTOR_DIR_EE); //continue to move to closed position + } + + else if (data == release) { //else check if release button pressed + endEff.stop(); // stop when above condition reached + } + + else if (data == homeValEE) { + if (resetEE) { + endEff.setCurrentPosition(1000); + resetEE = false; + } + + else { + endEff.setCurrentPosition(0); + resetEE = true; + } + } +} + +void getEEForce() { + // if(scale.wait_ready_timeout(1)) + // { + // float force = scale.get_units()/1000*9.81; + // forcePct = force*100.0/maxForce; + // } +} + +void sendEEForce() { + //String force_value = String(forcePct); + //String force_message = String("EE: Gripper Force: ") + String(force_value) + String(" Z"); + // String force_message = String("EEForceZ"); + // Serial.print(force_message); +} + +void drillCommands(String inMsg) { + char function = inMsg[2]; + + if (function == manual) + manualDrill(inMsg[3]); + else if (function == drillRelease) + stopDrill(); + else if (function == prepare) + spinDrill(); + else if (function == collect) + stopDrill(); + else if (function == deposit) + depositSample(); +} + +void manualDrill(char dir) { + if (dir == left) { + endEff.move(99000); + } + + else { + endEff.move(-99000); + } +} + +void stopDrill() { + endEff.stop(); +} + +void spinDrill() { + endEff.move(99000); +} + +void collectSample() { +} + +void depositSample() { +} + +void sendCurrentPosition() { + for (int i = 0; i < NUM_AXES; i++) { + OBSangles.positions[i] = float(steppers[i].currentPosition()) / axisDir[i] / red[i] / ppr[i]; + } + observer.publish(&OBSangles); +} + +// converts cartesian angles to joint space stepper current positions +void cartesianToJointSpace() { + long curJointPos[NUM_AXES_EFF]; + readEncPos(curEncSteps); + curJointPos[0] = curEncSteps[0] / 5.12; + curJointPos[1] = curEncSteps[1] / 5.12; + curJointPos[3] = curEncSteps[2] / 5.12; + curJointPos[2] = curEncSteps[3] / 5.12; + curJointPos[6] = curEncSteps[4] / 5.12; + curJointPos[7] = curEncSteps[4] / 5.12; + curJointPos[4] = curEncSteps[5] / 5.12; + curJointPos[5] = curEncSteps[5] / 5.12; + + for (int i = 0; i < NUM_AXES_EFF; i++) { + steppers[i].setCurrentPosition(curJointPos[i] * axisDir[i]); + } +} + +// Sets movement target positions when in joint space mode +void jointMovement(char joystick, char dir) { + if (joystick == wrist) { + if (dir == up) + runWrist(FWD, 6); + else if (dir == down) + runWrist(REV, 6); + else if (dir == left) + runWrist(FWD, 5); + else if (dir == right) + runWrist(REV, 5); + } + + else if (joystick == left) { + if (dir == left) + runAxes(FWD, currentAxis); + else if (dir == right) + runAxes(REV, currentAxis); + } + + else { + if (dir == up) + runAxes(FWD, currentAxis + 1); + else if (dir == down) + runAxes(REV, currentAxis + 1); + } +} + +//****//ENCODER RELATED FUNCTIONS//****// + +void readEncPos(int* encPos) { + encPos[0] = enc1.read() * ENC_DIR[0]; + encPos[1] = enc2.read() * ENC_DIR[1]; + encPos[2] = enc3.read() * ENC_DIR[2]; + encPos[3] = enc4.read() * ENC_DIR[3]; + encPos[4] = enc5.read() * ENC_DIR[4]; + encPos[5] = enc6.read() * ENC_DIR[5]; + long temp = encPos[4]; + encPos[5] = temp + encPos[5]; + encPos[4] = encPos[5] - temp; +} + +void zeroEncoders() { + enc1.write(0); + enc2.write(0); + enc3.write(0); + enc4.write(0); + enc5.write(0); + enc6.write(0); + + readEncPos(curEncSteps); +} + +void cmdArmBase() { + for (int i = 0; i < NUM_AXES_EX_WRIST; i++) { + int diffEncSteps = cmdEncSteps[i] - curEncSteps[i]; + if (abs(diffEncSteps) > 6) { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < ppr[i]) { + // for the last rev of motor, introduce artificial decceleration + // to help prevent overshoot + diffMotSteps = diffMotSteps / 2; + } + + steppersIK[i].move(diffMotSteps * axisDirIK[i]); + } + } +} + +void cmdArmWrist() { + int diffMotStepsA5, diffMotStepsA6, diffEncStepsA5, diffEncStepsA6; + + diffEncStepsA5 = cmdEncSteps[4] - curEncSteps[4]; + diffEncStepsA6 = cmdEncSteps[5] - curEncSteps[5]; + + if (abs(diffEncStepsA5) > 10) { + diffMotStepsA5 = diffEncStepsA5 / ENC_MULT[4]; + if (diffMotStepsA5 < ppr[4]) { + diffMotStepsA5 = diffEncStepsA5 / 2; + } + } + + if (abs(diffEncStepsA6) > 10) { + diffMotStepsA6 = diffEncStepsA6 / ENC_MULT[5]; + if (diffMotStepsA6 < ppr[5]) { + diffMotStepsA6 = diffEncStepsA6 / 2; + } + } + + int actualMotStepsA5 = diffMotStepsA6 / 2 - diffMotStepsA5 / 2; + int actualMotStepsA6 = diffMotStepsA6 / 2 + diffMotStepsA5 / 2; + + steppersIK[4].move(actualMotStepsA5 * axisDirIK[4]); + steppersIK[5].move(actualMotStepsA6 * axisDirIK[5]); +} + + + +//****//JOINT SPACE MODE FUNCTIONS//****// + +// sets target position for axes in joint space mode +void runAxes(int dir, int axis) { // assigns run flags to indicate forward / reverse motion and sets target position + + if ((axis == 3) || (axis == 1) || (axis == 2)) { + dir = !dir; + } + + if (runFlags[axis - 1] == 1 && dir == FWD) { + } + + else if (runFlags[axis - 1] == -1 && dir == REV) { + } + + else if (dir == FWD) { + steppers[axis - 1].moveTo(max_steps[axis - 1]); + runFlags[axis - 1] = 1; + } + + else { + steppers[axis - 1].moveTo(min_steps[axis - 1]); + runFlags[axis - 1] = -1; + } +} + +void runWrist(int dir, int axis) { // assigns target position for selected axis based on user input. + + if (axis == 5) { + if (runFlags[5] == 1 && dir == FWD) { + } + + else if (runFlags[5] == -1 && dir == REV) { + } + + else if (dir == FWD) { + steppers[6].moveTo(axisDir[6] * max_steps[5]); + steppers[7].moveTo(axisDir[7] * max_steps[5]); + runFlags[5] = 1; + } + + else { + steppers[6].moveTo(axisDir[6] * min_steps[5]); + steppers[7].moveTo(axisDir[7] * min_steps[5]); + runFlags[5] = -1; + } + } + + else if (axis == 6) { + dir = !dir; + if (runFlags[4] == 1 && dir == FWD) { + } + + else if (runFlags[4] == -1 && dir == REV) { + } + + else if (dir == FWD) { + steppers[4].moveTo(axisDir[4] * max_steps[4]); + steppers[5].moveTo(axisDir[5] * max_steps[4]); + runFlags[4] = 1; + } + + else { + steppers[4].moveTo(axisDir[4] * min_steps[4]); + steppers[5].moveTo(axisDir[5] * min_steps[4]); + runFlags[4] = -1; + } + } +} + +void changeAxis(int dir) { // when user hits specified button, axis targets change + + if ((dir == up) && (currentAxis == 1)) { + currentAxis = 3; + zeroRunFlags(); + } + + else if ((dir == down) && (currentAxis == 3)) { + currentAxis = 1; + zeroRunFlags(); + } +} + +void releaseEvent(char joystick, char dir) { // when user releases a joystick serial sends a character + + if (joystick == wrist) { + if ((dir == left) || (dir == right)) { + steppers[6].stop(); + steppers[7].stop(); + runFlags[5] = 0; + } + + else { + steppers[4].stop(); + steppers[5].stop(); + runFlags[4] = 0; + } + } + + else if (joystick == left) { + steppers[currentAxis - 1].stop(); + runFlags[currentAxis - 1] = 0; + } + + else if (joystick == right) { + steppers[currentAxis].stop(); + runFlags[currentAxis] = 0; + } +} + +void changeSpeed(char speedVal) { // changes speed of all axes based on user input + + if (speedVal == faster) { + if (speedIndex < maxSpeedIndex) { + speedIndex++; + for (i = 0; i < NUM_AXES_EFF; i++) { + steppers[i].setMaxSpeed(speedVals[speedIndex][i]); + } + } + } + + else if (speedVal == slower) { + if (speedIndex > 0) { + speedIndex--; + for (i = 0; i < NUM_AXES_EFF; i++) { + steppers[i].setMaxSpeed(speedVals[speedIndex][i]); + } + } + } +} + +void zeroRunFlags() { // when user changes axis to control on switch, slow current moving axes to a stop and reset run flags (all motors stagnant) + + for (i = 0; i < NUM_AXES_EX_WRIST; i++) { + steppers[i].stop(); + } + + for (i = 0; i < NUM_AXES_EX_WRIST; i++) { + runFlags[i] = 0; + } +} + +//****// ARM CALIBRATION FUNCTIONS//****// + +void homeArm() { // main function for full arm homing + if (!simOnly) { + initializeWristHomingMotion(); + homeWrist(); + initializeHomingMotion(); + homeBase(); + initializeMotion(); + zeroEncoders(); + } + + J1Flag = true; + + for (int i = 0; i < NUM_AXES; i++) { + steppersIK[i].setCurrentPosition(0); + } + + // clear serial port in case of garbage values + while (Serial.available() != 0) { + Serial.read(); + } + + // notify hardware driver that homing has completed + Serial.print("HCZ"); +} + +void homeBase() { // homes axes 1-4 + + bool stopFlags[4] = { false, false, false, false }; + bool finishFlags[4] = { false, false, false, false }; + bool completeFlag = false; + int count = 0; + + while (!completeFlag) { + + for (i = 0; i < NUM_AXES_EX_WRIST; i++) { + + if (!finishFlags[i]) { + + if (!digitalRead(limPins[i]) && !stopFlags[i]) { + steppers[i].run(); + } + + else if (!stopFlags[i]) { + steppers[i].setCurrentPosition(-homeComp[i]); + steppers[i].stop(); + steppers[i].setMaxSpeed(maxSpeed[i] / 2); + steppers[i].setAcceleration(homeAccel[i]); + steppers[i].moveTo(homeCompSteps[i]); + stopFlags[i] = true; + } + + else if (steppers[i].distanceToGo() != 0) { + steppers[i].run(); + } + + else { + finishFlags[i] = true; + count++; + } + } + } + if (count == NUM_AXES_EX_WRIST) { + completeFlag = true; + } + } + initializeMotion(); +} + +void homeWrist() { // homes axes 5-6 + + bool stopFlags[2] = { false, false }; + bool calibFlags[2] = { false, false }; + bool completeFlag = false; + + while (!completeFlag) { + + if (!digitalRead(limPins[5]) && !stopFlags[0]) { + steppers[6].run(); + steppers[7].run(); + } + + else if (!stopFlags[0]) { + steppers[6].setCurrentPosition(-homeComp[6]); + steppers[7].setCurrentPosition(-homeComp[7]); + steppers[6].stop(); + steppers[7].stop(); + steppers[6].setMaxSpeed(maxSpeed[6]); + steppers[7].setMaxSpeed(maxSpeed[7]); + steppers[6].setAcceleration(maxAccel[6]); + steppers[7].setAcceleration(maxAccel[7]); + steppers[6].moveTo(homeCompSteps[6]); + steppers[7].moveTo(homeCompSteps[7]); + stopFlags[0] = true; + } + + else if (steppers[6].distanceToGo() != 0 && !calibFlags[0]) { + steppers[6].run(); + steppers[7].run(); + } + + else if (!calibFlags[0]) { + calibFlags[0] = true; + } + + else if (!digitalRead(limPins[4]) && !stopFlags[1]) { + steppers[4].run(); + steppers[5].run(); + } + + else if (!stopFlags[1]) { + steppers[4].setCurrentPosition(-homeComp[4]); + steppers[5].setCurrentPosition(-homeComp[5]); + steppers[4].stop(); + steppers[5].stop(); + steppers[4].setMaxSpeed(maxSpeed[4]); + steppers[5].setMaxSpeed(maxSpeed[5]); + steppers[4].setAcceleration(maxAccel[4]); + steppers[5].setAcceleration(maxAccel[5]); + steppers[4].moveTo(homeCompSteps[4]); + steppers[5].moveTo(homeCompSteps[5]); + stopFlags[1] = true; + } + + else if (steppers[4].distanceToGo() != 0 && !calibFlags[1]) { + steppers[4].run(); + steppers[5].run(); + } + + else if (!calibFlags[1]) { + calibFlags[1] = true; + completeFlag = true; + } + } +} + +//void homeEE() +//{ +// EEforce=scale.get_units()/1000*9.81; +// +// // target position for end effector in closed direction +// endEff.move(-99000*MOTOR_DIR_EE); +// +// while(abs(EEforce) < calForce) +// { +// if (scale.wait_ready_timeout(1)) { +// EEforce=scale.get_units()/1000*9.81; //converting mass to force +// // close end effector +// } +// endEff.run(); +// } +// +// endEff.setCurrentPosition(-30); +// endEff.moveTo(openPos*MOTOR_DIR_EE); +// while(endEff.distanceToGo() != 0) +// { +// endEff.run(); +// } +//} + +void initializeHomingMotion() { // sets homing speed and acceleration for axes 1-4 and sets target homing position + + for (i = 0; i < NUM_AXES_EX_WRIST; i++) { + + steppers[i].setMaxSpeed(homeSpeed[i]); + steppers[i].setAcceleration(homeAccel[i]); + steppers[i].setCurrentPosition(0); + + if (i != 0) { + steppers[i].move(homePos[i]); + } else if (J1Flag == true) { + if ((steppers[0].currentPosition() / axisDir[0] < 0) && (abs(steppers[0].currentPosition() / axisDir[0]) > (homeCompSteps[0] + homeComp[0]) / axisDir[0])) { + steppers[0].move(-homePos[i]); + } else { + steppers[0].move(homePos[i]); + } + } else { + steppers[0].move(homePos[i]); + } + } +} + +void initializeWristHomingMotion() { // sets homing speed and acceleration for axes 5-6 and sets target homing position + + for (i = 4; i < NUM_AXES_EFF; i++) { + + steppers[i].setMaxSpeed(homeSpeed[i]); + steppers[i].setAcceleration(homeAccel[i]); + steppers[i].setCurrentPosition(0); + steppers[i].move(homePos[i]); + } +} + +void initializeMotion() { // sets main program speeds for each axis + + for (i = 0; i < NUM_AXES_EFF; i++) { + steppers[i].setMaxSpeed(speedVals[maxSpeedIndex][i]); + steppers[i].setAcceleration(maxAccel[i]); + steppers[i].setCurrentPosition(0); + } + + for (i = 0; i < NUM_AXES; i++) { + steppersIK[i].setMaxSpeed(speedVals[maxSpeedIndex][i]); + steppersIK[i].setAcceleration(maxAccel[i]); + } +} + +void zeroAxes() { // sets current position of all axes to 0 + + for (i = 0; i < NUM_AXES_EFF; i++) { + steppers[i].setCurrentPosition(0); + } +} + +void runSteppers() { // runs all stepper motors (if no target position has been assigned, stepper will not move) + + for (i = 0; i < NUM_AXES_EFF; i++) { + steppers[i].run(); + } + + endEff.run(); +} + +void runSteppersIK() { // runs all stepper motors (if no target position has been assigned, stepper will not move) + + for (i = 0; i < NUM_AXES; i++) { + steppersIK[i].run(); + } + + endEff.run(); +} + +void waitForHome() { + String inData; + char recieved = '\0'; + + while (!initFlag) { + inData = ""; + if (Serial.available() > 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); + } + + if (recieved == '\n') { + if (inData.substring(0, 2) == "HM") + homeArm(); + initFlag = true; + } + } +} + +// updated aug 22, 2022 \ No newline at end of file diff --git a/src/marker_qr_detection/include/DetectMarker.h b/src/marker_qr_detection/include/DetectMarker.h index 2ac28dd53..0ee26b330 100644 --- a/src/marker_qr_detection/include/DetectMarker.h +++ b/src/marker_qr_detection/include/DetectMarker.h @@ -52,7 +52,7 @@ class DetectMarker { cv::Ptr dictionary; cv::Ptr parameters; - bool draw_markers = false; + bool draw_markers = true; int camera = 1; }; #endif // MARKER_QR_DETECTION_DETECT_MARKER_H diff --git a/src/marker_qr_detection/src/DetectMarker.cpp b/src/marker_qr_detection/src/DetectMarker.cpp index 12309c2af..c97dc1850 100644 --- a/src/marker_qr_detection/src/DetectMarker.cpp +++ b/src/marker_qr_detection/src/DetectMarker.cpp @@ -66,16 +66,16 @@ void DetectMarker::subscriberCallBack(const sensor_msgs::Image::ConstPtr& msg) { std::vector DetectMarker::fetchMarkerIds(const cv::Mat& image) { std::vector markerIds; std::vector> markerCorners; - cv::aruco::detectMarkers( - image, dictionary, markerCorners, markerIds, parameters); + cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters); + if (draw_markers) { cv::Mat outputImage; image.copyTo(outputImage); cv::aruco::drawDetectedMarkers(outputImage, markerCorners, markerIds); - bounder.publish( - cv_bridge::CvImage(std_msgs::Header(), "bgr8", outputImage) + bounder.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", outputImage) .toImageMsg()); - ROS_INFO("attempted draw"); + + ROS_INFO("attempted draw, corner 0 is at %d", markerCorners[0][0]); } return markerIds; } diff --git a/src/motor_control_circ/src/move_motor.cpp b/src/motor_control_circ/src/move_motor.cpp index 2feb46df6..25179342b 100644 --- a/src/motor_control_circ/src/move_motor.cpp +++ b/src/motor_control_circ/src/move_motor.cpp @@ -12,7 +12,8 @@ int main(int argc, char** argv) { MoveMotor controller(argc, argv, node_name); - ros::spin(); + ros::spinOnce(); + controller.close(); return 0; } diff --git a/src/procontroller_snowbots/include/ProController.h b/src/procontroller_snowbots/include/ProController.h index 8a7cb4006..f1494cb40 100644 --- a/src/procontroller_snowbots/include/ProController.h +++ b/src/procontroller_snowbots/include/ProController.h @@ -92,10 +92,7 @@ class ProController { const char rightJSPressRel = '8'; const char homeVal = '4'; const char homeValEE = '6'; - const char J1 = '1'; - const char J2 = '2'; - const char J3 = '3'; - const char J4 = '4'; + // arm modes const char jointMode = '1'; const char IKMode = '2'; diff --git a/src/procontroller_snowbots/src/ProController.cpp b/src/procontroller_snowbots/src/ProController.cpp index f6cb159a4..bceb9768c 100755 --- a/src/procontroller_snowbots/src/ProController.cpp +++ b/src/procontroller_snowbots/src/ProController.cpp @@ -130,7 +130,11 @@ void ProController::readInputs() { else if (state == Mode::arm_joint_space) { // Joint space control of the arm armOutMsg += jointMode; armOutMsg += armOutVal; + + if(armOutVal != "") + { publishArmMessage(armOutMsg); + } } else if (state == Mode::arm_cartesian) { // Inverse Kinematics mode i.e. cartesian motion diff --git a/src/procontroller_snowbots/src/procon_calibration_data b/src/procontroller_snowbots/src/procon_calibration_data index a344537ad..7c0fe7848 100644 --- a/src/procontroller_snowbots/src/procon_calibration_data +++ b/src/procontroller_snowbots/src/procon_calibration_data @@ -1 +1 @@ -$áÙ#ÝÝ \ No newline at end of file +"áã.á=æ \ No newline at end of file diff --git a/src/sb_all_controller/CMakeLists.txt b/src/sb_all_controller/CMakeLists.txt new file mode 100644 index 000000000..886dd515a --- /dev/null +++ b/src/sb_all_controller/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sb_all_controller) + +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") +endif() + +add_definitions(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + roscpp + geometry_msgs + sensor_msgs + ) +find_package(sb_utils REQUIRED) + + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs + +) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${sb_utils_INCLUDE_DIRS} + ./include +) + + +add_executable(all_controller src/all_controller.cpp + src/AllController.cpp include/AllController.h) + + +#target_link_libraries(all_controller evdev) +target_link_libraries(all_controller ${catkin_LIBRARIES} ${sb_utils_LIBRARIES}) + diff --git a/src/sb_all_controller/include/AllController.h b/src/sb_all_controller/include/AllController.h new file mode 100644 index 000000000..be29cda0b --- /dev/null +++ b/src/sb_all_controller/include/AllController.h @@ -0,0 +1,129 @@ +/* + * Created By: Kevin Lin + * Created On: December 21st, 2019 + * Description: Simple header file for switch controller-->ROS Twist message cpp + */ + +#ifndef ALLCONTROLLER_SNOWBOTS_CONTROLLER_H +#define ALLCONTROLLER_SNOWBOTS_CONTROLLER_H +#define NUM_BUTTONS 12 + +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include +#include +#include + +using namespace std; + +class AllController { + public: + AllController(int argc, char** argv, std::string node_name); + + private: //TODO: make vel relative to analog values + void setup(); + void processInputs(); + void readJoyInputs(const sensor_msgs::Joy::ConstPtr& msg); + bool inDeadzone(int value); + void leftJoystickX(int value); // ABS_X + void leftJoystickY(int value); // ABS_Y + void rightJoystickX(int value); // ABS_RX + void rightJoystickY(int value); // ABS_RY + void A(int value); // BTN_EAST + void B(int value); // BTN_SOUTH + void X(int value); // BTN_WEST + void Y(int value); // BTN_NORTH + void leftBumper(int value); // BTN_TL + void rightBumper(int value); // BTN_TR + void select(int value); // BTN_SELECT + void start(int value); // BTN_START + void home(int value); // BTN_MODE + void leftTrigger(int value); // ABS_Z + void rightTrigger(int value); // ABS_RZ + void leftPaddle(int value); // Same as trigger, but discreet not analog + void rightPaddle(int value); // ABS_RZ + void arrowsRorL(int value); // ABS_HAT0X + void arrowsUorD(int value); // ABS_HAT0Y + void leftJoystickPress(int value); // BTN_THUMBL + void rightJoystickPress(int value); // BTN_THUMBR + tuple publishMoveXZ(double x_new, double z_new, double x_old, double z_old); + void publishArmMessage(std::string outMsg); + void printState(); + void printControllerDebug(int type, int code, int value); + void publishCmds(); + + // see documentation to changes sensitivities at runtime + double X_SENSITIVITY = 1.0; + double Z_SENSITIVITY = 1.0; + double x; + double z; + std::string armOutMsg, armOutVal; +// character representations of buttons for arm communication + const char leftJSU = 'A'; + const char leftJSD = 'B'; + const char rightJSU = 'C'; + const char rightJSD = 'D'; + const char buttonA = 'E'; + const char buttonB = 'F'; + const char buttonX = 'G'; + const char buttonY = 'H'; + const char triggerL = 'I'; + const char triggerR = 'J'; + const char bumperL = 'K'; + const char bumperR = 'L'; + const char buttonARel = 'M'; + const char buttonBRel = 'N'; + const char buttonXRel = 'O'; + const char buttonYRel = 'P'; + const char triggerLRel = 'Q'; + const char triggerRRel = 'R'; + const char bumperLRel = 'S'; + const char bumperRRel = 'T'; + const char arrowL = 'U'; + const char arrowR = 'V'; + const char arrowU = 'W'; + const char arrowD = 'X'; + const char arrowRLRel = '0'; + const char arrowUDRel = '5'; + const char leftJSRel = 'Y'; + const char rightJSRel = 'Z'; + const char rightJSPress = '7'; + const char rightJSPressRel = '8'; + const char homeVal = '4'; + const char homeValEE = '6'; + const char J1 = '1'; + const char J2 = '2'; + const char J3 = '3'; + const char J4 = '4'; + // arm modes + const char jointMode = '1'; + const char IKMode = '2'; + const char drillMode = '3'; + + struct libevdev* dev = NULL; + enum Mode { wheels = 0, arm_joint_space = 1, arm_cartesian = 2, drilling = 3, num_modes = 2 }; + Mode state; + bool debug = false; + ros::Publisher pubmove; + ros::Publisher pubarm; + ros::Publisher pubmode; + ros::Publisher pubmovegrp; + ros::Subscriber joyinput; + + std_msgs::Bool true_message; + std_msgs::Bool false_message; + int speed = 50; + int max_speed = 100; + int increment = 10; + +}; + +#endif // ALLCONTROLLER_SNOWBOTS_CONTROLLER_H diff --git a/src/sb_all_controller/package.xml b/src/sb_all_controller/package.xml new file mode 100644 index 000000000..787315a3a --- /dev/null +++ b/src/sb_all_controller/package.xml @@ -0,0 +1,21 @@ + + + sb_all_controller + 0.0.0 + The procontroller_snowbots package, modified to accept more controllers. xbox, ps4 + kevinlinxc + TODO + catkin + geometry_msgs + sensor_msgs + roscpp + + sb_utils + geometry_msgs + roscpp + geometry_msgs + sensor_msgs + roscpp + + + diff --git a/src/sb_all_controller/src/AllController.cpp b/src/sb_all_controller/src/AllController.cpp new file mode 100755 index 000000000..0f8b44d5d --- /dev/null +++ b/src/sb_all_controller/src/AllController.cpp @@ -0,0 +1,649 @@ +/* + * Created By: Rowan Zawadkzki, modified from the old procontroller_snowbots + * Created On: December 21st, 2019 + * Description: Uses Libevdev to turn Nintendo Switch Pro Controller left + * joystick inputs into a ROS Twist message + * old summary^^ + */ + +#include "../include/AllController.h" + +int32_t button[NUM_BUTTONS]; +int32_t Tbutton[NUM_BUTTONS]; + +_Float32 axes[8]; +_Float32 Taxes[8]; +bool proccessing; +bool switchMode; +// Read the master documentation if there's any issues with this package +AllController::AllController(int argc, char** argv, string node_name) { + string publisher = "/cmd_vel"; + string armPublisher = "/cmd_arm"; + string modePublisher = "/moveit_toggle"; + string joyTopic = "/joy"; + // string moveGrpPublisher = "/move_group_trigger"; + ros::init(argc, argv, node_name); + ros::NodeHandle private_nh("~"); + if (private_nh.param("X", X_SENSITIVITY, 1.0)) { + ROS_INFO("X sensitivity set to %f", X_SENSITIVITY); + } + if (private_nh.param("Z", Z_SENSITIVITY, 1.0)) { + ROS_INFO("Z sensitivity set to %f", Z_SENSITIVITY); + } + if (private_nh.param("debug", debug, false)) { + ROS_INFO("Debug mode %s", (debug) ? "on" : "off"); + } + setup(); + pubmove = private_nh.advertise(publisher, 1); + pubarm = private_nh.advertise(armPublisher, 1); + pubmode = private_nh.advertise(modePublisher, 1); + joyinput = private_nh.subscribe(joyTopic, 55, &AllController::readJoyInputs, this); + // pubmovegrp = private_nh.advertise(moveGrpPublisher,1); + + true_message.data = true; + false_message.data = false; + + ROS_INFO("Preparing to process inputs...\n"); + state = Mode::wheels; + printState(); + x = 0; + z = 0; + processInputs(); +} + +void AllController::readJoyInputs(const sensor_msgs::Joy::ConstPtr& msg){ +// int32_t A = msg->buttons[0]; +// int32_t B = msg->buttons[1]; +// int32_t X = msg->buttons[2]; +// int32_t Y = msg->buttons[2]; +// ROS_INFO("reacieved msg"); +// ROS_INFO("A is %i", msg->buttons[0]); +// ROS_INFO("B is %i", msg->buttons[1]); +// ROS_INFO("X is %i", msg->buttons[2]); +// ROS_INFO("Y is %i", msg->buttons[3]); +// ROS_INFO("HOME is %f", msg->axes[0]); +//if(!proccessing){ + for (int i = 0; i < NUM_BUTTONS; i ++ ){ + if(button[i] == msg->buttons[i]){ + Tbutton[i] = button[i]; + } + button[i] = msg->buttons[i]; + +//ROS_INFO("Buttons Recorded: %i", i); + + } + for (int i = 0; i < 8; i++){ + if(axes[i] == msg->axes[i]){ + Taxes[i] = axes[i]; + + } + axes[i] = msg->axes[i] * 128; + + + } + +//ROS_INFO("Axes Recorded: %i", i); + +//} +//proccessing = true; + //printState(); + processInputs(); + + +} + + + void AllController::setup() { + system( + "gnome-terminal --tab -- bash -c 'rosrun joy joy_node _deadzone:=0.1 _autorepeat_rate:=40 _coalesce_interval:=0.025'"); + + ROS_INFO( + "ALLCONTROLLER INITIATED, CURRENTLY PARSING FOR PROCONTROLLER"); +} + +void AllController::processInputs() { + +//i think deals with a lot more floats than needed + // vehicle for arm output message + + // handle all controller inputs using API functions + for(int i = 0; i < 12; i++){ + if (Tbutton[i] != button[i]){ + armOutMsg = ""; + armOutVal = ""; + switch (i) + { + case 0: + B(button[i]); + break; + case 1: + A(button[i]); + break; + case 2: + X(button[i]); + break; + case 3: + Y(button[i]); + break; + case 4: + select(button[i]); + break; + case 5: + leftBumper(button[i]); + break; + case 6: + rightBumper(button[i]); + break; + case 7: + leftPaddle(button[i]); + break; + case 8: + rightPaddle(button[i]); + break; + case 9: + select(button[i]); + break; + case 10: + start(button[i]); + break; + case 11: + home(button[i]); + break; + default: + break; + } + publishCmds(); + + } + } + // int axeflag = 0; + for(int i = 0; i < 5; i++){ + if ( Taxes[i] != axes[i] || inDeadzone(axes[i])) { + armOutMsg = ""; + armOutVal = ""; + switch (i) + { + case 0: + // leftJoystickX(axes[i]); + break; + case 1: + leftJoystickY(axes[i]); + break; + case 2: + // rightJoystickX(axes[i]); + break; + case 3: + rightJoystickY(axes[i]); + break; + // triggers here too + default: + break; + } + publishCmds(); + + } + } + //for XBOX | TODO: quick controller change parameter + // for(int i = 0; i < 9; i++){ + // if (Tbutton[i] != button[i]){ + // switch (i) + // { + // case 0: + // A(button[0]); + // break; + // case 1: + // B(button[1]); + // break; + // case 2: + // X(button[2]); + // break; + // case 3: + // Y(button[3]); + // break; + // case 4: + // leftBumper(button[4]); + // break; + // case 5: + // rightBumper(button[5]); + // break; + // case 6: + // select(button[6]); //or back on an xbox360 controlller + // break; + // case 7: + // start(button[7]); + // break; + // case 8: + // home(button[8]); + // break; + // default: + // break; + // } + // } + // } + // // int axeflag = 0; + // for(int i = 0; i < 5; i++){ + // if (Taxes[i] > axes[i] + 0.3 || Taxes[i] < axes[i] - 0.3 ){ + // switch (i) + // { + // case 0: + // leftJoystickX(axes[0]); + // break; + // case 1: + // leftJoystickY(axes[1]); + // break; + // case 2: + // rightJoystickX(axes[3]); + // break; + // case 3: + // rightJoystickY(axes[4]); + // break; + // // paddles here too + // default: + // break; + // } + // Taxes[i] = axes[i]; + // } + // } + + + + + + + + + + + + + + // switch (ev.code) { + // case ABS_X: leftJoystickX(ev.value); break; + // case ABS_Y: leftJoystickY(ev.value); break; + // case ABS_RX: rightJoystickX(ev.value); break; + // case ABS_RY: rightJoystickY(ev.value); break; + // case BTN_EAST: A(ev.value); break; + // case BTN_SOUTH: B(ev.value); break; + // case BTN_WEST: X(ev.value); break; + // case BTN_NORTH: Y(ev.value); break; + // case BTN_TL: leftBumper(ev.value); break; + // case BTN_TR: rightBumper(ev.value); break; + // case BTN_SELECT: select(ev.value); break; + // case BTN_START: start(ev.value); break; + // case BTN_MODE: home(ev.value); break; + // case ABS_Z: leftTrigger(ev.value); break; + // case ABS_RZ: rightTrigger(ev.value); break; + // case ABS_HAT0X: arrowsRorL(ev.value); break; + // case ABS_HAT0Y: arrowsUorD(ev.value); break; + // case BTN_THUMBL: leftJoystickPress(ev.value); break; + // case BTN_THUMBR: rightJoystickPress(ev.value); break; + // } + // publish move command and update oldx, oldz + + + + + // proccessing = false; + +} + +void AllController::publishCmds(){ + double x_old = 0; + double z_old = 0; + if (state == Mode::wheels) { + // Publish motion, update x and z old using tuple + tie(x_old, z_old) = publishMoveXZ(x, z, x_old, z_old); + } + else if (state == Mode::arm_joint_space) { // Joint space control of the arm + armOutMsg += jointMode; + armOutMsg += armOutVal; + publishArmMessage(armOutMsg); + } + + else if (state == Mode::arm_cartesian) { // Inverse Kinematics mode i.e. cartesian motion + armOutMsg += IKMode; + armOutMsg += armOutVal; + publishArmMessage(armOutMsg); + } + + else { // Drilling mode for arm + // armOutMsg += drillMode; + // armOutMsg += armOutVal; + // publishArmMessage(armOutMsg); + ROS_INFO("error"); + } +ROS_INFO("Attempted Publish"); + +} + + + +// Prints out a controller event using ROS_INFO +void AllController::printControllerDebug(int type, int code, int value) { + // auto codeout = libevdev_event_code_get_name(type, code); + //auto typeout = libevdev_event_type_get_name(type); + // ROS_DEBUG("Event: Type: %s Code: %s Value: %d\n", typeout, codeout, value); +} + +// Prints a status message detailing the current control mode +void AllController::printState() { + if (state == Mode::wheels) { + ROS_INFO("Current mode: controlling wheels"); + } else if (state == Mode::arm_joint_space) { + ROS_INFO("Current mode: controlling arm in the joint space"); + } else if (state == Mode::arm_cartesian) { + ROS_INFO("Current mode: controlling arm in the cartesian space"); + } else if (state == Mode::drilling) { + ROS_INFO("Current mode: drilling with the arm"); + } else { + ROS_INFO("There is no current mode, which is a problem"); + } +} + +// If x and z are new commands, this function uses the global pubmove to publish +// a movement message and return the new or old xz to update readInputs() +tuple AllController::publishMoveXZ(double x_new, + double z_new, + double x_old, + double z_old) { + if (abs(x_old - x_new) > 0.0001 || abs(z_old - z_new) > 0.0001) { + geometry_msgs::Twist msg; + msg.linear.x = x_new * speed / 100; + msg.angular.z = z_new * speed / 100; + pubmove.publish(msg); + // return tuple + return make_tuple(x_new, z_new); + } + return make_tuple(x_old, z_old); +} + +// If controller recieves new commands and is in an arm mode, send message to arm +void AllController::publishArmMessage(std::string outMsg) { + std_msgs::String outMsgWrapper; + outMsg += '\n'; + outMsgWrapper.data = outMsg; + pubarm.publish(outMsgWrapper); +} + +bool AllController::inDeadzone(int value) { + + if (value > 50 || value < -50){ + return false; + } + return true; + +} + +// Updates z, which is then published by publish___XZ in readInputs() +void AllController::leftJoystickX(int value) { + + if (inDeadzone(value)) { + if(state == Mode::wheels) { + x = 0; + } + } + + else { + // 128 is the center, so this normalizes the result to + // [-1,1]*Z_SENSITIVITY + ROS_INFO("Left Joystick X event with value: %d\n", value); + // x = (value - 128) / 128.0 * X_SENSITIVITY; + } +} + +// Updates x, which is then published by publish___XZ in readInputs() +void AllController::leftJoystickY(int value) { + if (inDeadzone(value)) { + if(state != Mode::wheels){ + armOutVal = leftJSRel; + } + } + + else { + + // 128 is the center, so this normalizes the result to + // [-1,1]*Z_SENSITIVITY + ROS_INFO("Left Joystick Y event with value: %d\n", value); + //z = -(value - 128) / 128.0 * Z_SENSITIVITY; + + // Right joystick is only used in Y direction in all arm modes + if(state != Mode::wheels) { + + if(value > 0) { + armOutVal = leftJSU; + } + + else { + armOutVal = leftJSD; + } + } + } +} + +// Currently doing nothing +void AllController::rightJoystickX(int value) { + if (inDeadzone(value)) { + // do nothing + } else { + ROS_INFO("Right Joystick X event with value: %d\n", value); + } +} + +void AllController::rightJoystickY(int value) { + + if (inDeadzone(value)) { + if(state != Mode::wheels) + armOutVal = rightJSRel; + } + + else { + + // 128 is the center, so this normalizes the result to + // [-1,1]*Z_SENSITIVITY + ROS_INFO("Right Joystick Y event with value: %d\n", value); + //z = -(value - 128) / 128.0 * Z_SENSITIVITY; + + // Right joystick is only used in Y direction in all arm modes + if(state != Mode::wheels) { + + if(value > 0) { + armOutVal = rightJSU; + } + + else { + armOutVal = rightJSD; + } + } + } +} + +void AllController::A(int value) { + if (value == 1) { + ROS_INFO("A button pressed"); + armOutVal = buttonA; + } else if (value == 0) { + ROS_INFO("A button released"); + armOutVal = buttonARel; + } +} +void AllController::leftPaddle(int value) { + if (value == 1) { + ROS_INFO("Left paddle/trigger button pressed"); + armOutVal = triggerL; + } else if (value == 0) { + ROS_INFO("Left paddle/trigger button released"); + armOutVal = triggerLRel; + } +} +void AllController::rightPaddle(int value) { + if (value == 1) { + ROS_INFO("Right paddle/trigger button pressed"); + armOutVal = triggerR; + } else if (value == 0) { + ROS_INFO("Right paddle/trigger button released"); + armOutVal = triggerRRel; + } +} + +void AllController::B(int value) { + if (value == 1) { + ROS_INFO("B button pressed"); + armOutVal = buttonB; + } else if (value == 0) { + ROS_INFO("B button released"); + armOutVal = buttonBRel; + } +} + +void AllController::X(int value) { + if (value == 1) { + ROS_INFO("X button pressed"); + armOutVal = buttonX; + } else if (value == 0) { + ROS_INFO("X button released"); + armOutVal = buttonXRel; + } +} + +void AllController::Y(int value) { + if (value == 1) { + ROS_INFO("Y button pressed"); + armOutVal = buttonY; + } else if (value == 0) { + ROS_INFO("Y button released"); + armOutVal = buttonYRel; + } +} + +void AllController::leftBumper(int value) { + if (value == 1) { + ROS_INFO("Left bumper pressed"); + armOutVal = bumperL; + } else if (value == 0) { + ROS_INFO("Left bumper released"); + armOutVal = bumperLRel; + } +} + +void AllController::rightBumper(int value) { + if (value == 1) { + ROS_INFO("Right bumper pressed"); + armOutVal = bumperR; + } else if (value == 0) { + ROS_INFO("Right bumper released"); + armOutVal = bumperRRel; + } +} + +void AllController::select(int value) { + if (value == 1) { + ROS_INFO("Select button pressed"); + } else if (value == 0) { + ROS_INFO("Select button released"); + armOutVal = homeValEE; + } +} + +void AllController::start(int value) { + if (value == 1) { + ROS_INFO("Start button pressed"); + } else if (value == 0) { + ROS_INFO("Start button released"); + armOutVal = homeVal; + } +} + +// Currently switches between wheels and arm mode +void AllController::home(int value) { + // if (!debug) { + + if (value == 0) { + ROS_INFO("Home button pressed"); + } else if (value == 1) { + state = static_cast((state + 1) % (Mode::num_modes)); + if (state == Mode::wheels || state == Mode::arm_joint_space) { + pubmode.publish(false_message); + } else { + // pubmovegrp.publish(true_message); + // sleep(8); + pubmode.publish(true_message); + } + printState(); + } + + // } +} + +void AllController::leftTrigger(int value) { + if (value == 255) { + ROS_INFO("Left trigger pressed"); + armOutVal = triggerL; + } else if (value == 0) { + ROS_INFO("Left trigger released"); + armOutVal = triggerLRel; + } +} + +void AllController::rightTrigger(int value) { + if (value == 255) { + ROS_INFO("Right trigger pressed"); + armOutVal = triggerR; + } else if (value == 0) { + ROS_INFO("Right trigger released"); + armOutVal = triggerRRel; + } +} + +void AllController::arrowsRorL(int value) { + if (value == 1) { + ROS_INFO("Right button pressed"); + armOutVal = arrowR; + } else if (value == 0) { + ROS_INFO("Arrow button released"); + armOutVal = arrowRLRel; + } else { + ROS_INFO("Left button pressed"); + armOutVal = arrowL; + } +} + +void AllController::arrowsUorD(int value) { + if (value == 1) { + ROS_INFO("Up button pressed"); + armOutVal = arrowU; + if (state == Mode::wheels) { + speed = speed < max_speed ? speed + increment : speed; + ROS_INFO("Speed increased to %d%% of max output", speed); + } + } else if (value == 0) { + ROS_INFO("Arrow button released"); + armOutVal = arrowUDRel; + } else { + ROS_INFO("Down button pressed"); + armOutVal = arrowD; + if (state == Mode::wheels) { + speed = speed > increment ? speed - increment : speed; + ROS_INFO("Speed decreased to %d%% of max output", speed); + } + } +} + +// Currently doing nothing +void AllController::leftJoystickPress(int value) { + if (value == 1) { + ROS_INFO("Left Joystick pressed"); + } else if (value == 0) { + ROS_INFO("Left Joystick released"); + } +} + +// Currently doing nothing +void AllController::rightJoystickPress(int value) { + if (value == 1) { + ROS_INFO("Right Joystick pressed"); + armOutVal = rightJSPress; + } else if (value == 0) { + ROS_INFO("Right Joystick released"); + armOutVal = rightJSPressRel; + } +} diff --git a/src/sb_all_controller/src/all_controller.cpp b/src/sb_all_controller/src/all_controller.cpp new file mode 100755 index 000000000..c641b7a52 --- /dev/null +++ b/src/sb_all_controller/src/all_controller.cpp @@ -0,0 +1,22 @@ +/* + * Created By: Kevin Lin + * Created On: February 29th, 2020 + * Description: The node that listens to switch pro controller and publishes + * twist messages + */ + +#include "../include/AllController.h" + +int main(int argc, char** argv) { + // Setup your ROS node + std::string node_name = "all_controller"; + + // Create an instance of your class + AllController all_controller(argc, argv, node_name); + + // Start up ros. This will continue to run until the node is killed + ros::spin(); + + // Once the node stops, return 0 + return 0; +} diff --git a/src/sb_msgs/CMakeLists.txt b/src/sb_msgs/CMakeLists.txt index fdd4b0e41..c3f08cd8c 100644 --- a/src/sb_msgs/CMakeLists.txt +++ b/src/sb_msgs/CMakeLists.txt @@ -17,7 +17,10 @@ find_package(catkin REQUIRED ## Generate messages in the 'msg' folder add_message_files( DIRECTORY msg - FILES ArmPosition.msg + FILES + ArmPosition.msg + armFirmware.msg + ) ## Generate added messages and services with any dependencies listed here diff --git a/src/sb_msgs/msg/armFirmware.msg b/src/sb_msgs/msg/armFirmware.msg new file mode 100644 index 000000000..0a09239af --- /dev/null +++ b/src/sb_msgs/msg/armFirmware.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +char mode +int8 axis1 +float64[] axes diff --git a/src/snowbots_arm_simplified_configs/config/fake_controllers.yaml b/src/snowbots_arm_simplified_configs/config/fake_controllers.yaml index f07bcd7fc..4337c4452 100755 --- a/src/snowbots_arm_simplified_configs/config/fake_controllers.yaml +++ b/src/snowbots_arm_simplified_configs/config/fake_controllers.yaml @@ -11,7 +11,7 @@ controller_list: - name: fake_end_effector_controller type: $(arg fake_execution_type) joints: - [] + - ef initial: # Define initial robot poses. - group: arm pose: default \ No newline at end of file diff --git a/src/snowbots_arm_simplified_configs/config/gazebo_controllers.yaml b/src/snowbots_arm_simplified_configs/config/gazebo_controllers.yaml new file mode 100644 index 000000000..e4d2eb00c --- /dev/null +++ b/src/snowbots_arm_simplified_configs/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/src/snowbots_arm_simplified_configs/config/simple_moveit_controllers.yaml b/src/snowbots_arm_simplified_configs/config/simple_moveit_controllers.yaml new file mode 100644 index 000000000..1e79255f7 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/config/simple_moveit_controllers.yaml @@ -0,0 +1,34 @@ +controller_list: + - name: /controllers/position + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 + - name: arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 + - name: velarm + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - j1 + - j2 + - j3 + - j4 + - j5 + - j6 \ No newline at end of file diff --git a/src/snowbots_arm_simplified_configs/config/stomp_planning.yaml b/src/snowbots_arm_simplified_configs/config/stomp_planning.yaml new file mode 100644 index 000000000..e8508593c --- /dev/null +++ b/src/snowbots_arm_simplified_configs/config/stomp_planning.yaml @@ -0,0 +1,78 @@ +stomp/arm: + group_name: arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/end_effector: + group_name: end_effector + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/src/snowbots_arm_simplified_configs/launch/demo.launch b/src/snowbots_arm_simplified_configs/launch/demo.launch index 2f48a37ee..ba527cf47 100755 --- a/src/snowbots_arm_simplified_configs/launch/demo.launch +++ b/src/snowbots_arm_simplified_configs/launch/demo.launch @@ -14,8 +14,13 @@ + + + + + - - + [move_group/fake_controller_joint_states] @@ -41,9 +45,9 @@ - + - + diff --git a/src/snowbots_arm_simplified_configs/launch/move_group.launch b/src/snowbots_arm_simplified_configs/launch/move_group.launch index b67e40d89..87688df0f 100755 --- a/src/snowbots_arm_simplified_configs/launch/move_group.launch +++ b/src/snowbots_arm_simplified_configs/launch/move_group.launch @@ -14,7 +14,7 @@ - + diff --git a/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch b/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch index 1f030f098..2645d097a 100644 --- a/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch +++ b/src/snowbots_arm_simplified_configs/launch/moveit_rviz.launch @@ -1,12 +1,12 @@ + - - - + + diff --git a/src/snowbots_arm_simplified_configs/launch/ompl-chomp_planning_pipeline.launch.xml b/src/snowbots_arm_simplified_configs/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..0c3a40920 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/src/snowbots_arm_simplified_configs/launch/ros_control_moveit_controller_manager.launch.xml b/src/snowbots_arm_simplified_configs/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..9ebc91c10 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/snowbots_arm_simplified_configs/launch/simple_moveit_controller_manager.launch.xml b/src/snowbots_arm_simplified_configs/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 000000000..67105e318 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/snowbots_arm_simplified_configs/launch/stomp_planning_pipeline.launch.xml b/src/snowbots_arm_simplified_configs/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 000000000..f65427ac9 --- /dev/null +++ b/src/snowbots_arm_simplified_configs/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/snowbots_arm_urdf_simplified_v5_12/launch/gazebo.launch b/src/snowbots_arm_urdf_simplified_v5_12/launch/gazebo.launch index ca6cd45fe..5ab52717b 100755 --- a/src/snowbots_arm_urdf_simplified_v5_12/launch/gazebo.launch +++ b/src/snowbots_arm_urdf_simplified_v5_12/launch/gazebo.launch @@ -5,7 +5,7 @@ name="tf_footprint_base" pkg="tf" type="static_transform_publisher" - args="0 0 0 0 0 0 base_link base_footprint 40" /> + args="5 5 5 5 0 0 base_link base_footprint 40" /> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + / + + + + diff --git a/src/teensy-firmware/src/armFirmware.cpp b/src/teensy-firmware/src/armFirmware.cpp index ecaddd288..2a1f709e8 100644 --- a/src/teensy-firmware/src/armFirmware.cpp +++ b/src/teensy-firmware/src/armFirmware.cpp @@ -1,477 +1,370 @@ /* -Created By: Tate Kolton, Rowan Zawadzki +Created By: Tate Kolton, Graeme Dockrill Created On: December 21, 2021 -Updated: April 27, 2022 +Updated On: January 10, 2023 Description: Main firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU */ - -#include - -// general parameters -#define NUM_AXES 6 -#define NUM_AXES_EX_WRIST 4 -#define NUM_AXES_EFF 8 -#define NUM_PARAMS 7 -#define ON 0 -#define OFF 1 -#define SW_ON 0 -#define SW_OFF 1 -#define FWD 1 -#define REV 0 - -int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; - -static const char release = 'R'; -static const char move = 'M'; -static const char change = 'A'; -static const char speed = 'S'; -static const char right = 'R'; -static const char left = 'L'; -static const char up = 'U'; -static const char down = 'D'; -static const char wrist = 'W'; -static const char garbage = 'G'; -static const char faster = 'U'; -static const char slower = 'D'; - -int stepPins[8] = {11, 9, 5, 7, 1, 3, 1, 3}; -int dirPins[8] = {10, 8, 4, 6, 0, 2, 0, 2}; - -// limit switch pins -int limPins[6] = {23, 22, 20, 21, 18, 19}; - -// pulses per revolution for motors -long ppr[6] = {400, 400, 400, 400, 400, 400}; - -// Gear Reduction Ratios -float red[6] = {30.0, 161.0, 44.8, 100, 57.34, 57.34}; - -// Encoder pulses per revolution -long pprEnc = 512; - -// Motor speeds and accelerations -int maxSpeed[8] = {1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200}; -int maxAccel[8] = {900, 3000, 4000, 3000, 5000, 5000, 5000, 5000}; -int homeSpeed[8] = {500, 1200, 600, 400, 2000, 2000, 2000, 2000}; // {500, 1500, 700, 1200, 1200, 1200, 1200, 1200}; -int homeAccel[8] = {500, 2000, 1500, 1000, 1500, 1500, 1500, 1500}; //{500, 2000, 1000, 1500, 1500, 1500, 1500, 1500}; - -// Range of motion (degrees) for each axis -int maxAngles[6] = {180, 70, 180, 120, 140, 100}; - -const unsigned long readInterval = 10; -unsigned long currentTime; -unsigned long previousTime = 0; - -// stepper motor objects for AccelStepper library -AccelStepper steppers[8]; - -// variable declarations -long max_steps[] = {red[0]*maxAngles[0]/360.0*ppr[0], red[1]*maxAngles[1]/360.0*ppr[1], red[2]*maxAngles[2]/360.0*ppr[2], red[3]*maxAngles[3]/360.0*ppr[3], red[4]*maxAngles[4]/360.0*ppr[4], red[5]*maxAngles[5]/360.0*ppr[5]}; -int axisDir[8] = {1, -1, 1, -1, 1, 1, -1, 1}; -int currentAxis = 1; -int runFlags[] = {0, 0, 0, 0, 0, 0}; -int i; -bool initFlag = false; - -// variables for homing / arm calibration -long homePosConst = -99000; -long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst, axisDir[6]*homePosConst, axisDir[7]*homePosConst}; -long homeCompAngles[] = {axisDir[0]*90, axisDir[1]*5, axisDir[2]*93, axisDir[3]*12, axisDir[4]*102, axisDir[5]*102, axisDir[6]*80, axisDir[7]*80}; -long homeCompConst[] = {2000, 2000, 1000, 500, 500, 500, 500, 500}; -long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5], axisDir[6]*homeCompConst[6], axisDir[7]*homeCompConst[7]}; -long homeCompSteps[] = {homeCompAngles[0]*red[0]*ppr[0]/360.0, homeCompAngles[1]*red[1]*ppr[1]/360.0, homeCompAngles[2]*red[2]*ppr[2]/360.0, homeCompAngles[3]*red[3]*ppr[3]/360.0, homeCompAngles[4]*red[4]*ppr[4]/360.0, homeCompAngles[5]*red[5]*ppr[5]/360.0, homeCompAngles[6]*red[4]*ppr[4]/360.0, homeCompAngles[7]*red[5]*ppr[5]/360.0}; -char value; - -// values for changing speed -const int maxSpeedIndex = 2; -int speedVals[maxSpeedIndex+1][NUM_AXES_EFF] = {{600, 900, 1500, 1250, 1050, 1050, 1050, 1050}, {900, 1200, 2000, 1665, 1460, 1460, 1460, 1460}, {1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200}}; -int speedIndex = maxSpeedIndex; - - - -void setup() { // setup function to initialize pins and provide initial homing to the arm + +// header file with all constants defined and libraries included +#include "armFirmware.h" + +// setup function to initialize pins and provide initial homing to the arm. +void setup() { Serial.begin(9600); + for (int i = 0; i < NUM_AXES; i++) { + ENC_STEPS_PER_DEG[i] = ppr[i] * red[i] * (ENC_MULT[i] / 360.0); + + min_steps[i] = -homeCompSteps[i]; + max_steps[i] = max_steps[i] - homeCompSteps[i]; + } + + min_steps[0] = -max_steps[0]; + + // initializes end effector motor + pinMode(EEstepPin, OUTPUT); + pinMode(EEdirPin, OUTPUT); + endEff.setMinPulseWidth(200); + endEff.setMaxSpeed(speedEE); + endEff.setAcceleration(accEE); + endEff.setCurrentPosition(1000); + // initializes step pins, direction pins, limit switch pins, and stepper motor objects for accelStepper library - for(i = 0; i readInterval) { - recieveCommand(); - previousTime = currentTime; - } + // receives command from serial and executes accoringly + recieveCommand(); -runSteppers(); + // run steppers to target position + runSteppers(); } -void recieveCommand() -{ +void recieveCommand() { String inData = ""; - char recieved = garbage; - if(Serial.available()>0) - { + char recieved = '\0'; + + // if a message is present, copy it to inData + if (Serial.available() > 0) { do { recieved = Serial.read(); inData += String(recieved); - } while(recieved != '\n'); + } while (recieved != '\n'); } - if(recieved == '\n') - { + // parse the received data + if (recieved == '\n') { parseMessage(inData); } } -void parseMessage(String inMsg) -{ +void parseMessage(String inMsg) { String function = inMsg.substring(0, 2); - - if(function == "MT") - { + + // choose which function to run + if (function == "MT") { cartesianCommands(inMsg); } - else if(function == "JM") - { + else if (function == "JM") { jointCommands(inMsg); } - else if(function == "EE") - { + else if (function == "EE") { endEffectorCommands(inMsg); } - else if(function == "DM") - { - drillCommands(inMsg); + else if (function == "PM") { + executePose(inMsg); } - else if(function == "HM") - { + else if (function == "HM") { homeArm(); } -} -//****//CARTESIAN MODE FUNCTIONS//****// - -void cartesianCommands(String inMsg) -{ - // read current joint positions - readEncPos(curEncSteps); + // Send arm angles and gripper force to hardware driver + sendArmFeedback(); +} - // update host with joint positions - sendCurrentPosition(); +void executePose(String inMsg) { + int completeCount = 0; - // get new position commands int msgIdxJ1 = inMsg.indexOf('A'); int msgIdxJ2 = inMsg.indexOf('B'); int msgIdxJ3 = inMsg.indexOf('C'); int msgIdxJ4 = inMsg.indexOf('D'); int msgIdxJ5 = inMsg.indexOf('E'); int msgIdxJ6 = inMsg.indexOf('F'); - cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt(); - cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt(); - cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt(); - cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt(); - cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt(); - cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt(); - - // update target joint positions - readEncPos(curEncSteps); - for (int i = 0; i < NUM_AXES; i++) - { - int diffEncSteps = cmdEncSteps[i] - curEncSteps[i]; - if (abs(diffEncSteps) > 2) - { - int diffMotSteps = diffEncSteps / ENC_MULT[i]; - if (diffMotSteps < MOTOR_STEPS_PER_REV[i]) - { - // for the last rev of motor, introduce artificial decceleration - // to help prevent overshoot - diffMotSteps = diffMotSteps / 2; - } - steppers[i].move(diffMotSteps); - } - } -} + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt() * IK_DIR[0]; + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt() * IK_DIR[1]; + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt() * IK_DIR[2]; + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt() * IK_DIR[3]; + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt() * IK_DIR[4]; + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt() * IK_DIR[5]; -// parses which commands to execute when in joint space mode -void jointCommands(String inMsg) -{ - char function = inMsg[2]; - char detail1 = inMsg[3]; - switch(function) - { - case release: releaseEvent(detail1, inMsg[4]); break; - case speed: changeSpeed(detail1); break; - case axis: changeAxis(detail1); break; - case move: jointMovement(detail1, inMsg[4]); break; + while (completeCount != NUM_AXES) { + readEncPos(curEncSteps); + completeCount = cmdArm(); + runSteppers(); } } -void endEffectorCommands(String inMsg) -{ - // fill with end effector commands depending on substring -} +void sendArmFeedback() { -void drillMotion(String inMsg) -{ - // fill with calls to drill functions depending on substring -} + char garbage; + while (Serial.available() > 0) { + garbage = Serial.read(); + } + + int gripperForce = 0; //readGripperForce(); + readEncPos(curEncSteps); -void sendCurrentPosition() -{ String outMsg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2]) - + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n"); - Serial.print(outMsg); + + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("G") + String(gripperForce) + String("Z"); + Serial.print(outMsg); } -// Sets movement target positions when in joint space mode -void jointMovement(char joystick, char dir) -{ - if(joystick == wrist) - { - switch(dir) - { - case up: runWrist(FWD, 5); break; - case down: runWrist(REV, 5); break; - case left: runWrist(FWD, 6); break; - case right: runWrist(REV, 6); break; - } +void runSteppers() { + + for (i = 0; i < NUM_AXES; i++) { + steppers[i].run(); } - else if(joystick == left) + endEff.run(); +} + +//****//JOINT MODE FUNCTIONS//****// + +// Parses which commands to execute when in joint space mode +void jointCommands(String inMsg) { + char function = inMsg[2]; + char axis = inMsg[3]; + char dir = inMsg[4]; + + if (!jointFlag) // Check if we are switching from cartesian mode { - switch(dir) - { - case left: runAxes(FWD, currentAxis); break; - case right: runAxes(REV, currentAxis); break; - } + IKFlag = false; + jointFlag = true; + setJointSpeed(); } - else - { - switch(dir) - { - case up: runAxes(FWD, currentAxis+1); break; - case down: runAxes(REV, currentAxis+1); break; - } - } + if (function == move) + moveArm(axis, dir); + + else if (function == release) + relArm(axis); } -//****//ENCODER RELATED FUNCTIONS//****// +void moveArm(char axis, char dir) { + int axisNum = String(axis).toInt(); // String to int for math -void readEncPos(int* encPos) -{ - encPos[0] = J1encPos.read() * ENC_DIR[0]; - encPos[1] = J2encPos.read() * ENC_DIR[1]; - encPos[2] = J3encPos.read() * ENC_DIR[2]; - encPos[3] = J4encPos.read() * ENC_DIR[3]; - encPos[4] = J5encPos.read() * ENC_DIR[4]; - encPos[5] = J6encPos.read() * ENC_DIR[5]; + if ((dir == left) || (dir == up)) { + moveAxis((axisNum - 1), FWD); + } else if ((dir == right) || (dir == down)) { + moveAxis((axisNum - 1), REV); + } } -//****//JOINT SPACE MODE FUNCTIONS//****// - -// sets target position for axes in joint space mode -void runAxes(int dir, int axis) { // assigns run flags to indicate forward / reverse motion and sets target position +void relArm(char axis) { + int axisNum = String(axis).toInt(); // String to int for math + steppers[axisNum - 1].stop(); +} - if(axis == 3) { +void moveAxis(int axis, int dir) { + if ((axis == 0) || (axis == 1)) { // switching direction if axis 1 or 2 (arm moves in intuitive dir) dir = !dir; } - - if(runFlags[axis-1] == 1 && dir == FWD) { - } - else if(runFlags[axis-1] == -1 && dir == REV) { - } - - else if(dir == FWD) { - steppers[axis-1].moveTo(max_steps[axis-1]*axisDir[axis-1]); - runFlags[axis-1] = 1; - } + if (dir == FWD) { // sets stepper to move to max steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(max_steps[axis]); - else { - steppers[axis-1].moveTo(0); - runFlags[axis-1] = -1; + } else if (dir == REV) { // sets stepper to move to min steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(min_steps[axis]); } } -void runWrist(int dir, int axis) { // assigns target position for selected axis based on user input. - - if(axis == 5) { // axis 5 motion -> both wrist motors spin in opposite directions - if(runFlags[5] == 1 && dir == FWD) { - } +// sets Joint Mode speeds after switching out of cartesian mode +void setJointSpeed() { + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setMaxSpeed(maxSpeed[i]); + steppers[i].setAcceleration(maxAccel[i]); + } +} - else if(runFlags[5] == -1 && dir == REV) { - } - - else if(dir == FWD) { - steppers[6].moveTo(axisDir[6]*max_steps[5]); - steppers[7].moveTo(axisDir[7]*max_steps[5]); - runFlags[5] = 1; - } +//***//CARTESIAN MODE FUNCTIONS//***// - else { - steppers[6].moveTo(0); - steppers[7].moveTo(0); - runFlags[5] = -1; - } +void cartesianCommands(String inMsg) { + if (jointFlag) { + IKFlag = true; + jointFlag = false; + setCartesianSpeed(); } - else if(axis == 6) { // axis 6 motion -> both wrist motors spin in same direction - dir = !dir; - if(runFlags[4] == 1 && dir == FWD) { - } + // get new position commands - else if(runFlags[4] == -1 && dir == REV) { - } - - else if(dir == FWD) { - steppers[4].moveTo(axisDir[4]*max_steps[4]); - steppers[5].moveTo(axisDir[5]*max_steps[4]); - runFlags[4] = 1; - } + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt() * IK_DIR[0]; + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt() * IK_DIR[1]; + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt() * IK_DIR[2]; + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt() * IK_DIR[3]; + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt() * IK_DIR[4]; + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt() * IK_DIR[5]; - else { - steppers[4].moveTo(0); - steppers[5].moveTo(0); - runFlags[4] = -1; - } - } + readEncPos(curEncSteps); + cmdArm(); } -void changeAxis(int dir) { // when user hits specified button, axis targets change +void setCartesianSpeed() { + float JOINT_MAX_SPEED[NUM_AXES]; + float MOTOR_MAX_SPEED[NUM_AXES]; + float JOINT_MAX_ACCEL[NUM_AXES]; + float MOTOR_MAX_ACCEL[NUM_AXES]; - if((dir == up) && (currentAxis == 1)) - { - currentAxis = 3; - zeroRunFlags(); - } + for (int i = 0; i < NUM_AXES; i++) { + JOINT_MAX_SPEED[i] = IKspeeds[i] * (180.0 / 3.14159); + JOINT_MAX_ACCEL[i] = IKaccs[i] * (180.0 / 3.14159); + MOTOR_MAX_SPEED[i] = JOINT_MAX_SPEED[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + MOTOR_MAX_ACCEL[i] = JOINT_MAX_ACCEL[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + steppers[i].setAcceleration(MOTOR_MAX_ACCEL[i]); + steppers[i].setMaxSpeed(MOTOR_MAX_SPEED[i]); + } +} - else if((dir == down) && (currentAxis == 3)) - { - currentAxis = 1; - zeroRunFlags(); +// Set target position for cartesian movements of arm +int cmdArm() { + int count = 0; + + for (int i = 0; i < NUM_AXES; i++) { + int diffEncSteps = cmdEncSteps[i] - curEncSteps[i]; + if (abs(diffEncSteps) > 10) { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < ppr[i]) { + diffMotSteps = diffMotSteps / 2; + } + steppers[i].move(diffMotSteps); + } else { + count++; } + } + return count; } -void releaseEvent(char joystick, char dir) { // when user releases a joystick serial sends a character +//***// END EFFECTOR RELATED FUNCTIONS //***// - if(joystick == wrist) - { - if ((dir == up) || (dir == down)) - { - steppers[6].stop(); - steppers[7].stop(); - runFlags[5] = 0; - } +void endEffectorCommands(String inMsg) { + char data = inMsg[2]; - else - { - steppers[4].stop(); - steppers[5].stop(); - runFlags[4] = 0; - } + //opening code + if (data == open) { //check if open button pressed and if force is less than max + endEff.moveTo(openPos * MOTOR_DIR_EE); //continue to move to open position } - else if(joystick == left) - { - steppers[currentAxis-1].stop(); - runFlags[currentAxis-1].stop(); + //closing code + else if (data == close) { //check if open button pressed and if force is less than max + endEff.moveTo(closePos * MOTOR_DIR_EE); //continue to move to closed position } - else - { - steppers[currentAxis].stop(); - runFlags[currentAxis].stop(); + else if (data == release) { //else check if release button pressed + endEff.stop(); // stop when above condition reached } -} -void changeSpeed(char speedVal) { // changes speed of all axes based on user input - - if(speedVal == faster){ - if(speedIndex < maxSpeedIndex) { - speedIndex++; - for(i=0;i 0) { - speedIndex--; - for(i=0;i 0) - { - recieved = Serial.read(); - inData += String(recieved) - if(recieved == '\n') - { - serialFlag = true; - } +void waitForHome() { + String inData; + char recieved = '\0'; + + while (!initFlag) { + inData = ""; + if (Serial.available() > 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); } - if(serialFlag) - { - if(inData = "HM") - { - homeArm(); - initFlag = true; - } - - else - { - inData = ""; - serialFlag = false; - } + if (recieved == '\n') { + if (inData.substring(0, 2) == "HM") + homeArm(); + initFlag = true; } } } \ No newline at end of file diff --git a/src/teensy-firmware/src/armFirmware/armFirmware.h b/src/teensy-firmware/src/armFirmware/armFirmware.h new file mode 100644 index 000000000..8c2006c5a --- /dev/null +++ b/src/teensy-firmware/src/armFirmware/armFirmware.h @@ -0,0 +1,184 @@ +/* +Created By: Tate Kolton, Graeme Dockrill +Created On: November 11, 2022 +Updated On: January 10, 2023 +Description: Header file for firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU +*/ + +#include +#include + +// general parameters +#define SIM 0 //firmware simulation + +#define NUM_AXES 6 +#define ON 0 +#define OFF 1 +#define SW_ON 0 +#define SW_OFF 1 +#define FWD 1 +#define REV 0 + + + +static const char release = 'R'; +static const char move = 'M'; +static const char right = 'R'; +static const char left = 'L'; +static const char up = 'U'; +static const char down = 'D'; +static const char open = 'O'; +static const char close = 'C'; +static const char EEval = 'E'; +static const char homeValEE = 'H'; + +// Motor pins +int stepPins[6] = {6, 8, 2, 10, 12, 25}; +int dirPins[6] = {5, 7, 1, 9, 11, 24}; + +// Encoder pins +int encPinA[6] = {17, 38, 40, 36, 13, 14}; +int encPinB[6] = {16, 37, 39, 35, 41, 15}; + +// limit switch pins +int limPins[6] = {18, 19, 20, 21, 23, 22}; + +// pulses per revolution for motors +long ppr[6] = {400, 400, 400, 400, 400, 400}; + +// Gear Reductions +float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 14.0}; + +// End effector variables +const int closePos = 0; +const int openPos = 500; +const int EEstepPin = 4; +const int EEdirPin = 3; +const int speedEE = 500; +const int accEE = 500; +const int MOTOR_DIR_EE = 1; +const int forcePin = 12; + + +// Encoder Variables +int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; +const int pprEnc = 512; +const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12}; +float ENC_STEPS_PER_DEG[NUM_AXES]; + +// Motor speeds and accelerations +const int maxSpeed[6] = {1000, 1800, 2000, 2000, 3000, 2000}; +const int maxAccel[6] = {2000, 3000, 3000, 3300, 4000, 3000}; +const int homeSpeed[6] = {1500, 2000, 2000, 2500, 1500, 1000}; +const int homeAccel[6] = {1500, 2000, 2000, 2500, 1500, 1000}; + +// Stepper motor objects for AccelStepper library +AccelStepper steppers[6]; +AccelStepper endEff(1, EEstepPin, EEdirPin); + +// Encoder objects for Encoder library +Encoder enc1(encPinA[0], encPinB[0]); +Encoder enc2(encPinA[1], encPinB[1]); +Encoder enc3(encPinA[2], encPinB[2]); +Encoder enc4(encPinA[3], encPinB[3]); +Encoder enc5(encPinA[4], encPinB[4]); +Encoder enc6(encPinA[5], encPinB[5]); + +// General Global Variable declarations +const int axisDir[6] = {1, -1, -1, 1, -1, -1}; +const int IK_DIR[6] = {1, 1, 1, 1, 1, 1}; +int i; +bool initFlag = false; +bool jointFlag = false; +bool IKFlag = false; +bool resetEE = false; + +// Variables for homing / arm calibration +long homePosConst = -99000; +long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst}; +long homeCompAngles[] = {50, 0, 0, 100, 100, 90}; +long homeCompConst[] = {500, 1000, 1000, 500, 500, 200}; +long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5]}; +long homeCompSteps[] = {axisDir[0]*homeCompAngles[0]*red[0]*ppr[0]/360.0, axisDir[1]*homeCompAngles[1]*red[1]*ppr[1]/360.0, axisDir[2]*homeCompAngles[2]*red[2]*ppr[2]/360.0, axisDir[3]*homeCompAngles[3]*red[3]*ppr[3]/360.0, axisDir[4]*homeCompAngles[4]*red[4]*ppr[4]/360.0, axisDir[5]*homeCompAngles[5]*red[5]*ppr[5]/360.0}; + +// Range of motion (degrees) for each axis +int maxAngles[6] = {200, 100, 180, 180, 180, 290}; +long max_steps[] = {axisDir[0]*red[0]*maxAngles[0]/360.0*ppr[0], axisDir[1]*red[1]*maxAngles[1]/360.0*ppr[1], axisDir[2]*red[2]*maxAngles[2]/360.0*ppr[2], axisDir[3]*red[3]*maxAngles[3]/360.0*ppr[3], axisDir[4]*red[4]*maxAngles[4]/360.0*ppr[4], axisDir[5]*red[5]*maxAngles[5]/360.0*ppr[5]}; +long min_steps[NUM_AXES]; +char value; + +// Cartesian mode speed settings +float IKspeeds[] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2}; +float IKaccs[] = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; + + +//****// FUNCTION PROTOTYPES //****// + +// Waits for a message to be published to the serial port. +void recieveCommand(); + +// Parses the message passed to it and determines which subfunction to call for control of arm. +void parseMessage(String inMsg); + + //For closed loop control. Reads and sends current position. Sets new position based on feedback while the arm is in cartesian mode. +void cartesianCommands(String inMsg); + +// Sets the max speed and acceleration for each joint and motor to its Inverse Kinematic speed. +void setCartesianSpeed(); + +// Parses which commands to execute when in joint space mode. +void jointCommands(String inMsg); + +// Sets the max speed and acceleration for each joint and motor to its Joint Mode speed. +void setJointSpeed(); + +// Takes in the axis to move and the direction, and calls moveAxis in FWD or REV. +void moveArm(char axis, char dir); + +// Stops the passed axis and sets its run flag to 0. +void relArm(char axis); + +// Changes the axis target position and sets run flag to 1 or -1 depending on dir. +void moveAxis(int axis, int dir); + +// Opens, closes, releases, or homes the end effector depending on the data in inMsg [2]. +void endEffectorCommands(String inMsg); + +// Selects which drill function to execute based on inMsg[2]. +void drillCommands(String inMsg); + +// Prints the current encoder position for each axis to serial. +void sendArmFeedback(); + +// Reads each encoder's position and stores them in encPos[]. +void readEncPos(int* encPos); + +// Zeroes each encoder at its current position. +void zeroEncoders(); + +// For closed loop movement (Inverse Kin Mode) +int cmdArm(); + +// Main function for homing the entire arm. +void homeArm(); + +// Runs through and checks if each axis has reached its limit switch, then runs it a couple steps away from switch for 0 position. +void homeAxes(); + +// sets homing speed and acceleration for axes 1-6 and sets target homing position. +void initializeHomingMotion(); + +// sets main program speeds for each axis. +void initializeMotion(); + +// Runs all stepper motors to target position (if no target defined, motors will not move). +void runSteppers(); + +// Waits for the user to press the home button before continuing with other functions. Reads serial port and homes the arm if "HM___" is read. +void waitForHome(); + +// Reads end effector gripper force +int readGripperForce(); + +// Move arm to GUI specified pose +void executePose(String inMsg); diff --git a/src/teensy-firmware/src/armFirmware/armFirmware.ino b/src/teensy-firmware/src/armFirmware/armFirmware.ino index 2002cde82..d191271a1 100644 --- a/src/teensy-firmware/src/armFirmware/armFirmware.ino +++ b/src/teensy-firmware/src/armFirmware/armFirmware.ino @@ -1,192 +1,27 @@ /* -Created By: Tate Kolton, Rowan Zawadzki +Created By: Tate Kolton, Graeme Dockrill Created On: December 21, 2021 -Updated: April 27, 2022 +Updated On: January 10, 2023 Description: Main firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU */ - -#include -#include -#include - -// general parameters -#define NUM_AXES 6 -#define NUM_AXES_EX_WRIST 4 -#define NUM_AXES_EFF 8 -#define NUM_PARAMS 7 -#define ON 0 -#define OFF 1 -#define SW_ON 0 -#define SW_OFF 1 -#define FWD 1 -#define REV 0 - -static const char release = 'R'; -static const char move = 'M'; -static const char change = 'A'; -static const char speed = 'S'; -static const char right = 'R'; -static const char left = 'L'; -static const char up = 'U'; -static const char down = 'D'; -static const char wrist = 'W'; -static const char garbage = 'G'; -static const char faster = 'U'; -static const char slower = 'D'; -static const char prepare = 'P'; -static const char collect = 'C'; -static const char deposit = 'D'; -static const char manual = 'M'; -static const char drillRelease = 'X'; -static const char open = 'O'; -static const char close = 'C'; -static const char joint = 'J'; -static const char EEval = 'E'; -static const char homeValEE = 'H'; -static const char moveBase = 'T'; -static const char moveWrist = 'M'; -static const char relWrist = 'R'; -static const char relBase = 'W'; - -// Motor variables -int stepPins[8] = {6, 8, 10, 2, 12, 25, 12, 25}; -int dirPins[8] = {5, 7, 9, 1, 11, 24, 11, 24}; -int stepPinsIK[6] = {6, 8, 2, 10, 25, 12}; -int dirPinsIK[6] = {5, 7, 1, 9, 24, 11}; -int encPinA[6] = {17, 38, 40, 36, 15, 13}; -int encPinB[6] = {16, 37, 39, 35, 14, 41}; - -// end effector variables -const float calibrationFactor = -111.25; -float force; -HX711 scale; -const int dataPin = 34; -const int clkPin = 33; -int calPos = 0; -int closePos = 0; -int openPos = 500; // needs to be calibrated -int EEstepPin = 4; -int EEdirPin = 3; -int speedEE = 100; -int accEE = 500; -int speedDrill = 3000; -int accDrill = 1000; -const int MOTOR_DIR_EE = 1; -const int openButton = 5; -const int closeButton = 4; -const float calForce = 0.3; -const float maxForce = 10.0; -float EEforce; -int forcePct = 0; - -// limit switch pins -int limPins[6] = {18, 19, 21, 20, 23, 22}; - -// pulses per revolution for motors -long ppr[6] = {400, 400, 400, 400, 400, 400}; - -// Gear Reduction Ratios -float red[6] = {50.0, 161.0, 44.8, 93.07, 57.34, 57.34}; -float redIK[6] = {50.0, 161.0, 93.07, 44.8, 57.34, 57.34}; - - -// Encoder Variables -int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; -int pprEnc = 512; -int ENC_DIR[6] = {-1, -1, -1, -1, 1, 1}; -const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12}; -float ENC_STEPS_PER_DEG[NUM_AXES]; - -// Motor speeds and accelerations -int maxSpeed[8] = {1200, 1800, 3000, 2500, 2200, 2200, 2200, 2200}; -int maxAccel[8] = {1300, 3500, 4600, 3300, 5000, 5000, 5000, 5000}; -int homeSpeed[8] = {300, 1000, 1000, 400, 2000, 2000, 2000, 2000}; // {500, 1200, 600, 400, 2000, 2000, 2000, 2000}; -int homeAccel[8] = {500, 2000, 1500, 1000, 1500, 1500, 1500, 1500}; //{500, 2000, 1000, 1500, 1500, 1500, 1500, 1500}; - - -// Time variables -const unsigned long readInterval = 10; -unsigned long currentTime; -unsigned long currentTimeJP; -unsigned long currentTimeEE; -unsigned long previousTime = 0; -unsigned long previousTimeEE = 0; -const unsigned long timeIntervalEE = 500; -const unsigned long timeIntervalJP = 250; -unsigned long previousTimeJP = 0; - -// stepper motor objects for AccelStepper library -AccelStepper steppers[8]; -AccelStepper endEff(1, EEstepPin, EEdirPin); -AccelStepper steppersIK[8]; - - -Encoder enc1(encPinA[0], encPinB[0]); -Encoder enc2(encPinA[1], encPinB[1]); -Encoder enc3(encPinA[2], encPinB[2]); -Encoder enc4(encPinA[3], encPinB[3]); -Encoder enc5(encPinA[4], encPinB[4]); -Encoder enc6(encPinA[5], encPinB[5]); - -Encoder encoders[] = {enc1, enc2, enc3, enc4, enc5, enc6}; - -// variable declarations - -int axisDir[8] = {1, -1, 1, -1, 1, 1, -1, 1}; -int axisDirIK[6] = {-1, -1, -1, 1, -1, -1}; -int currentAxis = 1; -int runFlags[] = {0, 0, 0, 0, 0, 0}; -int i; -bool initFlag = false; -bool jointFlag = true; -bool IKFlag = false; -bool J1Flag = false; -bool resetEE = false; -bool vertFlag = false; -bool horizFlag = false; - -// variables for homing / arm calibration -long homePosConst = -99000; -long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst, axisDir[6]*homePosConst, axisDir[7]*homePosConst}; -long homeCompAngles[] = {axisDir[0]*54, axisDir[1]*10, axisDir[2]*90, axisDir[3]*1, axisDir[4]*85, axisDir[5]*85, axisDir[6]*170, axisDir[7]*170}; -long homeCompConst[] = {500, 2000, 1000, 500, 500, 500, 500, 500}; -long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5], axisDir[6]*homeCompConst[6], axisDir[7]*homeCompConst[7]}; -long homeCompSteps[] = {homeCompAngles[0]*red[0]*ppr[0]/360.0, homeCompAngles[1]*red[1]*ppr[1]/360.0, homeCompAngles[2]*red[2]*ppr[2]/360.0, homeCompAngles[3]*red[3]*ppr[3]/360.0, homeCompAngles[4]*red[4]*ppr[4]/360.0, homeCompAngles[5]*red[5]*ppr[5]/360.0, homeCompAngles[6]*red[4]*ppr[4]/360.0, homeCompAngles[7]*red[5]*ppr[5]/360.0}; -// Range of motion (degrees) for each axis -int maxAngles[6] = {190, 160, 180, 120, 160, 180}; -long max_steps[] = {axisDir[0]*red[0]*maxAngles[0]/360.0*ppr[0], axisDir[1]*red[1]*maxAngles[1]/360.0*ppr[1], axisDir[2]*red[2]*maxAngles[2]/360.0*ppr[2], axisDir[3]*red[3]*maxAngles[3]/360.0*ppr[3], red[4]*maxAngles[4]/360.0*ppr[4], red[5]*maxAngles[5]/360.0*ppr[5]}; -long min_steps[NUM_AXES]; -char value; - -// values for changing speed -const int maxSpeedIndex = 2; -int speedVals[maxSpeedIndex+1][NUM_AXES_EFF] = {{600, 900, 1500, 1250, 1050, 1050, 1050, 1050}, {900, 1200, 2000, 1665, 1460, 1460, 1460, 1460}, {900, 1600, 2500, 2200, 2000, 2000, 2000, 2000}}; -int speedIndex = maxSpeedIndex; - -// Cartesian mode speed settings -float IKspeeds[] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2}; -float IKaccs[] = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; - -void setup() { // setup function to initialize pins and provide initial homing to the arm + +// header file with all constants defined and libraries included +#include "armFirmware.h" + + +// setup function to initialize pins and provide initial homing to the arm. +void setup() { Serial.begin(9600); - for(int i=0; i0) - { + // if a message is present, copy it to inData + if (Serial.available() > 0) { do { recieved = Serial.read(); inData += String(recieved); - } while(recieved != '\n'); + } while (recieved != '\n'); } - if(recieved == '\n') - { + // parse the received data + if (recieved == '\n') { parseMessage(inData); } } -void parseMessage(String inMsg) -{ +void parseMessage(String inMsg) { String function = inMsg.substring(0, 2); - - if(function == "MT") - { + + // choose which function to run + if (function == "MT") { cartesianCommands(inMsg); } - else if(function == "JM") - { + else if (function == "JM") { jointCommands(inMsg); } - else if(function == "EE") - { + else if (function == "EE") { endEffectorCommands(inMsg); } - else if(function == "DM") - { + else if (function == "DM") { drillCommands(inMsg); } - else if(function == "FB") - { + else if (function == "FB") { sendFeedback(inMsg); } - else if(function == "HM") - { + else if (function == "HM") { homeArm(); } } -void sendMessage(char outChar) -{ +void sendMessage(char outChar) { String outMsg = String(outChar); Serial.print(outMsg); } -void sendFeedback(String inMsg) -{ +void sendCurrentPosition() { + String outMsg; + if (!SIM) { + outMsg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2]) + + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("Z"); + } else { + outMsg = String("JP") + String("A") + String(steppers[0].currentPosition()) + String("B") + String(steppers[1].currentPosition()) + String("C") + String(steppers[2].currentPosition()) + + String("D") + String(steppers[3].currentPosition()) + String("E") + String(steppers[4].currentPosition()) + String("F") + String(steppers[5].currentPosition() + String("Z")); + } + + Serial.print(outMsg); +} + +void sendFeedback(String inMsg) { char function = inMsg[2]; - if(function == joint) - { + // if in joint mode, send feedback over serial + if (function == joint) { readEncPos(curEncSteps); sendCurrentPosition(); } } -//****//CARTESIAN MODE FUNCTIONS//****// +void runSteppers() { -void cartesianCommands(String inMsg) -{ - // read current joint positions - readEncPos(curEncSteps); + for (i = 0; i < NUM_AXES; i++) { + steppers[i].run(); + } - // update host with joint positions - sendCurrentPosition(); + endEff.run(); +} + +//****//JOINT MODE FUNCTIONS//****// + +// Parses which commands to execute when in joint space mode +void jointCommands(String inMsg) { + char function = inMsg[2]; + char detail1 = inMsg[3]; - if(!IKFlag) + if (!jointFlag) // Check if we are switching from cartesian mode { - setCartesianSpeed(); + IKFlag = false; + jointFlag = true; + cartesianToJoint(); + setJointSpeed(); + } + + if (function == move) + moveArm(detail1, inMsg[4]); + + else if (function == release) + relArm(detail1); +} + +void moveArm(char axis, char dir) { + int axisNum = String(axis).toInt(); // String to int for math + + if ((dir == left) || (dir == up)) { + moveAxis((axisNum - 1), FWD); + } else if ((dir == right) || (dir == down)) { + moveAxis((axisNum - 1), REV); + } +} + +void relArm(char axis) { + int axisNum = String(axis).toInt(); // String to int for math + steppers[axisNum - 1].stop(); + runFlags[axis] = 0; +} + +void moveAxis(int axis, int dir) { + if ((axis == 0) || (axis == 1)) { // switching direction if axis 1 or 2 (arm moves in intuitive dir) + dir = !dir; + } + + if (dir == FWD && runFlags[axis] != 1) { // sets stepper to move to max steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(max_steps[axis]); + runFlags[axis] = 1; + } else if (dir == REV && runFlags[axis] != -1) { // sets stepper to move to min steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(min_steps[axis]); + runFlags[axis] = -1; + } +} + +// sets Joint Mode speeds after switching out of cartesian mode +void setJointSpeed() { + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setMaxSpeed(speedVals[maxSpeedIndex][i]); + steppers[i].setAcceleration(maxAccel[i]); + } +} + +void cartesianToJoint() { + readEncPos(curEncSteps); + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setCurrentPosition(curEncSteps[i] / ENC_MULT[i]); + } +} + +//***//CARTESIAN MODE FUNCTIONS//***// + +void cartesianCommands(String inMsg) { + + if (jointFlag) { IKFlag = true; jointFlag = false; + setCartesianSpeed(); } + // read current joint positions + readEncPos(curEncSteps); + + // update host with joint positions + sendCurrentPosition(); // get new position commands int msgIdxJ1 = inMsg.indexOf('A'); @@ -333,286 +237,141 @@ void cartesianCommands(String inMsg) // update target joint positions readEncPos(curEncSteps); - cmdArmBase(); - cmdArmWrist(); + cmdArm(); } -void setCartesianSpeed() -{ +void setCartesianSpeed() { float JOINT_MAX_SPEED[NUM_AXES]; float MOTOR_MAX_SPEED[NUM_AXES]; float JOINT_MAX_ACCEL[NUM_AXES]; float MOTOR_MAX_ACCEL[NUM_AXES]; - for(int i=0; i 5) { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < ppr[i]) { + diffMotSteps = diffMotSteps / 2; + } -// converts cartesian angles to joint space stepper current positions -void cartesianToJointSpace() -{ - long curJointPos[NUM_AXES_EFF]; - readEncPos(curEncSteps); - curJointPos[0] = curEncSteps[0]/5.12; - curJointPos[1] = curEncSteps[1]/5.12; - curJointPos[3] = curEncSteps[2]/5.12; - curJointPos[2] = curEncSteps[3]/5.12; - curJointPos[6] = curEncSteps[4]/5.12; - curJointPos[7] = curEncSteps[4]/5.12; - curJointPos[4] = curEncSteps[5]/5.12; - curJointPos[5] = curEncSteps[5]/5.12; - - for(int i=0; i 6) - { - int diffMotSteps = diffEncSteps / ENC_MULT[i]; - if (diffMotSteps < ppr[i]) - { - // for the last rev of motor, introduce artificial decceleration - // to help prevent overshoot - diffMotSteps = diffMotSteps / 2; - } - - steppersIK[i].move(diffMotSteps*axisDirIK[i]); - } - } -} - -void cmdArmWrist() -{ - int diffMotStepsA5, diffMotStepsA6, diffEncStepsA5, diffEncStepsA6; - - diffEncStepsA5 = cmdEncSteps[4] - curEncSteps[4]; - diffEncStepsA6 = cmdEncSteps[5] - curEncSteps[5]; - - if(abs(diffEncStepsA5) > 10) - { - diffMotStepsA5 = diffEncStepsA5 / ENC_MULT[4]; - if(diffMotStepsA5 < ppr[4]) - { - diffMotStepsA5 = diffEncStepsA5 / 2; - } - } - - if(abs(diffEncStepsA6) > 10) - { - diffMotStepsA6 = diffEncStepsA6 / ENC_MULT[5]; - if(diffMotStepsA6 < ppr[5]) - { - diffMotStepsA6 = diffEncStepsA6 / 2; - } - } - - int actualMotStepsA5 = diffMotStepsA6/2 - diffMotStepsA5/2; - int actualMotStepsA6 = diffMotStepsA6/2 + diffMotStepsA5/2; - - steppersIK[4].move(actualMotStepsA5*axisDirIK[4]); - steppersIK[5].move(actualMotStepsA6*axisDirIK[5]); -} - - - -//****//JOINT SPACE MODE FUNCTIONS//****// - -// sets target position for axes in joint space mode -void runAxes(int dir, int axis) { // assigns run flags to indicate forward / reverse motion and sets target position - - if((axis == 3) || (axis == 1) || (axis == 2)) { - dir = !dir; - } - - if(runFlags[axis-1] == 1 && dir == FWD) { - } - - else if(runFlags[axis-1] == -1 && dir == REV) { - } - - else if(dir == FWD) { - steppers[axis-1].moveTo(max_steps[axis-1]); - runFlags[axis-1] = 1; - } - - else { - steppers[axis-1].moveTo(min_steps[axis-1]); - runFlags[axis-1] = -1; - } -} - -void runWrist(int dir, int axis) { // assigns target position for selected axis based on user input. - - if(axis == 5) { - if(runFlags[5] == 1 && dir == FWD) { - } - - else if(runFlags[5] == -1 && dir == REV) { - } - - else if(dir == FWD) { - steppers[6].moveTo(axisDir[6]*max_steps[5]); - steppers[7].moveTo(axisDir[7]*max_steps[5]); - runFlags[5] = 1; - } - - else { - steppers[6].moveTo(axisDir[6]*min_steps[5]); - steppers[7].moveTo(axisDir[7]*min_steps[5]); - runFlags[5] = -1; - } - } - - else if(axis == 6) { - dir = !dir; - if(runFlags[4] == 1 && dir == FWD) { - } - - else if(runFlags[4] == -1 && dir == REV) { - } - - else if(dir == FWD) { - steppers[4].moveTo(axisDir[4]*max_steps[4]); - steppers[5].moveTo(axisDir[5]*max_steps[4]); - runFlags[4] = 1; - } - - else { - steppers[4].moveTo(axisDir[4]*min_steps[4]); - steppers[5].moveTo(axisDir[5]*min_steps[4]); - runFlags[4] = -1; - } - } -} - -void changeAxis(int dir) { // when user hits specified button, axis targets change - - if((dir == up) && (currentAxis == 1)) - { - currentAxis = 3; - zeroRunFlags(); - } - - else if((dir == down) && (currentAxis == 3)) - { - currentAxis = 1; - zeroRunFlags(); - } -} - -void releaseEvent(char joystick, char dir) { // when user releases a joystick serial sends a character - - if(joystick == wrist) - { - if ((dir == left) || (dir == right)) - { - steppers[6].stop(); - steppers[7].stop(); - runFlags[5] = 0; - } - - else - { - steppers[4].stop(); - steppers[5].stop(); - runFlags[4] = 0; - } - } - - else if(joystick == left) - { - steppers[currentAxis-1].stop(); - runFlags[currentAxis-1] = 0; - } - - else if(joystick == right) - { - steppers[currentAxis].stop(); - runFlags[currentAxis] = 0; - } -} - -void changeSpeed(char speedVal) { // changes speed of all axes based on user input - - if(speedVal == faster){ - if(speedIndex < maxSpeedIndex) { - speedIndex++; - for(i=0;i 0) { - speedIndex--; - for(i=0;i 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); } - else if(!calibFlags[1]) { - calibFlags[1] = true; - completeFlag = true; + if (recieved == '\n') { + if (inData.substring(0, 2) == "HM") + homeArm(); + initFlag = true; } } } + +// OLD FUNCTIONS THAT MAY BE USED IN THE FIRMWARE // + //void homeEE() //{ // EEforce=scale.get_units()/1000*9.81; @@ -971,9 +508,9 @@ void homeWrist() { // homes axes 5-6 // // target position for end effector in closed direction // endEff.move(-99000*MOTOR_DIR_EE); // -// while(abs(EEforce) < calForce) +// while(abs(EEforce) < calForce) // { -// if (scale.wait_ready_timeout(1)) { +// if (scale.wait_ready_timeout(1)) { // EEforce=scale.get_units()/1000*9.81; //converting mass to force // // close end effector // } @@ -988,110 +525,23 @@ void homeWrist() { // homes axes 5-6 // } //} -void initializeHomingMotion() { // sets homing speed and acceleration for axes 1-4 and sets target homing position - - for(i = 0; i (homeCompSteps[0] + homeComp[0])/axisDir[0])) - { - steppers[0].move(-homePos[i]); - } - else - { - steppers[0].move(homePos[i]); - } - } - else - { - steppers[0].move(homePos[i]); - } - } -} - -void initializeWristHomingMotion() { // sets homing speed and acceleration for axes 5-6 and sets target homing position - - for(i = 4; i0) - { - do { - recieved = Serial.read(); - inData += String(recieved); - } while(recieved != '\n'); - } - - if(recieved == '\n') - { - if(inData.substring(0, 2) == "HM") - homeArm(); - initFlag = true; - } - } -} - -// updated aug 22, 2022 +// void changeSpeed(char speedVal) { // changes speed of all axes based on user input + +// if(speedVal == faster){ +// if(speedIndex < maxSpeedIndex) { +// speedIndex++; +// for(i=0;i 0) { +// speedIndex--; +// for(i=0;i +#include + +// general parameters +#define SIM 0 //firmware simulation + +#define NUM_AXES 6 +#define ON 0 +#define OFF 1 +#define SW_ON 0 +#define SW_OFF 1 +#define FWD 1 +#define REV 0 + + + +static const char release = 'R'; +static const char move = 'M'; +static const char right = 'R'; +static const char left = 'L'; +static const char up = 'U'; +static const char down = 'D'; +static const char open = 'O'; +static const char close = 'C'; +static const char EEval = 'E'; +static const char homeValEE = 'H'; + +// Motor pins +int stepPins[6] = {6, 8, 2, 10, 12, 25}; +int dirPins[6] = {5, 7, 1, 9, 11, 24}; + +// Encoder pins +int encPinA[6] = {17, 38, 40, 36, 13, 14}; +int encPinB[6] = {16, 37, 39, 35, 41, 15}; + +// limit switch pins +int limPins[6] = {18, 19, 20, 21, 23, 22}; + +// pulses per revolution for motors +long ppr[6] = {400, 400, 400, 400, 400, 400}; + +// Gear Reductions +float red[6] = {50.0, 160.0, 92.3077, 43.936, 57.0, 14.0}; + +// End effector variables +const int closePos = 0; +const int openPos = 500; +const int EEstepPin = 4; +const int EEdirPin = 3; +const int speedEE = 500; +const int accEE = 500; +const int MOTOR_DIR_EE = 1; +const int forcePin = 12; + + +// Encoder Variables +int curEncSteps[NUM_AXES], cmdEncSteps[NUM_AXES]; +const int pprEnc = 512; +const float ENC_MULT[] = {5.12, 5.12, 5.12, 5.12, 5.12, 5.12}; +float ENC_STEPS_PER_DEG[NUM_AXES]; + +// Motor speeds and accelerations +const int maxSpeed[6] = {1000, 1800, 2000, 2000, 3000, 2000}; +const int maxAccel[6] = {2000, 3000, 3000, 3300, 4000, 3000}; +const int homeSpeed[6] = {1500, 2000, 2000, 2500, 1500, 1000}; +const int homeAccel[6] = {1500, 2000, 2000, 2500, 1500, 1000}; + +// Stepper motor objects for AccelStepper library +AccelStepper steppers[6]; +AccelStepper endEff(1, EEstepPin, EEdirPin); + +// Encoder objects for Encoder library +Encoder enc1(encPinA[0], encPinB[0]); +Encoder enc2(encPinA[1], encPinB[1]); +Encoder enc3(encPinA[2], encPinB[2]); +Encoder enc4(encPinA[3], encPinB[3]); +Encoder enc5(encPinA[4], encPinB[4]); +Encoder enc6(encPinA[5], encPinB[5]); + +// General Global Variable declarations +const int axisDir[6] = {1, -1, -1, 1, -1, -1}; +const int IK_DIR[6] = {1, 1, 1, 1, 1, 1}; +int i; +bool initFlag = false; +bool jointFlag = false; +bool IKFlag = false; +bool resetEE = false; + +// Variables for homing / arm calibration +long homePosConst = -99000; +long homePos[] = {axisDir[0]*homePosConst, axisDir[1]*homePosConst, axisDir[2]*homePosConst, axisDir[3]*homePosConst, axisDir[4]*homePosConst, axisDir[5]*homePosConst}; +long homeCompAngles[] = {50, 0, 0, 100, 100, 90}; +long homeCompConst[] = {500, 1000, 1000, 500, 500, 200}; +long homeComp[] = {axisDir[0]*homeCompConst[0], axisDir[1]*homeCompConst[1], axisDir[2]*homeCompConst[2], axisDir[3]*homeCompConst[3], axisDir[4]*homeCompConst[4], axisDir[5]*homeCompConst[5]}; +long homeCompSteps[] = {axisDir[0]*homeCompAngles[0]*red[0]*ppr[0]/360.0, axisDir[1]*homeCompAngles[1]*red[1]*ppr[1]/360.0, axisDir[2]*homeCompAngles[2]*red[2]*ppr[2]/360.0, axisDir[3]*homeCompAngles[3]*red[3]*ppr[3]/360.0, axisDir[4]*homeCompAngles[4]*red[4]*ppr[4]/360.0, axisDir[5]*homeCompAngles[5]*red[5]*ppr[5]/360.0}; + +// Range of motion (degrees) for each axis +int maxAngles[6] = {200, 100, 180, 180, 180, 290}; +long max_steps[] = {axisDir[0]*red[0]*maxAngles[0]/360.0*ppr[0], axisDir[1]*red[1]*maxAngles[1]/360.0*ppr[1], axisDir[2]*red[2]*maxAngles[2]/360.0*ppr[2], axisDir[3]*red[3]*maxAngles[3]/360.0*ppr[3], axisDir[4]*red[4]*maxAngles[4]/360.0*ppr[4], axisDir[5]*red[5]*maxAngles[5]/360.0*ppr[5]}; +long min_steps[NUM_AXES]; +char value; + +// Cartesian mode speed settings +float IKspeeds[] = {0.2, 0.2, 0.2, 0.2, 0.2, 0.2}; +float IKaccs[] = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3}; + + +//****// FUNCTION PROTOTYPES //****// + +// Waits for a message to be published to the serial port. +void recieveCommand(); + +// Parses the message passed to it and determines which subfunction to call for control of arm. +void parseMessage(String inMsg); + + //For closed loop control. Reads and sends current position. Sets new position based on feedback while the arm is in cartesian mode. +void cartesianCommands(String inMsg); + +// Sets the max speed and acceleration for each joint and motor to its Inverse Kinematic speed. +void setCartesianSpeed(); + +// Parses which commands to execute when in joint space mode. +void jointCommands(String inMsg); + +// Sets the max speed and acceleration for each joint and motor to its Joint Mode speed. +void setJointSpeed(); + +// Takes in the axis to move and the direction, and calls moveAxis in FWD or REV. +void moveArm(char axis, char dir); + +// Stops the passed axis and sets its run flag to 0. +void relArm(char axis); + +// Changes the axis target position and sets run flag to 1 or -1 depending on dir. +void moveAxis(int axis, int dir); + +// Opens, closes, releases, or homes the end effector depending on the data in inMsg [2]. +void endEffectorCommands(String inMsg); + +// Selects which drill function to execute based on inMsg[2]. +void drillCommands(String inMsg); + +// Prints the current encoder position for each axis to serial. +void sendArmFeedback(); + +// Reads each encoder's position and stores them in encPos[]. +void readEncPos(int* encPos); + +// Zeroes each encoder at its current position. +void zeroEncoders(); + +// For closed loop movement (Inverse Kin Mode) +int cmdArm(); + +// Main function for homing the entire arm. +void homeArm(); + +// Runs through and checks if each axis has reached its limit switch, then runs it a couple steps away from switch for 0 position. +void homeAxes(); + +// sets homing speed and acceleration for axes 1-6 and sets target homing position. +void initializeHomingMotion(); + +// sets main program speeds for each axis. +void initializeMotion(); + +// Runs all stepper motors to target position (if no target defined, motors will not move). +void runSteppers(); + +// Waits for the user to press the home button before continuing with other functions. Reads serial port and homes the arm if "HM___" is read. +void waitForHome(); + +// Reads end effector gripper force +int readGripperForce(); + +// Move arm to GUI specified pose +void executePose(String inMsg); diff --git a/src/teensy-firmware/src/armFirmware/armFirmware/armFirmware.ino b/src/teensy-firmware/src/armFirmware/armFirmware/armFirmware.ino new file mode 100644 index 000000000..35d1b2e67 --- /dev/null +++ b/src/teensy-firmware/src/armFirmware/armFirmware/armFirmware.ino @@ -0,0 +1,434 @@ +/* +Created By: Tate Kolton, Graeme Dockrill +Created On: December 21, 2021 +Updated On: January 10, 2023 +Description: Main firmware for driving a 6 axis arm via ROS on a teensy 4.1 MCU +*/ + +// header file with all constants defined and libraries included +#include "armFirmware.h" + +// setup function to initialize pins and provide initial homing to the arm. +void setup() { + + Serial.begin(115200); + + for (int i = 0; i < NUM_AXES; i++) { + ENC_STEPS_PER_DEG[i] = ppr[i] * red[i] * (ENC_MULT[i] / 360.0); + + min_steps[i] = -homeCompSteps[i]; + max_steps[i] = max_steps[i] - homeCompSteps[i]; + } + + min_steps[0] = -max_steps[0]; + + // initializes end effector motor + pinMode(EEstepPin, OUTPUT); + pinMode(EEdirPin, OUTPUT); + endEff.setMinPulseWidth(200); + endEff.setMaxSpeed(speedEE); + endEff.setAcceleration(accEE); + endEff.setCurrentPosition(1000); + + // initializes step pins, direction pins, limit switch pins, and stepper motor objects for accelStepper library + for (i = 0; i < NUM_AXES; i++) { + pinMode(dirPins[i], OUTPUT); + pinMode(stepPins[i], OUTPUT); + pinMode(limPins[i], INPUT_PULLUP); + steppers[i] = AccelStepper(1, stepPins[i], dirPins[i]); + steppers[i].setMinPulseWidth(200); + steppers[i].setCurrentPosition(0); + } + + // waits for user to press "home" button before rest of functions are available + // waitForHome(); +} + +// main program loop +void loop() { + + // receives command from serial and executes accoringly + recieveCommand(); + + // run steppers to target position + runSteppers(); +} + +void recieveCommand() { + String inData = ""; + char recieved = '\0'; + + // if a message is present, copy it to inData + if (Serial.available() > 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); + } + + // parse the received data + if (recieved == '\n') { + parseMessage(inData); + } +} + +void parseMessage(String inMsg) { + String function = inMsg.substring(0, 2); + + // choose which function to run + if (function == "MT") { + cartesianCommands(inMsg); + } + + else if (function == "JM") { + jointCommands(inMsg); + } + + else if (function == "EE") { + endEffectorCommands(inMsg); + } + + else if (function == "PM") { + executePose(inMsg); + } + + else if (function == "HM") { + homeArm(); + } + + // Send arm angles and gripper force to hardware driver + sendArmFeedback(); +} + +void executePose(String inMsg) { + int completeCount = 0; + + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt() * IK_DIR[0]; + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt() * IK_DIR[1]; + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt() * IK_DIR[2]; + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt() * IK_DIR[3]; + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt() * IK_DIR[4]; + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt() * IK_DIR[5]; + + + while (completeCount != NUM_AXES) { + readEncPos(curEncSteps); + completeCount = cmdArm(); + runSteppers(); + } +} + +void sendArmFeedback() { + + char garbage; + while (Serial.available() > 0) { + garbage = Serial.read(); + } + + int gripperForce = 0; //readGripperForce(); + readEncPos(curEncSteps); + + String outMsg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2]) + + String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("G") + String(gripperForce) + String("Z"); + Serial.print(outMsg); +} + +void runSteppers() { + + for (i = 0; i < NUM_AXES; i++) { + steppers[i].run(); + } + + endEff.run(); +} + +//****//JOINT MODE FUNCTIONS//****// + +// Parses which commands to execute when in joint space mode +void jointCommands(String inMsg) { + char function = inMsg[2]; + char axis = inMsg[3]; + char dir = inMsg[4]; + + if (!jointFlag) // Check if we are switching from cartesian mode + { + IKFlag = false; + jointFlag = true; + setJointSpeed(); + } + + if (function == move) + moveArm(axis, dir); + + else if (function == release) + relArm(axis); +} + +void moveArm(char axis, char dir) { + int axisNum = String(axis).toInt(); // String to int for math + + if ((dir == left) || (dir == up)) { + moveAxis((axisNum - 1), FWD); + } else if ((dir == right) || (dir == down)) { + moveAxis((axisNum - 1), REV); + } +} + +void relArm(char axis) { + int axisNum = String(axis).toInt(); // String to int for math + steppers[axisNum - 1].stop(); +} + +void moveAxis(int axis, int dir) { + if ((axis == 0) || (axis == 1)) { // switching direction if axis 1 or 2 (arm moves in intuitive dir) + dir = !dir; + } + + if (dir == FWD) { // sets stepper to move to max steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(max_steps[axis]); + + } else if (dir == REV) { // sets stepper to move to min steps and sets run flag so doesn't keep executing + steppers[axis].moveTo(min_steps[axis]); + } +} + +// sets Joint Mode speeds after switching out of cartesian mode +void setJointSpeed() { + for (i = 0; i < NUM_AXES; i++) { + steppers[i].setMaxSpeed(maxSpeed[i]); + steppers[i].setAcceleration(maxAccel[i]); + } +} + +//***//CARTESIAN MODE FUNCTIONS//***// + +void cartesianCommands(String inMsg) { + if (jointFlag) { + IKFlag = true; + jointFlag = false; + setCartesianSpeed(); + } + + // get new position commands + + int msgIdxJ1 = inMsg.indexOf('A'); + int msgIdxJ2 = inMsg.indexOf('B'); + int msgIdxJ3 = inMsg.indexOf('C'); + int msgIdxJ4 = inMsg.indexOf('D'); + int msgIdxJ5 = inMsg.indexOf('E'); + int msgIdxJ6 = inMsg.indexOf('F'); + cmdEncSteps[0] = inMsg.substring(msgIdxJ1 + 1, msgIdxJ2).toInt() * IK_DIR[0]; + cmdEncSteps[1] = inMsg.substring(msgIdxJ2 + 1, msgIdxJ3).toInt() * IK_DIR[1]; + cmdEncSteps[2] = inMsg.substring(msgIdxJ3 + 1, msgIdxJ4).toInt() * IK_DIR[2]; + cmdEncSteps[3] = inMsg.substring(msgIdxJ4 + 1, msgIdxJ5).toInt() * IK_DIR[3]; + cmdEncSteps[4] = inMsg.substring(msgIdxJ5 + 1, msgIdxJ6).toInt() * IK_DIR[4]; + cmdEncSteps[5] = inMsg.substring(msgIdxJ6 + 1).toInt() * IK_DIR[5]; + + readEncPos(curEncSteps); + cmdArm(); +} + +void setCartesianSpeed() { + float JOINT_MAX_SPEED[NUM_AXES]; + float MOTOR_MAX_SPEED[NUM_AXES]; + float JOINT_MAX_ACCEL[NUM_AXES]; + float MOTOR_MAX_ACCEL[NUM_AXES]; + + for (int i = 0; i < NUM_AXES; i++) { + JOINT_MAX_SPEED[i] = IKspeeds[i] * (180.0 / 3.14159); + JOINT_MAX_ACCEL[i] = IKaccs[i] * (180.0 / 3.14159); + MOTOR_MAX_SPEED[i] = JOINT_MAX_SPEED[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + MOTOR_MAX_ACCEL[i] = JOINT_MAX_ACCEL[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i]; + steppers[i].setAcceleration(MOTOR_MAX_ACCEL[i]); + steppers[i].setMaxSpeed(MOTOR_MAX_SPEED[i]); + } +} + +// Set target position for cartesian movements of arm +int cmdArm() { + int count = 0; + + for (int i = 0; i < NUM_AXES; i++) { + int diffEncSteps = cmdEncSteps[i] - curEncSteps[i]; + if (abs(diffEncSteps) > 10) { + int diffMotSteps = diffEncSteps / ENC_MULT[i]; + if (diffMotSteps < ppr[i]) { + diffMotSteps = diffMotSteps / 2; + } + steppers[i].move(diffMotSteps); + } else { + count++; + } + } + return count; +} + +//***// END EFFECTOR RELATED FUNCTIONS //***// + +void endEffectorCommands(String inMsg) { + char data = inMsg[2]; + + //opening code + if (data == open) { //check if open button pressed and if force is less than max + endEff.moveTo(openPos * MOTOR_DIR_EE); //continue to move to open position + } + + //closing code + else if (data == close) { //check if open button pressed and if force is less than max + endEff.moveTo(closePos * MOTOR_DIR_EE); //continue to move to closed position + } + + else if (data == release) { //else check if release button pressed + endEff.stop(); // stop when above condition reached + } + + else if (data == homeValEE) { + if (resetEE) { + endEff.setCurrentPosition(1000); + resetEE = false; + } + + else { + endEff.setCurrentPosition(0); + resetEE = true; + } + } +} + +int readGripperForce() { + int force = analogRead(forcePin); + int forcePct = 100 * (1023 - force) / 1023.0; + return forcePct; +} + +//***//ENCODER RELATED FUNCTIONS//***// + +void readEncPos(int* encPos) { + encPos[0] = enc1.read() * IK_DIR[0] * -1; + encPos[1] = enc2.read() * IK_DIR[1] * -1; + encPos[2] = enc3.read() * IK_DIR[2] * -1; + encPos[3] = enc4.read() * IK_DIR[3] * -1; + encPos[4] = enc5.read() * IK_DIR[4] * -1; + encPos[5] = enc6.read() * IK_DIR[5] * -1; +} + +void zeroEncoders() { + enc1.write(0); + enc2.write(0); + enc3.write(0); + enc4.write(0); + enc5.write(0); + enc6.write(0); + + readEncPos(curEncSteps); +} + +//****// ARM CALIBRATION FUNCTIONS//****// + +void homeArm() { // main function for full arm homing + + initializeHomingMotion(); + homeAxes(); + initializeMotion(); +} + +// Runs through and checks if each axis has reached its limit switch, then runs it to specified home position +void homeAxes() { + bool stopFlags[6] = { false, false, false, false, false, false }; + bool finishFlags[6] = { false, false, false, false, false, false }; + bool completeFlag = false; + int count = 0; + + while (!completeFlag) { + + for (i = 0; i < NUM_AXES; i++) { + + if (!finishFlags[i]) { + + if (!digitalRead(limPins[i]) && !stopFlags[i]) { + steppers[i].run(); + } + + else if (!stopFlags[i]) { + steppers[i].setCurrentPosition(-homeComp[i]); + steppers[i].stop(); + steppers[i].setMaxSpeed(maxSpeed[i] / 2); + steppers[i].setAcceleration(homeAccel[i]); + steppers[i].moveTo(homeCompSteps[i]); + stopFlags[i] = true; + } + + else if (steppers[i].distanceToGo() != 0) { + steppers[i].run(); + } + + else { + finishFlags[i] = true; + count++; + } + } + } + if (count == NUM_AXES) { + completeFlag = true; + } + } +} + +void initializeHomingMotion() { // sets homing speed and acceleration and sets target homing position + + for (i = 0; i < NUM_AXES; i++) { + + steppers[i].setMaxSpeed(homeSpeed[i]); + steppers[i].setAcceleration(homeAccel[i]); + + if (i == 0) // Special case for axis 1 + { + if ((axisDir[0] * steppers[0].currentPosition() < -homeCompSteps[0])) { + steppers[0].move(-homePos[i]); + } else { + steppers[0].move(homePos[i]); + } + } + + else { + steppers[i].move(homePos[i]); + } + } +} + +void initializeMotion() { // sets main program speeds for each axis and zeros position + + setJointSpeed(); + zeroEncoders(); + + for (int i = 0; i < NUM_AXES; i++) { + steppers[i].setCurrentPosition(0); + } +} + +void waitForHome() { + String inData; + char recieved = '\0'; + + while (!initFlag) { + inData = ""; + if (Serial.available() > 0) { + do { + recieved = Serial.read(); + inData += String(recieved); + } while (recieved != '\n'); + } + + if (recieved == '\n') { + if (inData.substring(0, 2) == "HM") + homeArm(); + initFlag = true; + } + } +} \ No newline at end of file diff --git a/src/teensy-firmware/src/armFirmware/readme b/src/teensy-firmware/src/armFirmware/readme new file mode 100644 index 000000000..01b79950e --- /dev/null +++ b/src/teensy-firmware/src/armFirmware/readme @@ -0,0 +1 @@ +Arduino needs .ino file in a folder with the same name.