merging master
This commit is contained in:
Your Name
@ -13,18 +13,23 @@ diff_cont:
|
||||
|
||||
publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
|
||||
base_frame_id: base_link
|
||||
publish_limited_velocity: true
|
||||
|
||||
left_wheel_names: ['left_wheel_joint']
|
||||
right_wheel_names: ['right_wheel_joint']
|
||||
wheel_separation: 0.215
|
||||
wheel_radius: 0.049338032
|
||||
|
||||
# linear:
|
||||
# x:
|
||||
# max_acceleration: 0.15
|
||||
# has_acceleration_limits: true
|
||||
|
||||
use_stamped_vel: false
|
||||
|
||||
# joint_broad:
|
||||
# ros__parameters:
|
||||
joint_limits:
|
||||
ros__parameters:
|
||||
left_wheel_joint:
|
||||
has_position_limits: false # Continuous joint
|
||||
has_velocity_limits: true
|
||||
max_velocity: 10.0
|
||||
right_wheel_joint:
|
||||
has_position_limits: false # Continuous joint
|
||||
has_velocity_limits: true
|
||||
max_velocity: 10.0
|
||||
@ -1 +0,0 @@
|
||||
# This is an empty file, so that git commits the folder correctly
|
||||
@ -34,19 +34,19 @@
|
||||
|
||||
<sensor name="laser" type="ray">
|
||||
<pose> 0 0 0 0 0 0 </pose>
|
||||
<visualize>true</visualize>
|
||||
<visualize>false</visualize>
|
||||
<update_rate>10</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<samples>360*5</samples>
|
||||
<min_angle>-3.14</min_angle>
|
||||
<max_angle>3.14</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.3</min>
|
||||
<max>12</max>
|
||||
<max>100</max>
|
||||
</range>
|
||||
</ray>
|
||||
<plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
|
||||
|
||||
@ -5,6 +5,24 @@
|
||||
<xacro:arg name="use_ros2_control" default="true"/>
|
||||
<xacro:arg name="use_sim" default="false"/>
|
||||
|
||||
<xacro:arg name="tube_thickness" default="0.024"/>
|
||||
<xacro:arg name="tube_radius" default="false"/>
|
||||
|
||||
<xacro:arg name="camera_height" default="0.201"/>
|
||||
<xacro:arg name="camera_offset_x" default="0.016"/>
|
||||
|
||||
<xacro:arg name="lidar_height" default="0.201"/>
|
||||
|
||||
<xacro:arg name="wheel_radius" default="0.05"/>
|
||||
<xacro:arg name="wheel_width" default="0.02"/>
|
||||
|
||||
<xacro:arg name="base_height" default="0.1"/>
|
||||
<xacro:arg name="base_width" default="0.2"/>
|
||||
<xacro:arg name="base_length" default="0.2"/>
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- Include the colours -->
|
||||
<xacro:include filename="colours.xacro" />
|
||||
<!-- Include the inertial macros -->
|
||||
|
||||
@ -25,6 +25,7 @@ def generate_launch_description():
|
||||
package_name='dcaiti_control' #<--- CHANGE ME
|
||||
|
||||
use_ros2_control = LaunchConfiguration('use_ros2_control')
|
||||
world_path='~/.gazebo/models'
|
||||
|
||||
rsp = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
@ -36,7 +37,7 @@ def generate_launch_description():
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
|
||||
launch_arguments={'params_file': gazebo_params_path}.items()
|
||||
launch_arguments={'params_file': gazebo_params_path, 'world': world_path }.items()
|
||||
)
|
||||
|
||||
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
|
||||
|
||||
@ -28,7 +28,6 @@ def generate_launch_description():
|
||||
node_robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[params]
|
||||
)
|
||||
|
||||
|
||||
233
src/dcaiti_navigation/config/ekf.yaml
Normal file
233
src/dcaiti_navigation/config/ekf.yaml
Normal file
@ -0,0 +1,233 @@
|
||||
### ekf config file ###
|
||||
ekf_filter_node:
|
||||
ros__parameters:
|
||||
|
||||
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
|
||||
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
|
||||
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
|
||||
frequency: 30.0
|
||||
|
||||
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
|
||||
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
|
||||
# filter will generate new output. Defaults to 1 / frequency if not specified.
|
||||
sensor_timeout: 0.1
|
||||
|
||||
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
|
||||
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
|
||||
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
|
||||
# by, for example, an IMU. Defaults to false if unspecified.
|
||||
two_d_mode: true
|
||||
|
||||
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
|
||||
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
|
||||
# unspecified.
|
||||
transform_time_offset: 0.0
|
||||
|
||||
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
|
||||
# Defaults to 0.0 if unspecified.
|
||||
transform_timeout: 0.0
|
||||
|
||||
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
|
||||
# unhappy with any settings or data.
|
||||
print_diagnostics: true
|
||||
|
||||
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
|
||||
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
|
||||
# effects on the performance of the node. Defaults to false if unspecified.
|
||||
debug: false
|
||||
|
||||
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
|
||||
debug_out_file: /home/focalfossa/dev_ws/src/basic_mobile_robot/debug_ekf.txt
|
||||
|
||||
# Whether we'll allow old measurements to cause a re-publication of the updated state
|
||||
permit_corrected_publication: false
|
||||
|
||||
# Whether to publish the acceleration state. Defaults to false if unspecified.
|
||||
publish_acceleration: false
|
||||
|
||||
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
|
||||
publish_tf: true
|
||||
|
||||
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
|
||||
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
|
||||
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
|
||||
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
|
||||
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
|
||||
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
|
||||
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
|
||||
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
|
||||
# Here is how to use the following settings:
|
||||
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
|
||||
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
|
||||
# odom_frame.
|
||||
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
|
||||
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
|
||||
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
|
||||
# from landmark observations) then:
|
||||
# 3a. Set your "world_frame" to your map_frame value
|
||||
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
|
||||
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
|
||||
map_frame: map # Defaults to "map" if unspecified
|
||||
odom_frame: odom # Defaults to "odom" if unspecified
|
||||
base_link_frame: base_footprint # Defaults to "base_link" if unspecified
|
||||
world_frame: odom # Defaults to the value of odom_frame if unspecified
|
||||
|
||||
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
|
||||
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
|
||||
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
|
||||
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
|
||||
# default values, and must be specified.
|
||||
odom0: wheel/odometry
|
||||
|
||||
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
|
||||
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
|
||||
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
|
||||
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
|
||||
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
|
||||
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
|
||||
# if unspecified, effectively making this parameter required for each sensor.
|
||||
odom0_config: [true, true, true,
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
false, false, true,
|
||||
false, false, false]
|
||||
|
||||
# [x_pos , y_pos , z_pos,
|
||||
# roll , pitch , yaw,
|
||||
# x_vel , y_vel , z_vel,
|
||||
# roll_vel, pitch_vel, yaw_vel,
|
||||
# x_accel , y_accel , z_accel]
|
||||
|
||||
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
|
||||
# the size of the subscription queue so that more measurements are fused.
|
||||
odom0_queue_size: 2
|
||||
|
||||
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
|
||||
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
|
||||
# algorithm.
|
||||
odom0_nodelay: false
|
||||
|
||||
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
|
||||
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
|
||||
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
|
||||
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
|
||||
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
|
||||
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
|
||||
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
|
||||
# for twist measurements has no effect.
|
||||
odom0_differential: false
|
||||
|
||||
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
|
||||
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
|
||||
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
|
||||
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
|
||||
odom0_relative: false
|
||||
|
||||
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
|
||||
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
|
||||
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
|
||||
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
|
||||
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
|
||||
# the thresholds.
|
||||
odom0_pose_rejection_threshold: 5.0
|
||||
odom0_twist_rejection_threshold: 1.0
|
||||
|
||||
imu0: imu/data
|
||||
imu0_config: [false, false, false,
|
||||
true, true, true,
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
true, true, true]
|
||||
|
||||
# [x_pos , y_pos , z_pos,
|
||||
# roll , pitch , yaw,
|
||||
# x_vel , y_vel , z_vel,
|
||||
# roll_vel, pitch_vel, yaw_vel,
|
||||
# x_accel , y_accel , z_accel]
|
||||
|
||||
imu0_nodelay: false
|
||||
imu0_differential: false
|
||||
imu0_relative: true
|
||||
imu0_queue_size: 5
|
||||
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
|
||||
imu0_twist_rejection_threshold: 0.8 #
|
||||
imu0_linear_acceleration_rejection_threshold: 0.8 #
|
||||
|
||||
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
|
||||
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
|
||||
imu0_remove_gravitational_acceleration: true
|
||||
|
||||
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
|
||||
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
|
||||
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
|
||||
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
|
||||
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
|
||||
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
|
||||
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
|
||||
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
|
||||
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
|
||||
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
|
||||
# inputs, the control term will be ignored.
|
||||
# Whether or not we use the control input during predicition. Defaults to false.
|
||||
use_control: false
|
||||
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
|
||||
# false.
|
||||
stamped_control: false
|
||||
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
|
||||
control_timeout: 0.2
|
||||
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
|
||||
control_config: [true, false, false, false, false, true]
|
||||
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
|
||||
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
|
||||
# Acceleration and deceleration limits are not always the same for robots.
|
||||
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
|
||||
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
|
||||
# gains
|
||||
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
|
||||
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
|
||||
# gains
|
||||
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
|
||||
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
|
||||
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
|
||||
# However, if users find that a given variable is slow to converge, one approach is to increase the
|
||||
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
|
||||
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
|
||||
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
|
||||
# unspecified.
|
||||
process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
|
||||
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
|
||||
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
|
||||
# question. Users should take care not to use large values for variables that will not be measured directly. The values
|
||||
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
|
||||
#if unspecified.
|
||||
initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9]
|
||||
|
||||
11
src/dcaiti_navigation/config/nav2.yml
Normal file
11
src/dcaiti_navigation/config/nav2.yml
Normal file
@ -0,0 +1,11 @@
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
planner_plugins: ['GridBased']
|
||||
GridBased:
|
||||
plugin: 'nav2_navfn_planner/NavfnPlanner'
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
controller_plugins: ["FollowPath"]
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
0
src/dcaiti_navigation/dcaiti_navigation/__init__.py
Normal file
0
src/dcaiti_navigation/dcaiti_navigation/__init__.py
Normal file
144
src/dcaiti_navigation/launch/start_nav2.py
Normal file
144
src/dcaiti_navigation/launch/start_nav2.py
Normal file
@ -0,0 +1,144 @@
|
||||
# Author: Addison Sears-Collins
|
||||
# Date: September 2, 2021
|
||||
# Description: Launch a basic mobile robot using the ROS 2 Navigation Stack
|
||||
# https://automaticaddison.com
|
||||
|
||||
import os
|
||||
from ament_index_python import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Set the path to different files and folders.
|
||||
pkg_share = get_package_share_directory('dcaiti_control')
|
||||
world_file_name = 'basic_mobile_bot_world/smalltown.world'
|
||||
world_path = os.path.join(pkg_share, 'worlds', world_file_name)
|
||||
static_map_path = os.path.join(pkg_share, 'maps', 'smalltown_world.yaml')
|
||||
nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml')
|
||||
behavior_tree_xml_path = os.path.join(get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
|
||||
|
||||
# Launch configuration variables specific to simulation
|
||||
autostart = LaunchConfiguration('autostart')
|
||||
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
|
||||
map_yaml_file = LaunchConfiguration('map')
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
slam = LaunchConfiguration('slam')
|
||||
use_namespace = LaunchConfiguration('use_namespace')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
# Map fully qualified names to relative ones so the node's namespace can be prepended.
|
||||
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
|
||||
# https://github.com/ros/geometry2/issues/32
|
||||
# https://github.com/ros/robot_state_publisher/pull/30
|
||||
# TODO(orduno) Substitute with `PushNodeRemapping`
|
||||
# https://github.com/ros2/launch_ros/issues/56
|
||||
remappings = [('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static')]
|
||||
|
||||
# Declare the launch arguments
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
name='namespace',
|
||||
default_value='',
|
||||
description='Top-level namespace')
|
||||
|
||||
declare_use_namespace_cmd = DeclareLaunchArgument(
|
||||
name='use_namespace',
|
||||
default_value='False',
|
||||
description='Whether to apply a namespace to the navigation stack')
|
||||
|
||||
declare_autostart_cmd = DeclareLaunchArgument(
|
||||
name='autostart',
|
||||
default_value='true',
|
||||
description='Automatically startup the nav2 stack')
|
||||
|
||||
declare_bt_xml_cmd = DeclareLaunchArgument(
|
||||
name='default_bt_xml_filename',
|
||||
default_value=behavior_tree_xml_path,
|
||||
description='Full path to the behavior tree xml file to use')
|
||||
|
||||
declare_map_yaml_cmd = DeclareLaunchArgument(
|
||||
name='map',
|
||||
default_value=static_map_path,
|
||||
description='Full path to map file to load')
|
||||
|
||||
declare_params_file_cmd = DeclareLaunchArgument(
|
||||
name='params_file',
|
||||
default_value=nav2_params_path,
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes')
|
||||
|
||||
declare_simulator_cmd = DeclareLaunchArgument(
|
||||
name='headless',
|
||||
default_value='False',
|
||||
description='Whether to execute gzclient')
|
||||
|
||||
declare_slam_cmd = DeclareLaunchArgument(
|
||||
name='slam',
|
||||
default_value='False',
|
||||
description='Whether to run SLAM')
|
||||
|
||||
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
|
||||
name='use_robot_state_pub',
|
||||
default_value='True',
|
||||
description='Whether to start the robot state publisher')
|
||||
|
||||
declare_use_rviz_cmd = DeclareLaunchArgument(
|
||||
name='use_rviz',
|
||||
default_value='True',
|
||||
description='Whether to start RVIZ')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
name='use_sim_time',
|
||||
default_value='True',
|
||||
description='Use simulation (Gazebo) clock if true')
|
||||
|
||||
declare_use_simulator_cmd = DeclareLaunchArgument(
|
||||
name='use_simulator',
|
||||
default_value='True',
|
||||
description='Whether to start the simulator')
|
||||
|
||||
declare_world_cmd = DeclareLaunchArgument(
|
||||
name='world',
|
||||
default_value=world_path,
|
||||
description='Full path to the world model file to load')
|
||||
|
||||
# Specify the actions
|
||||
|
||||
# Launch the ROS 2 Navigation Stack
|
||||
start_ros2_navigation_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(get_package_share_directory('nav2'),'launch','bringup_launch.py'),
|
||||
launch_arguments = {'namespace': namespace,
|
||||
'use_namespace': use_namespace,
|
||||
'slam': slam,
|
||||
'map': map_yaml_file,
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': params_file,
|
||||
'default_bt_xml_filename': default_bt_xml_filename,
|
||||
'autostart': autostart}.items())
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_use_namespace_cmd)
|
||||
ld.add_action(declare_autostart_cmd)
|
||||
ld.add_action(declare_bt_xml_cmd)
|
||||
ld.add_action(declare_map_yaml_cmd)
|
||||
ld.add_action(declare_params_file_cmd)
|
||||
ld.add_action(declare_simulator_cmd)
|
||||
ld.add_action(declare_slam_cmd)
|
||||
ld.add_action(declare_use_robot_state_pub_cmd)
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_use_simulator_cmd)
|
||||
ld.add_action(declare_world_cmd)
|
||||
|
||||
# Add any actions
|
||||
ld.add_action(start_ros2_navigation_cmd)
|
||||
|
||||
return ld
|
||||
BIN
src/dcaiti_navigation/maps/my_map.pgm
Normal file
BIN
src/dcaiti_navigation/maps/my_map.pgm
Normal file
Binary file not shown.
7
src/dcaiti_navigation/maps/my_map.yaml
Normal file
7
src/dcaiti_navigation/maps/my_map.yaml
Normal file
@ -0,0 +1,7 @@
|
||||
image: my_map.pgm
|
||||
mode: trinary
|
||||
resolution: 0.05
|
||||
origin: [-18.7, -17.5, 0]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.25
|
||||
5
src/dcaiti_navigation/maps/smalltown_world.pgm
Normal file
5
src/dcaiti_navigation/maps/smalltown_world.pgm
Normal file
File diff suppressed because one or more lines are too long
6
src/dcaiti_navigation/maps/smalltown_world.yaml
Normal file
6
src/dcaiti_navigation/maps/smalltown_world.yaml
Normal file
@ -0,0 +1,6 @@
|
||||
image: smalltown_world.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-51.224998, -51.224998, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
18
src/dcaiti_navigation/package.xml
Normal file
18
src/dcaiti_navigation/package.xml
Normal file
@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>dcaiti_navigation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="you@example.com">korjakow</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
403
src/dcaiti_navigation/params/nav2_params.yaml
Normal file
403
src/dcaiti_navigation/params/nav2_params.yaml
Normal file
@ -0,0 +1,403 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
alpha1: 0.002
|
||||
alpha2: 0.002
|
||||
alpha3: 0.002
|
||||
alpha4: 0.002
|
||||
alpha5: 0.002
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
initial_pose: [0.0, 0.0, 0.0, 0.0]
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "differential"
|
||||
save_pose_rate: 0.5
|
||||
set_initial_pose: False
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
|
||||
amcl_map_client:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
amcl_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /diff_cont/odom #/wheel/odometry
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
enable_groot_monitoring: True
|
||||
groot_zmq_publisher_port: 1666
|
||||
groot_zmq_server_port: 1667
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
|
||||
bt_navigator_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 2.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
controller_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 10
|
||||
height: 10
|
||||
resolution: 0.05
|
||||
robot_radius: 0.75
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 10.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 9.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
local_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.75
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 10.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 9.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.95
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_costmap_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
yaml_filename: "turtlebot3_world.yaml"
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 2.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
planner_server_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
recoveries_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_recoveries/Spin"
|
||||
backup:
|
||||
plugin: "nav2_recoveries/BackUp"
|
||||
wait:
|
||||
plugin: "nav2_recoveries/Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_timeout: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 2
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
|
||||
# Plugin params
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
|
||||
# ROS Parameters
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: mapping #localization
|
||||
|
||||
# if you'd like to immediately start continuing a map at a given pose
|
||||
# or at the dock, but they are mutually exclusive, if pose is given
|
||||
# will use pose
|
||||
#map_file_name: test_steve
|
||||
#map_start_pose: [0.0, 0.0, 0.0]
|
||||
#map_start_at_dock: true
|
||||
|
||||
debug_logging: false
|
||||
throttle_scans: 1
|
||||
transform_publish_period: 0.02 #if 0 never publishes odometry
|
||||
map_update_interval: 5.0
|
||||
resolution: 0.05
|
||||
max_laser_range: 20.0 #for rastering images
|
||||
minimum_time_interval: 0.5
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.
|
||||
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
|
||||
enable_interactive_mode: true
|
||||
|
||||
# General Parameters
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
minimum_travel_distance: 0.5
|
||||
minimum_travel_heading: 0.5
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
loop_search_maximum_distance: 3.0
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
|
||||
# Correlation Parameters - Correlation Parameters
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# Correlation Parameters - Loop Closure Parameters
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# Scan Matcher Parameters
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
|
||||
fine_search_angle_offset: 0.00349
|
||||
coarse_search_angle_offset: 0.349
|
||||
coarse_angle_resolution: 0.0349
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
use_response_expansion: true
|
||||
0
src/dcaiti_navigation/resource/dcaiti_navigation
Normal file
0
src/dcaiti_navigation/resource/dcaiti_navigation
Normal file
4
src/dcaiti_navigation/setup.cfg
Normal file
4
src/dcaiti_navigation/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/dcaiti_navigation
|
||||
[install]
|
||||
install_scripts=$base/lib/dcaiti_navigation
|
||||
28
src/dcaiti_navigation/setup.py
Normal file
28
src/dcaiti_navigation/setup.py
Normal file
@ -0,0 +1,28 @@
|
||||
from glob import glob
|
||||
import os
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'dcaiti_navigation'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name), glob('launch/*.py')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='korjakow',
|
||||
maintainer_email='you@example.com',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
],
|
||||
},
|
||||
)
|
||||
25
src/dcaiti_navigation/test/test_copyright.py
Normal file
25
src/dcaiti_navigation/test/test_copyright.py
Normal file
@ -0,0 +1,25 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
25
src/dcaiti_navigation/test/test_flake8.py
Normal file
25
src/dcaiti_navigation/test/test_flake8.py
Normal file
@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
23
src/dcaiti_navigation/test/test_pep257.py
Normal file
23
src/dcaiti_navigation/test/test_pep257.py
Normal file
@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
4084
src/dcaiti_navigation/worlds/basic_mobile_bot_world/smalltown.world
Normal file
4084
src/dcaiti_navigation/worlds/basic_mobile_bot_world/smalltown.world
Normal file
File diff suppressed because it is too large
Load Diff
1
src/dcaitirobot/.gitignore
vendored
1
src/dcaitirobot/.gitignore
vendored
@ -3,7 +3,6 @@ logs/
|
||||
install/
|
||||
build/
|
||||
bin/
|
||||
lib/
|
||||
log/
|
||||
msg_gen/
|
||||
srv_gen/
|
||||
|
||||
6
src/dcaitirobot/config/slam.yml
Normal file
6
src/dcaitirobot/config/slam.yml
Normal file
@ -0,0 +1,6 @@
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
base_frame: base_link
|
||||
map_update_interval: 1.0
|
||||
scan_topic: /scan_no_nan
|
||||
odom_topic: /odom
|
||||
42
src/dcaitirobot/dcaitirobot/handle_nan_scan.py
Normal file
42
src/dcaitirobot/dcaitirobot/handle_nan_scan.py
Normal file
@ -0,0 +1,42 @@
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import LaserScan
|
||||
import geometry_msgs.msg as gm
|
||||
|
||||
|
||||
class TF2PublisherNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('nanhandler')
|
||||
|
||||
self.create_subscription(LaserScan, "/scan", self.callback, 1)
|
||||
self.pub = self.create_publisher(LaserScan, "/scan_no_nan", 1)
|
||||
def callback(self, msg):
|
||||
print(msg.ranges[0])
|
||||
|
||||
msg.ranges[0] = float("inf")
|
||||
msg.intensities[0] = 0.0
|
||||
|
||||
msg.ranges.append(float("inf"))
|
||||
msg.intensities.append(0.0)
|
||||
# ranges = np.array(msg.ranges)
|
||||
# mask_inf_dist = np.isinf(ranges)
|
||||
# print(np.array(msg.intensities)[mask_inf_dist])
|
||||
|
||||
self.pub.publish(msg)
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
tf2_publisher = TF2PublisherNode()
|
||||
rclpy.spin(tf2_publisher)
|
||||
|
||||
tf2_publisher.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
65
src/dcaitirobot/dcaitirobot/scale_odom.py
Normal file
65
src/dcaitirobot/dcaitirobot/scale_odom.py
Normal file
@ -0,0 +1,65 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
SCALE_FACTOR = 1.0
|
||||
|
||||
|
||||
|
||||
class TwistPublisher(Node):
|
||||
def __init__(
|
||||
self,
|
||||
scale_factor=20.0
|
||||
):
|
||||
super().__init__("twist_publisher")
|
||||
|
||||
self.scale_factor = scale_factor
|
||||
|
||||
self.publisher_ = self.create_publisher(
|
||||
Odometry, "/odom_scaled", 1
|
||||
)
|
||||
|
||||
self.create_subscription(Odometry, "/odom", self.odom_callback, 1)
|
||||
|
||||
def odom_callback(self, odom_message:Odometry):
|
||||
odom_message.twist.twist.linear.x *= self.scale_factor
|
||||
odom_message.twist.twist.linear.y *= self.scale_factor
|
||||
odom_message.twist.twist.linear.z *= self.scale_factor
|
||||
odom_message.twist.twist.angular.x *= self.scale_factor
|
||||
odom_message.twist.twist.angular.y *= self.scale_factor
|
||||
odom_message.twist.twist.angular.z *= self.scale_factor
|
||||
odom_message.pose.pose.position.x *= self.scale_factor
|
||||
odom_message.pose.pose.position.y *= self.scale_factor
|
||||
odom_message.pose.pose.position.z *= self.scale_factor
|
||||
odom_message.pose.pose.orientation.x *= self.scale_factor
|
||||
odom_message.pose.pose.orientation.y *= self.scale_factor
|
||||
odom_message.pose.pose.orientation.z *= self.scale_factor
|
||||
|
||||
self.publisher_.publish(odom_message)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
minimal_publisher = TwistPublisher(
|
||||
SCALE_FACTOR
|
||||
)
|
||||
|
||||
rclpy.spin(minimal_publisher)
|
||||
|
||||
minimal_publisher.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -12,16 +12,14 @@ class TF2PublisherNode(Node):
|
||||
|
||||
self.timer = self.create_timer(0.1, self.publish_identity_transform)
|
||||
|
||||
self.x = 0.0
|
||||
|
||||
def publish_identity_transform(self):
|
||||
def send_parent_child_identity_tf(self, parent, child):
|
||||
transform_stamped = gm.TransformStamped()
|
||||
transform_stamped.header.stamp = self.get_clock().now().to_msg()
|
||||
transform_stamped.header.frame_id = 'odom'
|
||||
transform_stamped.child_frame_id = 'base_link'
|
||||
transform_stamped.header.frame_id = parent
|
||||
transform_stamped.child_frame_id = child
|
||||
|
||||
# Identity transform (no translation, no rotation)
|
||||
transform_stamped.transform.translation.x = self.x
|
||||
transform_stamped.transform.translation.x = 0.0
|
||||
transform_stamped.transform.translation.y = 0.0
|
||||
transform_stamped.transform.translation.z = 0.0
|
||||
transform_stamped.transform.rotation.x = 0.0
|
||||
@ -29,29 +27,17 @@ class TF2PublisherNode(Node):
|
||||
transform_stamped.transform.rotation.z = 0.0
|
||||
transform_stamped.transform.rotation.w = 1.0
|
||||
|
||||
|
||||
self.x += 0.01
|
||||
|
||||
self.tf_broadcaster.sendTransform(transform_stamped)
|
||||
|
||||
transform_stamped = gm.TransformStamped()
|
||||
transform_stamped.header.stamp = self.get_clock().now().to_msg()
|
||||
transform_stamped.header.frame_id = 'base_link'
|
||||
transform_stamped.child_frame_id = 'velodyne'
|
||||
|
||||
# Identity transform (no translation, no rotation)
|
||||
transform_stamped.transform.translation.x = 0.0
|
||||
transform_stamped.transform.translation.y = self.x
|
||||
transform_stamped.transform.translation.z = 0.0
|
||||
transform_stamped.transform.rotation.x = 0.0
|
||||
transform_stamped.transform.rotation.y = 0.0
|
||||
transform_stamped.transform.rotation.z = 0.0
|
||||
transform_stamped.transform.rotation.w = 1.0
|
||||
|
||||
self.tf_broadcaster.sendTransform(transform_stamped)
|
||||
|
||||
|
||||
|
||||
def publish_identity_transform(self):
|
||||
# self.send_parent_child_identity_tf("map", "odom")
|
||||
# self.send_parent_child_identity_tf("odom", "base_link")
|
||||
# self.send_parent_child_identity_tf("base_link", "base_scan")
|
||||
# self.send_parent_child_identity_tf("base_scan", "velodyne")
|
||||
self.send_parent_child_identity_tf("base_link", "velodyne")
|
||||
self.send_parent_child_identity_tf("base_link", "base_footprint")
|
||||
|
||||
|
||||
self.get_logger().info('Published identity transform')
|
||||
|
||||
|
||||
@ -7,8 +7,8 @@ from sensor_msgs.msg import Joy
|
||||
|
||||
import numpy as np
|
||||
|
||||
SCALE_LINEAR_VELOCITY = 0.1
|
||||
SCALE_ANGULAR_VELOCITY = 0.1
|
||||
SCALE_LINEAR_VELOCITY = 0.2
|
||||
SCALE_ANGULAR_VELOCITY = 0.7
|
||||
PUBLISH_FREQUENCY_HZ = 30
|
||||
|
||||
SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
|
||||
|
||||
@ -37,7 +37,7 @@ public:
|
||||
* @param value_left encoder value left
|
||||
* @param value_right encoder value right
|
||||
*/
|
||||
static void sendBothEncoderValues(float value_left, float value_right);
|
||||
static void sendBothEncoderValues(float speed_right, float speed_left, float pos_right, float pos_left);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -13,7 +13,6 @@
|
||||
*/
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "R2WD.h"
|
||||
|
||||
class Utils {
|
||||
private:
|
||||
|
||||
385
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp
Normal file
385
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp
Normal file
@ -0,0 +1,385 @@
|
||||
#include<MotorWheel.h>
|
||||
|
||||
Motor::Motor(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr)
|
||||
:PID(&speedRPMInput,&speedRPMOutput,&speedRPMDesired,KC,TAUI,TAUD),
|
||||
pinPWM(_pinPWM),pinDir(_pinDir),isr(_isr) {
|
||||
debug();
|
||||
|
||||
isr->pinIRQ=_pinIRQ;
|
||||
isr->pinIRQB=_pinIRQB;
|
||||
|
||||
/*for maple*/
|
||||
#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
|
||||
pinMode(pinPWM,PWM);
|
||||
pinMode(pinDir,OUTPUT);
|
||||
pinMode(isr->pinIRQ,INPUT_FLOATING);
|
||||
|
||||
/*for arduino*/
|
||||
#else
|
||||
pinMode(pinPWM,OUTPUT);
|
||||
pinMode(pinDir,OUTPUT);
|
||||
pinMode(isr->pinIRQ,INPUT);
|
||||
|
||||
#endif
|
||||
if(isr->pinIRQB!=PIN_UNDEFINED) {
|
||||
pinMode(isr->pinIRQB,INPUT);
|
||||
}
|
||||
|
||||
PIDDisable();
|
||||
}
|
||||
|
||||
void Motor::setupInterrupt() {
|
||||
/*for maple*/
|
||||
#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
|
||||
attachInterrupt(isr->pinIRQ,isr->ISRfunc,TRIGGER); // RISING --> CHANGE 201207
|
||||
|
||||
/*for arduino*/
|
||||
#else
|
||||
if(isr->pinIRQ==2 || isr->pinIRQ==3) attachInterrupt(isr->pinIRQ-2,isr->ISRfunc,TRIGGER);
|
||||
else {
|
||||
PCattachInterrupt(isr->pinIRQ,isr->ISRfunc,TRIGGER); // RISING --> CHANGE 201207
|
||||
}
|
||||
//#ifndef _NAMIKI_MOTOR
|
||||
// // Namiki motor can use 2 times only, because of the distance of the two optical sensors
|
||||
// // 4 times of pulses, 201208
|
||||
// if(isr->pinIRQB==2 || isr->pinIRQB==3) attachInterrupt(isr->pinIRQB-2,isr->ISRfunc,CHANGE);
|
||||
// else {
|
||||
// PCattachInterrupt(isr->pinIRQB,isr->ISRfunc,CHANGE); // RISING --> CHANGE 201207
|
||||
// }
|
||||
//#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
unsigned char Motor::getPinPWM() const {
|
||||
debug();
|
||||
return pinPWM;
|
||||
}
|
||||
unsigned char Motor::getPinDir() const {
|
||||
debug();
|
||||
return pinDir;
|
||||
}
|
||||
unsigned char Motor::getPinIRQ() const {
|
||||
debug();
|
||||
return isr->pinIRQ;
|
||||
}
|
||||
unsigned char Motor::getPinIRQB() const {
|
||||
debug();
|
||||
return isr->pinIRQB;
|
||||
}
|
||||
|
||||
unsigned int Motor::runPWM(unsigned int PWM,bool dir,bool saveDir) {
|
||||
debug();
|
||||
speedPWM=PWM;
|
||||
if(saveDir) desiredDirection=dir;
|
||||
analogWrite(pinPWM,PWM);
|
||||
digitalWrite(pinDir,dir);
|
||||
return PWM;
|
||||
}
|
||||
unsigned int Motor::advancePWM(unsigned int PWM) {
|
||||
debug();
|
||||
return runPWM(PWM,DIR_ADVANCE);
|
||||
}
|
||||
unsigned int Motor::backoffPWM(unsigned int PWM) {
|
||||
debug();
|
||||
return runPWM(PWM,DIR_BACKOFF);
|
||||
}
|
||||
unsigned int Motor::getPWM() const {
|
||||
debug();
|
||||
return speedPWM;
|
||||
}
|
||||
bool Motor::setDesiredDir(bool dir) {
|
||||
debug();
|
||||
//runPWM(getPWM(),dir); // error
|
||||
desiredDirection=dir;
|
||||
return getDesiredDir();
|
||||
}
|
||||
bool Motor::getDesiredDir() const {
|
||||
debug();
|
||||
return desiredDirection;
|
||||
}
|
||||
bool Motor::reverseDesiredDir() {
|
||||
debug();
|
||||
runPWM(getPWM(),!getDesiredDir());
|
||||
return getDesiredDir();
|
||||
}
|
||||
|
||||
bool Motor::getCurrDir() const {
|
||||
return isr->currDirection;
|
||||
}
|
||||
bool Motor::setCurrDir() {
|
||||
if(getPinIRQB()!=PIN_UNDEFINED)
|
||||
return isr->currDirection=digitalRead(isr->pinIRQB);
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
int Motor::getAccRPMM() const {
|
||||
debug();
|
||||
return SPEEDPPS2SPEEDRPM(isr->accPPSS);
|
||||
}
|
||||
*/
|
||||
/*
|
||||
unsigned int Motor::getSpeedRPM() const {
|
||||
debug();
|
||||
return SPEEDPPS2SPEEDRPM(isr->speedPPS);
|
||||
}
|
||||
*/
|
||||
unsigned int Motor::setSpeedRPM(int speedRPM,bool dir) {
|
||||
debug();
|
||||
PIDSetSpeedRPMDesired(speedRPM);
|
||||
setDesiredDir(dir);
|
||||
return abs(getSpeedRPM());
|
||||
}
|
||||
// direction sensitive 201208
|
||||
int Motor::getSpeedRPM() const {
|
||||
debug();
|
||||
if(getCurrDir()==DIR_ADVANCE)
|
||||
return SPEEDPPS2SPEEDRPM(isr->speedPPS);
|
||||
return -SPEEDPPS2SPEEDRPM(isr->speedPPS);
|
||||
}
|
||||
int Motor::setSpeedRPM(int speedRPM) {
|
||||
debug();
|
||||
if(speedRPM>=0) return setSpeedRPM(speedRPM,DIR_ADVANCE);
|
||||
else return setSpeedRPM(abs(speedRPM),DIR_BACKOFF);
|
||||
}
|
||||
|
||||
|
||||
bool Motor::PIDGetStatus() const {
|
||||
debug();
|
||||
return pidCtrl;
|
||||
}
|
||||
bool Motor::PIDEnable(float kc,float taui,float taud,unsigned int sampleTime) {
|
||||
debug();
|
||||
setupInterrupt();
|
||||
PIDSetup(kc,taui,taud,sampleTime);
|
||||
return pidCtrl=true;
|
||||
}
|
||||
bool Motor::PIDDisable() {
|
||||
debug();
|
||||
|
||||
return pidCtrl=false;
|
||||
}
|
||||
bool Motor::PIDReset() {
|
||||
debug();
|
||||
if(PIDGetStatus()==false) return false;
|
||||
PID::Reset();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Motor::PIDSetup(float kc,float taui,float taud,unsigned int sampleTime) {
|
||||
debug();
|
||||
PID::SetTunings(kc,taui,taud);
|
||||
PID::SetInputLimits(0,MAX_SPEEDRPM);
|
||||
PID::SetOutputLimits(0,MAX_SPEEDRPM);
|
||||
PID::SetSampleTime(sampleTime);
|
||||
PID::SetMode(AUTO);
|
||||
return true;
|
||||
}
|
||||
unsigned int Motor::PIDSetSpeedRPMDesired(unsigned int speedRPM) {
|
||||
debug();
|
||||
if(speedRPM>MAX_SPEEDRPM) speedRPMDesired=MAX_SPEEDRPM;
|
||||
else speedRPMDesired=speedRPM;
|
||||
return PIDGetSpeedRPMDesired();
|
||||
}
|
||||
unsigned int Motor::PIDGetSpeedRPMDesired() const {
|
||||
debug();
|
||||
return speedRPMDesired;
|
||||
}
|
||||
|
||||
bool Motor::PIDRegulate(bool doRegulate) {
|
||||
debug();
|
||||
if(PIDGetStatus()==false) return false;
|
||||
if(getPinIRQB()!=PIN_UNDEFINED && getDesiredDir()!=getCurrDir()) {
|
||||
speedRPMInput=-SPEEDPPS2SPEEDRPM(isr->speedPPS);
|
||||
} else {
|
||||
speedRPMInput=SPEEDPPS2SPEEDRPM(isr->speedPPS);
|
||||
}
|
||||
|
||||
PID::Compute();
|
||||
if(doRegulate && PID::JustCalculated()) {
|
||||
speed2DutyCycle+=speedRPMOutput;
|
||||
|
||||
if(speed2DutyCycle>=MAX_SPEEDRPM) speed2DutyCycle=MAX_SPEEDRPM;
|
||||
else if(speed2DutyCycle<=-MAX_SPEEDRPM) speed2DutyCycle=-MAX_SPEEDRPM;
|
||||
if(speed2DutyCycle>=0) {
|
||||
runPWM(map(speed2DutyCycle,0,MAX_SPEEDRPM,0,MAX_PWM),getDesiredDir(),false);
|
||||
} else {
|
||||
runPWM(map(abs(speed2DutyCycle),0,MAX_SPEEDRPM,0,MAX_PWM),!getDesiredDir(),false);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int Motor::getSpeedPPS() const {
|
||||
return isr->speedPPS;
|
||||
}
|
||||
long Motor::getCurrPulse() const {
|
||||
return isr->pulses;
|
||||
}
|
||||
long Motor::setCurrPulse(long _pulse) {
|
||||
isr->pulses=_pulse;
|
||||
return getCurrPulse();
|
||||
}
|
||||
long Motor::resetCurrPulse() {
|
||||
return setCurrPulse(0);
|
||||
}
|
||||
|
||||
void Motor::delayMS(unsigned int ms,bool debug) {
|
||||
for(unsigned long endTime=millis()+ms;millis()<endTime;) {
|
||||
PIDRegulate();
|
||||
if(debug && (millis()%500==0)) debugger();
|
||||
if(endTime-millis()>=SAMPLETIME) delay(SAMPLETIME);
|
||||
else delay(endTime-millis());
|
||||
}
|
||||
}
|
||||
|
||||
// to reduce the binary for Atmega168 (16KB), Serial output is disabled
|
||||
void Motor::debugger() const {
|
||||
debug()
|
||||
|
||||
#ifdef DEBUG
|
||||
if(!Serial.available()) Serial.begin(Baudrate);
|
||||
/*
|
||||
Serial.print("pinPWM -> ");
|
||||
Serial.println(pinPWM,DEC);
|
||||
Serial.print("pinDir -> ");
|
||||
Serial.println(pinDir,DEC);
|
||||
Serial.print("pinIRQ -> ");
|
||||
Serial.println(pinIRQ,DEC);
|
||||
Serial.print("pinIRQB-> ");
|
||||
Serial.println(pinIRQB,DEC);
|
||||
*/
|
||||
|
||||
Serial.print("DesiredDir -> ");
|
||||
Serial.println(desiredDirection);
|
||||
Serial.print("currDir ->");
|
||||
Serial.println(isr->currDirection);
|
||||
|
||||
Serial.print("PWM -> ");
|
||||
Serial.println(speedPWM,DEC);
|
||||
Serial.print("Input -> ");
|
||||
Serial.println(speedRPMInput,DEC);
|
||||
Serial.print("Output -> ");
|
||||
Serial.println(speedRPMOutput,DEC);
|
||||
Serial.print("Desired-> ");
|
||||
Serial.println(speedRPMDesired,DEC);
|
||||
|
||||
/*
|
||||
Serial.print("speed2DutyCycle-> ");
|
||||
Serial.println(speed2DutyCycle);
|
||||
Serial.print("speedPPS> ");
|
||||
Serial.println(isr->speedPPS,DEC);
|
||||
Serial.print("pulses -> ");
|
||||
Serial.println(isr->pulses,DEC);
|
||||
*/
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
GearedMotor::GearedMotor(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr,unsigned int ratio):
|
||||
Motor(_pinPWM,_pinDir,_pinIRQ,_pinIRQB,_isr),_ratio(ratio) {
|
||||
debug();
|
||||
}
|
||||
unsigned int GearedMotor::getRatio() const {
|
||||
return _ratio;
|
||||
}
|
||||
unsigned int GearedMotor::setRatio(unsigned int ratio) {
|
||||
_ratio=ratio;
|
||||
return getRatio();
|
||||
}
|
||||
/*
|
||||
float GearedMotor::getGearedAccRPMM() const {
|
||||
debug();
|
||||
return (float)Motor::getAccRPMM()/getRatio();
|
||||
}
|
||||
*/
|
||||
float GearedMotor::getGearedSpeedRPM() const {
|
||||
debug();
|
||||
//return (float)Motor::getSpeedRPM()/REDUCTION_RATIO;
|
||||
//return (float)Motor::getSpeedRPM()/_ratio;
|
||||
return (float)Motor::getSpeedRPM()/getRatio();
|
||||
}
|
||||
float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM,bool dir) {
|
||||
debug();
|
||||
//Motor::setSpeedRPM(abs(gearedSpeedRPM*REDUCTION_RATIO),dir);
|
||||
Motor::setSpeedRPM(abs(round(gearedSpeedRPM*_ratio)),dir);
|
||||
return getGearedSpeedRPM();
|
||||
}
|
||||
// direction sensitive, 201208
|
||||
float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM) {
|
||||
debug();
|
||||
//Motor::setSpeedRPM(gearedSpeedRPM*_ratio);
|
||||
Motor::setSpeedRPM(round(gearedSpeedRPM*_ratio));
|
||||
return getGearedSpeedRPM();
|
||||
}
|
||||
|
||||
float GearedMotor::getPosition() {
|
||||
debug();
|
||||
int ticks_per_rev = getRatio() * CPR;
|
||||
return (float)(Motor::getCurrPulse() % ticks_per_rev)/ticks_per_rev;
|
||||
}
|
||||
|
||||
MotorWheel::MotorWheel(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr,
|
||||
unsigned int ratio,unsigned int cirMM):
|
||||
GearedMotor(_pinPWM,_pinDir,_pinIRQ,_pinIRQB,_isr,ratio),_cirMM(cirMM) {
|
||||
debug();
|
||||
|
||||
}
|
||||
unsigned int MotorWheel::getCirMM() const {
|
||||
return _cirMM;
|
||||
}
|
||||
unsigned int MotorWheel::setCirMM(unsigned int cirMM) {
|
||||
if(cirMM>0) _cirMM=cirMM;
|
||||
return getCirMM();
|
||||
}
|
||||
// direction sensitive, 201208
|
||||
int MotorWheel::getSpeedCMPM() const {
|
||||
debug();
|
||||
//return int(GearedMotor::getGearedSpeedRPM()*CIR);
|
||||
return int(GearedMotor::getGearedSpeedRPM()*getCirMM()/10);
|
||||
}
|
||||
int MotorWheel::setSpeedCMPM(unsigned int cm,bool dir) {
|
||||
debug();
|
||||
//GearedMotor::setGearedSpeedRPM(cm/CIR,dir);
|
||||
GearedMotor::setGearedSpeedRPM(cm*10.0/getCirMM(),dir);
|
||||
return getSpeedCMPM();
|
||||
}
|
||||
// direction sensitive, 201208
|
||||
int MotorWheel::setSpeedCMPM(int cm) {
|
||||
debug();
|
||||
//GearedMotor::setGearedSpeedRPM(cm/CIR,dir);
|
||||
GearedMotor::setGearedSpeedRPM(cm*10.0/getCirMM());
|
||||
return getSpeedCMPM();
|
||||
}
|
||||
|
||||
void MotorWheel::setGearedRPM(float rpm, bool dir) {
|
||||
debug();
|
||||
GearedMotor::setGearedSpeedRPM(rpm, dir);
|
||||
return ;
|
||||
}
|
||||
// direction sensitive, 201208
|
||||
int MotorWheel::getSpeedMMPS() const {
|
||||
debug();
|
||||
return int(getSpeedCMPM()/6);//(mm/sec)/(cm/min) = 6
|
||||
}
|
||||
|
||||
int MotorWheel::setSpeedMMPS(unsigned int mm,bool dir) {
|
||||
debug();
|
||||
setSpeedCMPM(mm*6,dir);
|
||||
return getSpeedMMPS();
|
||||
}
|
||||
// direction sensitive, 201208
|
||||
int MotorWheel::setSpeedMMPS(int mm) {
|
||||
debug();
|
||||
setSpeedCMPM(mm*6);
|
||||
return getSpeedMMPS();
|
||||
}
|
||||
349
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h
Normal file
349
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h
Normal file
@ -0,0 +1,349 @@
|
||||
|
||||
/*
|
||||
V1.1 201104
|
||||
1. motorwheel library version 1.1,compatible with maple.
|
||||
|
||||
V1.1.1 201110
|
||||
1. Add Acceleration
|
||||
|
||||
V1.2 201207
|
||||
1. Double CPR from 12 to 24, Interrupt Type from RISING to CHANGE.
|
||||
2. Reduce default sample time from 10ms to 5ms.
|
||||
3. Compatible with Namiki 22CL-3501PG80:1, by "#define _NAMIKI_MOTOR" before "#include ...".
|
||||
|
||||
V1.3 201209
|
||||
1. R2WD::delayMS(), Omni3WD::delayMS(), Omni4WD::delayMS() are re-implemented, more exactly.
|
||||
2. getSpeedRPM(), setSpeedRPM(), getSpeedMMPS(), setSpeedMMPS() are plug-minus/direction sensitive now.
|
||||
3. Acceleration is disabled.
|
||||
|
||||
V1.4 201209 Not Released
|
||||
1. Increase CPR from 24 to 48 for Faulhaber 2342.
|
||||
|
||||
V1.5 201209 Omni4WD is re-implemented, and now return value of Omni4WD::getSpeedMMPS() is correct.
|
||||
|
||||
*/
|
||||
|
||||
/*for maple*/
|
||||
#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
|
||||
#include "wirish.h"
|
||||
#include "./../PID_Beta6/PID_Beta6.h"
|
||||
#include "ext_interrupts.h"
|
||||
|
||||
/*for arduino*/
|
||||
#else
|
||||
//#include<Arduino.h>
|
||||
//#include<WProgram.h>
|
||||
#include<PID_Beta6.h>
|
||||
#include<PinChangeInt.h>
|
||||
|
||||
#endif
|
||||
|
||||
/*
|
||||
|
||||
class PID
|
||||
class Motor
|
||||
class GearedMotor
|
||||
class MotorWheel
|
||||
|
||||
*/
|
||||
|
||||
#ifndef MotorWheel_H
|
||||
#define MotorWheel_H
|
||||
|
||||
#define DIR_ADVANCE HIGH
|
||||
#define DIR_BACKOFF LOW
|
||||
|
||||
#define PIN_UNDEFINED 255
|
||||
#define REF_VOLT 12
|
||||
/*for maple*/
|
||||
#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini)
|
||||
#define MAX_PWM 3599
|
||||
|
||||
/*for arduino*/
|
||||
#else
|
||||
#define MAX_PWM 255
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef _NAMIKI_MOTOR
|
||||
#define TRIGGER CHANGE
|
||||
#define CPR 4 // Namiki motor
|
||||
#define DIR_INVERSE
|
||||
#define REDUCTION_RATIO 80
|
||||
#else
|
||||
#define TRIGGER CHANGE
|
||||
#define CPR 24 // Faulhaber motor
|
||||
#define DIR_INVERSE !
|
||||
#define REDUCTION_RATIO 64
|
||||
#endif
|
||||
|
||||
#define MAX_SPEEDRPM 8000
|
||||
//#define DESIRED_SPEEDRPM 3000
|
||||
//#define SPEED_CONSTANT_RPM 713
|
||||
//#define PWM2SPEED_RESOLUTION 143 //713 * 0.2
|
||||
|
||||
#define SEC_PER_MIN 60
|
||||
#define MICROS_PER_SEC 1000000
|
||||
//#define SPEEDPPS2SPEEDRPM(freq) ((freq)*((SEC_PER_MIN)/(CPR))) //(freq*SEC_PER_MIN/CPR)
|
||||
#define SPEEDPPS2SPEEDRPM(freq) ((unsigned long)(freq)*(SEC_PER_MIN)/(CPR)) //(freq*SEC_PER_MIN/CPR)
|
||||
//#define SPEEDPPS2SPEED2VOLT(spd) ((spd)/SPEED_CONSTANT_RPM)
|
||||
//#define MAX_SPEEDPPS ((MAX_SPEEDRPM*CPR)/SEC_PER_MIN)
|
||||
//#define MIN_SPEEDPPS 0
|
||||
|
||||
#define KC 0.31
|
||||
#define TAUI 0.02
|
||||
#define TAUD 0.00
|
||||
#define SAMPLETIME 5
|
||||
|
||||
#define Baudrate 19200
|
||||
/*
|
||||
#ifndef DEBUG
|
||||
#define DEBUG
|
||||
#endif
|
||||
*/
|
||||
#ifdef DEBUG
|
||||
#define debug() { \
|
||||
if(!Serial.available()) Serial.begin(Baudrate); \
|
||||
Serial.println(__func__);}
|
||||
#else
|
||||
#define debug() {}
|
||||
#endif
|
||||
|
||||
|
||||
// Interrupt Type: RISING --> CHANGE 201207
|
||||
#define irqISR(y,x) \
|
||||
void x(); \
|
||||
struct ISRVars y={x}; \
|
||||
void x() { \
|
||||
static bool first_pulse=true; \
|
||||
y.pulseEndMicros=micros(); \
|
||||
if(first_pulse==false && y.pulseEndMicros>y.pulseStartMicros) { \
|
||||
y.speedPPS=MICROS_PER_SEC/(y.pulseEndMicros-y.pulseStartMicros); \
|
||||
/* y.accPPSS=(y.speedPPS-y.lastSpeedPPS)*y.speedPPS; */ \
|
||||
} else first_pulse=false; \
|
||||
y.pulseStartMicros=y.pulseEndMicros; \
|
||||
/* y.lastSpeedPPS=y.speedPPS; */ \
|
||||
if(y.pinIRQB!=PIN_UNDEFINED) \
|
||||
y.currDirection=DIR_INVERSE(digitalRead(y.pinIRQ)^digitalRead(y.pinIRQB)); \
|
||||
y.currDirection==DIR_ADVANCE?++y.pulses:--y.pulses; \
|
||||
}
|
||||
|
||||
struct ISRVars {
|
||||
void (*ISRfunc)();
|
||||
//volatile unsigned long pulses;
|
||||
volatile long pulses; // 201104, direction sensitive
|
||||
volatile unsigned long pulseStartMicros;
|
||||
volatile unsigned long pulseEndMicros;
|
||||
volatile unsigned int speedPPS;
|
||||
//volatile unsigned int lastSpeedPPS;
|
||||
//volatile int accPPSS; // acceleration, Pulse Per Sec^2
|
||||
volatile bool currDirection;
|
||||
unsigned char pinIRQB;
|
||||
unsigned char pinIRQ; // pinIRQA 201207
|
||||
};
|
||||
|
||||
class Motor: public PID {
|
||||
public:
|
||||
Motor(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr);
|
||||
|
||||
void setupInterrupt();
|
||||
|
||||
unsigned char getPinPWM() const;
|
||||
unsigned char getPinDir() const;
|
||||
unsigned char getPinIRQ() const;
|
||||
unsigned char getPinIRQB() const;
|
||||
|
||||
unsigned int runPWM(unsigned int PWM,bool dir,bool saveDir=true);
|
||||
unsigned int getPWM() const;
|
||||
unsigned int advancePWM(unsigned int PWM);
|
||||
unsigned int backoffPWM(unsigned int PWM);
|
||||
|
||||
bool setDesiredDir(bool dir);
|
||||
bool getDesiredDir() const;
|
||||
bool reverseDesiredDir();
|
||||
|
||||
bool setCurrDir();
|
||||
bool getCurrDir() const;
|
||||
|
||||
//int getAccRPMM() const; // Acceleration, Round Per Min^2
|
||||
//unsigned int getSpeedRPM() const;
|
||||
//unsigned int setSpeedRPM(int speedRPM,bool dir);
|
||||
// direction sensitive 201208
|
||||
int getSpeedRPM() const;
|
||||
unsigned int setSpeedRPM(int speedRPM,bool dir); // preserve
|
||||
int setSpeedRPM(int speedRPM);
|
||||
|
||||
//void simpleRegulate();
|
||||
|
||||
bool PIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000);
|
||||
bool PIDGetStatus() const;
|
||||
bool PIDEnable(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000);
|
||||
bool PIDDisable();
|
||||
bool PIDReset();
|
||||
bool PIDRegulate(bool doRegulate=true);
|
||||
unsigned int PIDSetSpeedRPMDesired(unsigned int speedRPM);
|
||||
unsigned int PIDGetSpeedRPMDesired() const;
|
||||
|
||||
void delayMS(unsigned int ms,bool debug=false);
|
||||
void debugger() const;
|
||||
|
||||
//int getAccPPSS() const;
|
||||
int getSpeedPPS() const;
|
||||
long getCurrPulse() const;
|
||||
long setCurrPulse(long _pulse);
|
||||
long resetCurrPulse();
|
||||
|
||||
struct ISRVars* isr;
|
||||
|
||||
private:
|
||||
unsigned char pinPWM;
|
||||
unsigned char pinDir;
|
||||
//unsigned char pinIRQ; // moved to isr
|
||||
//unsigned char pinIRQB;
|
||||
|
||||
//bool currDirection; // current direction
|
||||
bool desiredDirection; // desired direction
|
||||
unsigned int speedPWM; // current PWM
|
||||
|
||||
int speedRPMInput; // RPM: Round Per Minute
|
||||
int speedRPMOutput; // RPM
|
||||
int speedRPMDesired; // RPM
|
||||
//float PWMEC;
|
||||
float speed2DutyCycle;
|
||||
/*
|
||||
// the followings are defined in struct ISRvars,
|
||||
// because ISR must be a global function, without parameters and no return value
|
||||
|
||||
volatile unsigned int speedPPS; // PPS: Pulses Per Second
|
||||
volatile unsigned long pulseStartMicros;
|
||||
volatile unsigned long pulseEndMicros;
|
||||
*/
|
||||
bool pidCtrl;
|
||||
|
||||
Motor();
|
||||
|
||||
};
|
||||
|
||||
|
||||
#ifndef REDUCTION_RATIO
|
||||
#define REDUCTION_RATIO 64
|
||||
#endif
|
||||
class GearedMotor: public Motor { // RPM
|
||||
public:
|
||||
GearedMotor(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr,
|
||||
unsigned int _ratio=REDUCTION_RATIO);
|
||||
//float getGearedAccRPMM() const; // Acceleration, Round Per Min^2
|
||||
float getGearedSpeedRPM() const;
|
||||
float setGearedSpeedRPM(float gearedSpeedRPM,bool dir);
|
||||
// direction sensitive 201208
|
||||
float setGearedSpeedRPM(float gearedSpeedRPM);
|
||||
unsigned int getRatio() const;
|
||||
unsigned int setRatio(unsigned int ratio=REDUCTION_RATIO);
|
||||
float getPosition();
|
||||
private:
|
||||
unsigned int _ratio;
|
||||
};
|
||||
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.1416
|
||||
#endif
|
||||
//#define CIR 31.4 // cm
|
||||
//#define CIRMM 150 // mm
|
||||
//#define CIRMM 188
|
||||
#define CIRMM 314
|
||||
class MotorWheel: public GearedMotor { //
|
||||
public:
|
||||
MotorWheel(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr,
|
||||
unsigned int ratio=REDUCTION_RATIO,unsigned int cirMM=CIRMM);
|
||||
|
||||
unsigned int getCirMM() const;
|
||||
unsigned int setCirMM(unsigned int cirMM=CIRMM);
|
||||
/*
|
||||
int getAccCMPMM() const; // Acceleration, CM Per Min^2
|
||||
unsigned int getSpeedCMPM() const; // cm/min
|
||||
unsigned int setSpeedCMPM(unsigned int cm,bool dir);
|
||||
int getAccMMPSS() const; // Acceleration, MM Per Sec^2
|
||||
unsigned int getSpeedMMPS() const; // mm/s
|
||||
unsigned int setSpeedMMPS(unsigned int mm,bool dir);
|
||||
*/
|
||||
// direction sensitive 201208
|
||||
//int getAccCMPMM() const; // Acceleration, CM Per Min^2
|
||||
int getSpeedCMPM() const; // cm/min
|
||||
int setSpeedCMPM(unsigned int cm,bool dir); // preserve
|
||||
int setSpeedCMPM(int cm);
|
||||
//int getAccMMPSS() const; // Acceleration, MM Per Sec^2
|
||||
int getSpeedMMPS() const; // mm/s
|
||||
int setSpeedMMPS(unsigned int mm,bool dir); // preserve
|
||||
int setSpeedMMPS(int mm);
|
||||
void setGearedRPM(float rpm, bool dir);
|
||||
|
||||
//unsigned int runTime(unsigned int speedMMPS,bool dir,unsigned int TimeMS);
|
||||
//unsigned int runDistance(unsigned int speedMMPS,bool dir,unsigned int distanceMM);
|
||||
|
||||
private:
|
||||
unsigned int _cirMM;
|
||||
};
|
||||
|
||||
/*
|
||||
class samePID: public PID {
|
||||
public:
|
||||
samePID(int* input,int* output,int* setPoint,float kc,float taui,float taud)
|
||||
:PID(input,output,setPoint,kc,taui,taud) { }
|
||||
};
|
||||
|
||||
class servoMotor: public Motor,public samePID {
|
||||
public:
|
||||
servoMotor(unsigned char _pinPWM,unsigned char _pinDir,
|
||||
unsigned char _pinIRQ,unsigned char _pinIRQB,
|
||||
struct ISRVars* _isr);
|
||||
|
||||
int SPIDSetPulseBoundary(int min,int max);
|
||||
bool SPIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=10);
|
||||
bool PIDRegulate(bool doRegulate=true);
|
||||
|
||||
enum {
|
||||
SPIDMODE_UNKNOWN,
|
||||
SPIDMODE_SPEED,
|
||||
SPIDMODE_STOP,
|
||||
SPIDMODE_PULSE,
|
||||
};
|
||||
unsigned char getSPIDMode() const;
|
||||
unsigned char setSPIDMode(unsigned char _mode);
|
||||
|
||||
int getPulseDesired() const;
|
||||
int setPulseDesired(int _pulse);
|
||||
int incPulseDesired();
|
||||
int decPulseDesired();
|
||||
|
||||
bool backToZero();
|
||||
bool scanZero();
|
||||
|
||||
void debugger() const;
|
||||
|
||||
private:
|
||||
int pulseInput; //
|
||||
int pulseOutput; //
|
||||
int pulseDesired; //
|
||||
|
||||
int pulse2SpeedRPM;
|
||||
|
||||
unsigned char SPIDMode;
|
||||
|
||||
bool SPIDRegulateSpeed(bool doRegulate=true); // 201104
|
||||
bool SPIDRegulateStop(bool doRegulate=true);
|
||||
bool SPIDRegulatePulse(bool doRegulate=true);
|
||||
|
||||
};
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -0,0 +1,31 @@
|
||||
/********************************************************
|
||||
* PID Simple Example
|
||||
* Reading analog input 0 to control analog PWM output 3
|
||||
********************************************************/
|
||||
|
||||
#include <PID_Beta6.h>
|
||||
|
||||
//Define Variables we'll be connecting to
|
||||
double Setpoint, Input, Output;
|
||||
|
||||
//Specify the links and initial tuning parameters
|
||||
PID myPID(&Input, &Output, &Setpoint,2,5,1);
|
||||
|
||||
void setup()
|
||||
{
|
||||
//initialize the variables we're linked to
|
||||
Input = analogRead(0);
|
||||
Setpoint = 100;
|
||||
|
||||
//turn the PID on
|
||||
myPID.SetMode(AUTO);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
Input = analogRead(0);
|
||||
myPID.Compute();
|
||||
analogWrite(3,Output);
|
||||
}
|
||||
|
||||
|
||||
@ -0,0 +1,55 @@
|
||||
/***************************************************
|
||||
* Advanced PID Example
|
||||
* PID input: Analog input 0
|
||||
* output = time proportion on/off of output 0 (50% = 2 sec on, 2 sec off)
|
||||
* and just to make it more complicated:
|
||||
* - start with a setpoint ramp (0 to 500 over 5 minutes (300 seconds))
|
||||
* - use gap control (moderate tunings when within 100 of setpoint, aggressive when outside 100)
|
||||
***************************************************/
|
||||
|
||||
#include <PID_Beta6.h>
|
||||
|
||||
unsigned long windowStartTime;
|
||||
unsigned long RampStartTime;
|
||||
double Input, Output, Setpoint;
|
||||
PID pid(&Input, &Output, &Setpoint, 3,4,1);
|
||||
|
||||
unsigned long printTime= millis();
|
||||
|
||||
void setup()
|
||||
{
|
||||
|
||||
pid.SetOutputLimits(0,4000); //tell the PID to range the output from 0 to 4000
|
||||
Output = 4000; //start the output at its max and let the PID adjust it from there
|
||||
|
||||
pid.SetMode(AUTO); //turn on the PID
|
||||
windowStartTime = millis();
|
||||
RampStartTime = millis();
|
||||
}
|
||||
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
//Set Point Ramp
|
||||
if(millis()-RampStartTime<300000)
|
||||
{
|
||||
Setpoint = ((double)(millis()-RampStartTime))/(300000.0)*500.0;
|
||||
}
|
||||
else Setpoint = 500;
|
||||
|
||||
//Set Tuning Parameters based on how close we are to setpoint
|
||||
if(abs(Setpoint-Input)>100)pid.SetTunings(6,4,1); //aggressive
|
||||
else pid.SetTunings(3,4,1); //comparatively moderate
|
||||
|
||||
//give the PID the opportunity to compute if needed
|
||||
Input = analogRead(0);
|
||||
pid.Compute();
|
||||
|
||||
//turn the output pin on/off based on pid output
|
||||
if(millis()-windowStartTime>4000) windowStartTime+=4000;
|
||||
if(Output > millis()-windowStartTime) digitalWrite(0,HIGH);
|
||||
else digitalWrite(0,LOW);
|
||||
|
||||
}
|
||||
|
||||
@ -0,0 +1,54 @@
|
||||
#include <fuzzy_table.h>
|
||||
#include <PID_Beta6.h>
|
||||
|
||||
/***************************************************
|
||||
* Feed Forward Example : RepRap Extruder Nozzle Temperature
|
||||
* PID input: Nozzle Temperature
|
||||
* PID output: PWM signal (0-255) to the heater control
|
||||
* Other Input: Plastic Feed rate is being read on input 1
|
||||
* For the feed forward piece...
|
||||
* The amount of heat we want to add to the nozzle is largely dependent
|
||||
* on how fast plastic is being fed. if the feed is stopped we're going
|
||||
* to want a lot less heat than when the system is running. A pid
|
||||
* control by itself will eventually adjust the heat, but by then the temp
|
||||
* will be really high. feed forward can help with this.
|
||||
* so to make Feed Forward work, at every cycle we look at the current feed speed
|
||||
* and compute a "bias" which we feed to the pid controller. sort of: "Based on my
|
||||
* knowledge, your output should be about X. adjust off of that as you see fit"
|
||||
* What does it get you? well, the instant the feed stops, the baseline drops. That
|
||||
* means that the controller output immediately drops. if you had been using just a pid,
|
||||
* you'd have to wait for the temperature to rise quite a bit before seeing the same
|
||||
* drop in output.
|
||||
*
|
||||
* (for this example, we're assuming that for a feed rate of 1023 the output should be
|
||||
* about 130, and at a rate of 0 the output should be around 10)
|
||||
***************************************************/
|
||||
|
||||
#include <PID_Beta6.h>
|
||||
int FeedRate;
|
||||
|
||||
double Input, Output, Setpoint, Bias;
|
||||
PID pid(&Input, &Output, &Setpoint, &Bias, 3, 4, 1);
|
||||
|
||||
void setup()
|
||||
{
|
||||
Setpoint = 400;
|
||||
pid.SetMode(AUTO); //turn on the PID
|
||||
|
||||
}
|
||||
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
//Read in the Feed rate and compute a baseline output to the heater
|
||||
FeedRate = analogRead(1); //read the feed rate
|
||||
Bias = (FeedRate/1023)*(120)+10;
|
||||
|
||||
//give the PID the opportunity to compute if needed
|
||||
Input = analogRead(0);
|
||||
pid.Compute();
|
||||
|
||||
analogWrite(0, Output);
|
||||
|
||||
}
|
||||
309
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp
Normal file
309
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp
Normal file
@ -0,0 +1,309 @@
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "wiring.h"
|
||||
#endif
|
||||
|
||||
#include <PID_Beta6.h>
|
||||
#include "fuzzy_table.h"
|
||||
#include <wiring_private.h>
|
||||
#include <HardwareSerial.h>
|
||||
|
||||
/* Standard Constructor (...)***********************************************
|
||||
* constructor used by most users. the parameters specified are those for
|
||||
* for which we can't set up reliable defaults, so we need to have the user
|
||||
* set them.
|
||||
***************************************************************************/
|
||||
PID::PID(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD)
|
||||
{
|
||||
|
||||
PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD);
|
||||
UsingFeedForward = false;
|
||||
PID::Reset();
|
||||
|
||||
|
||||
}
|
||||
|
||||
/* Overloaded Constructor(...)**********************************************
|
||||
* This one is for more advanced users. it's essentially the same as the
|
||||
* standard constructor, with one addition. you can link to a Feed Forward bias,
|
||||
* which lets you implement... um.. Feed Forward Control. good stuff.
|
||||
***************************************************************************/
|
||||
PID::PID(int *Input, int *Output, int *Setpoint, int *FFBias, float Kc, float TauI, float TauD)
|
||||
{
|
||||
|
||||
PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD);
|
||||
UsingFeedForward = true; //tell the controller that we'll be using an external
|
||||
myBias = FFBias; //bias, and where to find it
|
||||
PID::Reset();
|
||||
|
||||
}
|
||||
|
||||
/* ConstructorCommon(...)****************************************************
|
||||
* Most of what is done in the two constructors is the same. that code
|
||||
* was put here for ease of maintenance and (minor) reduction of library size
|
||||
****************************************************************************/
|
||||
void PID::ConstructorCommon(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD)
|
||||
{
|
||||
PID::SetInputLimits(0, 1023); //default the limits to the
|
||||
PID::SetOutputLimits(0, 255); //full ranges of the I/O
|
||||
|
||||
tSample = 1000; //default Controller Sample Time is 1 second
|
||||
|
||||
PID::SetTunings( Kc, TauI, TauD);
|
||||
|
||||
nextCompTime = millis();
|
||||
inAuto = false;
|
||||
myOutput = Output;
|
||||
myInput = Input;
|
||||
mySetpoint = Setpoint;
|
||||
|
||||
Err = lastErr = prevErr = 0;
|
||||
}
|
||||
|
||||
|
||||
/* SetInputLimits(...)*****************************************************
|
||||
* I don't see this function being called all that much (other than from the
|
||||
* constructor.) it needs to be here so we can tell the controller what it's
|
||||
* input limits are, and in most cases the 0-1023 default should be fine. if
|
||||
* there's an application where the signal being fed to the controller is
|
||||
* outside that range, well, then this function's here for you.
|
||||
**************************************************************************/
|
||||
void PID::SetInputLimits(int INMin, int INMax)
|
||||
{
|
||||
//after verifying that mins are smaller than maxes, set the values
|
||||
if(INMin >= INMax) return;
|
||||
|
||||
|
||||
inMin = INMin;
|
||||
inSpan = INMax - INMin;
|
||||
}
|
||||
|
||||
/* SetOutputLimits(...)****************************************************
|
||||
* This function will be used far more often than SetInputLimits. while
|
||||
* the input to the controller will generally be in the 0-1023 range (which is
|
||||
* the default already,) the output will be a little different. maybe they'll
|
||||
* be doing a time window and will need 0-8000 or something. or maybe they'll
|
||||
* want to clamp it from 0-125. who knows. at any rate, that can all be done
|
||||
* here.
|
||||
**************************************************************************/
|
||||
void PID::SetOutputLimits(int OUTMin, int OUTMax)
|
||||
{
|
||||
//after verifying that mins are smaller than maxes, set the values
|
||||
if(OUTMin >= OUTMax) return;
|
||||
|
||||
outMin = OUTMin;
|
||||
outSpan = OUTMax - OUTMin;
|
||||
}
|
||||
|
||||
/* SetTunings(...)*************************************************************
|
||||
* This function allows the controller's dynamic performance to be adjusted.
|
||||
* it's called automatically from the constructor, but tunings can also
|
||||
* be adjusted on the fly during normal operation
|
||||
******************************************************************************/
|
||||
void PID::SetTunings(float Kc, float TauI, float TauD)
|
||||
{
|
||||
//verify that the tunings make sense
|
||||
if (Kc == 0.0 || TauI < 0.0 || TauD < 0.0) return;
|
||||
|
||||
//we're going to do some funky things to the input numbers so all
|
||||
//our math works out, but we want to store the numbers intact
|
||||
//so we can return them to the user when asked.
|
||||
P_Param = Kc;
|
||||
I_Param = TauI;
|
||||
D_Param = TauD;
|
||||
|
||||
//convert Reset Time into Reset Rate, and compensate for Calculation frequency
|
||||
float tSampleInSec = ((float)tSample / 1000.0);
|
||||
float tempTauR;
|
||||
if (TauI == 0.0)
|
||||
tempTauR = 0.0;
|
||||
else
|
||||
tempTauR = (1.0 / TauI) * tSampleInSec;
|
||||
|
||||
|
||||
kc = Kc;
|
||||
taur = tempTauR;
|
||||
taud = TauD / tSampleInSec;
|
||||
|
||||
cof_A = kc * (1 + taur + taud);
|
||||
cof_B = kc * (1 + 2 * taud);
|
||||
cof_C = kc * taud;
|
||||
}
|
||||
|
||||
/* Reset()*********************************************************************
|
||||
* does all the things that need to happen to ensure a bumpless transfer
|
||||
* from manual to automatic mode. this shouldn't have to be called from the
|
||||
* outside. In practice though, it is sometimes helpful to start from scratch,
|
||||
* so it was made publicly available
|
||||
******************************************************************************/
|
||||
void PID::Reset()
|
||||
{
|
||||
|
||||
if(UsingFeedForward)
|
||||
bias = (*myBias - outMin) / outSpan;
|
||||
else
|
||||
bias = (*myOutput - outMin) / outSpan;
|
||||
|
||||
}
|
||||
|
||||
/* SetMode(...)****************************************************************
|
||||
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
|
||||
* when the transition from manual to auto occurs, the controller is
|
||||
* automatically initialized
|
||||
******************************************************************************/
|
||||
void PID::SetMode(int Mode)
|
||||
{
|
||||
if (Mode!=0 && !inAuto)
|
||||
{ //we were in manual, and we just got set to auto.
|
||||
//reset the controller internals
|
||||
PID::Reset();
|
||||
}
|
||||
inAuto = (Mode!=0);
|
||||
}
|
||||
|
||||
/* SetSampleTime(...)*******************************************************
|
||||
* sets the frequency, in Milliseconds, with which the PID calculation is performed
|
||||
******************************************************************************/
|
||||
void PID::SetSampleTime(int NewSampleTime)
|
||||
{
|
||||
if (NewSampleTime > 0)
|
||||
{
|
||||
//convert the time-based tunings to reflect this change
|
||||
taur *= ((float)NewSampleTime)/((float) tSample);
|
||||
taud *= ((float)NewSampleTime)/((float) tSample);
|
||||
tSample = (unsigned long)NewSampleTime;
|
||||
|
||||
cof_A = kc * (1 + taur + taud);
|
||||
cof_B = kc * (1 + 2 * taud);
|
||||
cof_C = kc * taud;
|
||||
}
|
||||
}
|
||||
|
||||
/* Compute() **********************************************************************
|
||||
* This, as they say, is where the magic happens. this function should be called
|
||||
* every time "void loop()" executes. the function will decide for itself whether a new
|
||||
* pid Output needs to be computed
|
||||
*
|
||||
* Some notes for people familiar with the nuts and bolts of PID control:
|
||||
* - I used the Ideal form of the PID equation. mainly because I like IMC
|
||||
* tunings. lock in the I and D, and then just vary P to get more
|
||||
* aggressive or conservative
|
||||
*
|
||||
* - While this controller presented to the outside world as being a Reset Time
|
||||
* controller, when the user enters their tunings the I term is converted to
|
||||
* Reset Rate. I did this merely to avoid the div0 error when the user wants
|
||||
* to turn Integral action off.
|
||||
*
|
||||
* - Derivative on Measurement is being used instead of Derivative on Error. The
|
||||
* performance is identical, with one notable exception. DonE causes a kick in
|
||||
* the controller output whenever there's a setpoint change. DonM does not.
|
||||
*
|
||||
* If none of the above made sense to you, and you would like it to, go to:
|
||||
* http://www.controlguru.com . Dr. Cooper was my controls professor, and is
|
||||
* gifted at concisely and clearly explaining PID control
|
||||
*********************************************************************************/
|
||||
void PID::Compute()
|
||||
{
|
||||
justCalced=false;
|
||||
if (!inAuto) return; //if we're in manual just leave;
|
||||
|
||||
unsigned long now = millis();
|
||||
|
||||
//millis() wraps around to 0 at some point. depending on the version of the
|
||||
//Arduino Program you are using, it could be in 9 hours or 50 days.
|
||||
//this is not currently addressed by this algorithm.
|
||||
|
||||
|
||||
//...Perform PID Computations if it's time...
|
||||
if (now>=nextCompTime)
|
||||
{
|
||||
|
||||
Err = *mySetpoint - *myInput;
|
||||
//if we're using an external bias (i.e. the user used the
|
||||
//overloaded constructor,) then pull that in now
|
||||
if(UsingFeedForward)
|
||||
{
|
||||
bias = *myBias - outMin;
|
||||
}
|
||||
|
||||
|
||||
// perform the PID calculation.
|
||||
//float output = bias + kc * ((Err - lastErr)+ (taur * Err) + (taud * (Err - 2*lastErr + prevErr)));
|
||||
noInterrupts();
|
||||
int output = bias + (cof_A * Err - cof_B * lastErr + cof_C * prevErr);
|
||||
interrupts();
|
||||
|
||||
//make sure the computed output is within output constraints
|
||||
if (output < -outSpan) output = -outSpan;
|
||||
else if (output > outSpan) output = outSpan;
|
||||
|
||||
|
||||
prevErr = lastErr;
|
||||
lastErr = Err;
|
||||
|
||||
|
||||
//scale the output from percent span back out to a real world number
|
||||
*myOutput = output;
|
||||
|
||||
nextCompTime += tSample; // determine the next time the computation
|
||||
if(nextCompTime < now) nextCompTime = now + tSample; // should be performed
|
||||
|
||||
justCalced=true; //set the flag that will tell the outside world that the output was just computed
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
* STATUS SECTION
|
||||
* These functions allow the outside world to query the status of the PID
|
||||
*****************************************************************************/
|
||||
|
||||
bool PID::JustCalculated()
|
||||
{
|
||||
return justCalced;
|
||||
}
|
||||
int PID::GetMode()
|
||||
{
|
||||
if(inAuto)return 1;
|
||||
else return 0;
|
||||
}
|
||||
|
||||
int PID::GetINMin()
|
||||
{
|
||||
return inMin;
|
||||
}
|
||||
int PID::GetINMax()
|
||||
{
|
||||
return inMin + inSpan;
|
||||
}
|
||||
int PID::GetOUTMin()
|
||||
{
|
||||
return outMin;
|
||||
}
|
||||
int PID::GetOUTMax()
|
||||
{
|
||||
return outMin+outSpan;
|
||||
}
|
||||
int PID::GetSampleTime()
|
||||
{
|
||||
return tSample;
|
||||
}
|
||||
float PID::GetP_Param()
|
||||
{
|
||||
return P_Param;
|
||||
}
|
||||
float PID::GetI_Param()
|
||||
{
|
||||
return I_Param;
|
||||
}
|
||||
|
||||
float PID::GetD_Param()
|
||||
{
|
||||
return D_Param;
|
||||
}
|
||||
|
||||
113
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h
Normal file
113
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h
Normal file
@ -0,0 +1,113 @@
|
||||
#ifndef PID_Beta6_h
|
||||
#define PID_Beta6_h
|
||||
|
||||
class PID
|
||||
{
|
||||
|
||||
|
||||
public:
|
||||
|
||||
#define AUTO 1
|
||||
#define MANUAL 0
|
||||
#define LIBRARY_VERSION 0.6
|
||||
|
||||
//commonly used functions **************************************************************************
|
||||
PID(int*, int*, int*, // * constructor. links the PID to the Input, Output, and
|
||||
float, float, float); // Setpoint. Initial tuning parameters are also set here
|
||||
|
||||
PID(int*, int*, int*, // * Overloaded Constructor. if the user wants to implement
|
||||
int*, float, float, float); // feed-forward
|
||||
|
||||
void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0)
|
||||
|
||||
void Compute(); // * performs the PID calculation. it should be
|
||||
// called every time loop() cycles. ON/OFF and
|
||||
// calculation frequency can be set using SetMode
|
||||
// SetSampleTime respectively
|
||||
|
||||
void SetInputLimits(int, int); //Tells the PID what 0-100% are for the Input
|
||||
|
||||
void SetOutputLimits(int, int); //Tells the PID what 0-100% are for the Output
|
||||
|
||||
|
||||
//available but not commonly used functions ********************************************************
|
||||
void SetTunings(float, float, // * While most users will set the tunings once in the
|
||||
float); // constructor, this function gives the user the option
|
||||
// of changing tunings during runtime for Adaptive control
|
||||
|
||||
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
|
||||
// the PID calculation is performed. default is 1000
|
||||
|
||||
void Reset(); // * reinitializes controller internals. automatically
|
||||
// called on a manual to auto transition
|
||||
|
||||
bool JustCalculated(); // * in certain situations, it helps to know when the PID has
|
||||
// computed this bit will be true for one cycle after the
|
||||
// pid calculation has occurred
|
||||
|
||||
|
||||
//Status functions allow you to query current PID constants ***************************************
|
||||
int GetMode();
|
||||
int GetINMin();
|
||||
int GetINMax();
|
||||
int GetOUTMin();
|
||||
int GetOUTMax();
|
||||
int GetSampleTime();
|
||||
float GetP_Param();
|
||||
float GetI_Param();
|
||||
float GetD_Param();
|
||||
|
||||
|
||||
private:
|
||||
|
||||
void ConstructorCommon(int*, int*, int*, // * code that is shared by the constructors
|
||||
float, float, float);
|
||||
|
||||
//scaled, tweaked parameters we'll actually be using
|
||||
float kc; // * (P)roportional Tuning Parameter
|
||||
float taur; // * (I)ntegral Tuning Parameter
|
||||
float taud; // * (D)erivative Tuning Parameter
|
||||
|
||||
float cof_A;
|
||||
float cof_B;
|
||||
float cof_C;
|
||||
|
||||
//nice, pretty parameters we'll give back to the user if they ask what the tunings are
|
||||
float P_Param;
|
||||
float I_Param;
|
||||
float D_Param;
|
||||
|
||||
|
||||
int *myInput; // * Pointers to the Input, Output, and Setpoint variables
|
||||
int *myOutput; // This creates a hard link between the variables and the
|
||||
int *mySetpoint; // PID, freeing the user from having to constantly tell us
|
||||
// what these values are. with pointers we'll just know.
|
||||
|
||||
int *myBias; // * Pointer to the External FeedForward bias, only used
|
||||
// if the advanced constructor is used
|
||||
bool UsingFeedForward; // * internal flag that tells us if we're using FeedForward or not
|
||||
|
||||
unsigned long nextCompTime; // * Helps us figure out when the PID Calculation needs to
|
||||
// be performed next
|
||||
// to determine when to compute next
|
||||
unsigned long tSample; // * the frequency, in milliseconds, with which we want the
|
||||
// the PID calculation to occur.
|
||||
bool inAuto; // * Flag letting us know if we are in Automatic or not
|
||||
|
||||
// the derivative required for the D term
|
||||
//float accError; // * the (I)ntegral term is based on the sum of error over
|
||||
// time. this variable keeps track of that
|
||||
float bias; // * the base output from which the PID operates
|
||||
|
||||
int Err;
|
||||
int lastErr;
|
||||
int prevErr;
|
||||
|
||||
float inMin, inSpan; // * input and output limits, and spans. used convert
|
||||
float outMin, outSpan; // real world numbers into percent span, with which
|
||||
// the PID algorithm is more comfortable.
|
||||
|
||||
bool justCalced; // * flag gets set for one cycle after the pid calculates
|
||||
};
|
||||
#endif
|
||||
|
||||
BIN
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o
Normal file
BIN
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o
Normal file
Binary file not shown.
66
src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h
Normal file
66
src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h
Normal file
@ -0,0 +1,66 @@
|
||||
|
||||
#if 0
|
||||
#ifndef _PID_TABLE_H_
|
||||
#define _PID_TABLE_H_
|
||||
|
||||
#define PB 6
|
||||
#define PM 5
|
||||
#define PS 4
|
||||
#define ZO 3
|
||||
#define NS 2
|
||||
#define NM 1
|
||||
#define NB 0
|
||||
|
||||
#define DELTA_KP_IDX 0
|
||||
#define DELATA_KI_IDX 1
|
||||
#define ERR 7
|
||||
#define DELTA_ERR 7
|
||||
|
||||
|
||||
const char pid_tbl[2][7][7]={
|
||||
{
|
||||
{PB,PB,PM,PM,PS,ZO,ZO},
|
||||
{PB,PB,PM,PS,PS,ZO,ZO},
|
||||
{PM,PM,PM,PS,ZO,NS,NS},
|
||||
{PM,PM,PS,ZO,NS,NM,NM},
|
||||
{PS,PS,ZO,NS,NS,NM,NM},
|
||||
{PS,ZO,NS,NM,NM,NM,NB},
|
||||
{ZO,ZO,NM,NM,NM,NB,NB},
|
||||
},
|
||||
|
||||
{
|
||||
{NB,NB,NM,NM,NS,ZO,ZO},
|
||||
{NB,NB,NM,NS,NS,ZO,ZO},
|
||||
{NB,NM,NS,NS,ZO,PS,PS},
|
||||
{NM,NM,NS,ZO,PS,PM,PM},
|
||||
{NM,NS,ZO,PS,PS,PM,PB},
|
||||
{ZO,ZO,PS,PS,PM,PB,PB},
|
||||
{ZO,ZO,PS,PM,PM,PB,PB},
|
||||
},
|
||||
};
|
||||
|
||||
const float kp_tbl[ERR] = {
|
||||
0.07987,
|
||||
0.08974,
|
||||
0.09948,
|
||||
0.12922,
|
||||
0.54896,
|
||||
0.70000,
|
||||
0.76000,
|
||||
};
|
||||
|
||||
|
||||
const float ki_tbl[DELTA_ERR] = {
|
||||
0.02451,
|
||||
0.07353,
|
||||
0.14706,
|
||||
0.19608,
|
||||
0.24510,
|
||||
0.30000,
|
||||
0.00000,
|
||||
};
|
||||
|
||||
#endif _PID_TABLE_H_
|
||||
|
||||
#endif
|
||||
|
||||
38
src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt
Normal file
38
src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt
Normal file
@ -0,0 +1,38 @@
|
||||
#######################################
|
||||
# Syntax Coloring Map For LiquidCrystal
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
PID KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
SetMode KEYWORD2
|
||||
Compute KEYWORD2
|
||||
SetInputLimits KEYWORD2
|
||||
SetOutputLimits KEYWORD2
|
||||
SetTunings KEYWORD2
|
||||
SetSampleTime KEYWORD2
|
||||
JustCalculated KEYWORD2
|
||||
Reset KEYWORD2
|
||||
GetMode KEYWORD2
|
||||
GetINMin KEYWORD2
|
||||
GetINMax KEYWORD2
|
||||
GetOUTMin KEYWORD2
|
||||
GetOUTMax KEYWORD2
|
||||
GetSampleTime KEYWORD2
|
||||
GetP_Param KEYWORD2
|
||||
GetI_Param KEYWORD2
|
||||
GetD_Param KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
AUTO LITERAL1
|
||||
MANUAL LITERAL1
|
||||
191
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp
Normal file
191
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp
Normal file
@ -0,0 +1,191 @@
|
||||
/*
|
||||
PinChangeInt.cpp
|
||||
*/
|
||||
|
||||
#include <PinChangeInt.h>
|
||||
|
||||
|
||||
PCintPort::PCintPin PCintPort::PCintPin::pinDataAlloc[MAX_PIN_CHANGE_PINS];
|
||||
|
||||
|
||||
PCintPort PCintPort::pcIntPorts[] = {
|
||||
PCintPort(0,PCMSK0),
|
||||
PCintPort(1,PCMSK1),
|
||||
PCintPort(2,PCMSK2)
|
||||
};
|
||||
|
||||
void PCintPort::addPin(uint8_t mode,uint8_t mask,PCIntvoidFuncPtr userFunc)
|
||||
{
|
||||
//int i = 0;
|
||||
PCintPin* p = PCintPin::pinDataAlloc;
|
||||
do {
|
||||
if (!p->PCintMode) { // found an available pin data structure
|
||||
// now find the next slot to put it in
|
||||
PCintPin** t = &pcIntPins[0];
|
||||
do {
|
||||
if (!*t) { // we have a slot
|
||||
// fill in the data
|
||||
p->PCintMode = mode;
|
||||
*t = p;
|
||||
p->PCintFunc = userFunc;
|
||||
// set the mask
|
||||
pcmask |= p->PCIntMask = mask;
|
||||
// enable the interrupt
|
||||
PCICR |= PCICRbit;
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.print("BitMask = ");
|
||||
Serial.print(mask, BIN);
|
||||
Serial.print("; ");
|
||||
Serial.print("slot = ");
|
||||
Serial.println((int)(t - &pcIntPins[0]), DEC);
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
} while (int(++t) < int(&pcIntPins[8]));
|
||||
}
|
||||
} while (int(++p) < int(&PCintPin::pinDataAlloc[MAX_PIN_CHANGE_PINS]));
|
||||
}
|
||||
|
||||
void PCintPort::delPin(uint8_t mask)
|
||||
{
|
||||
PCintPin** t = pcIntPins;
|
||||
while (*t) {
|
||||
PCintPin& p = **t;
|
||||
if (p.PCIntMask == mask) { // found the target
|
||||
uint8_t oldSREG = SREG;
|
||||
cli();
|
||||
// disable the mask.
|
||||
pcmask &= ~mask;
|
||||
// if that's the last one, disable the interrupt.
|
||||
if (pcmask == 0) {
|
||||
PCICR &= ~(PCICRbit);
|
||||
}
|
||||
p.PCIntMask = 0;
|
||||
p.PCintMode = 0;
|
||||
p.PCintFunc = NULL;
|
||||
do { // shuffle down as we pass through the list, filling the hole
|
||||
*t = t[1];
|
||||
} while (*t);
|
||||
SREG = oldSREG;
|
||||
return;
|
||||
}
|
||||
t++;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* attach an interrupt to a specific pin using pin change interrupts.
|
||||
*/
|
||||
void PCintPort::attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode)
|
||||
{
|
||||
uint8_t portNum = digitalPinToPort(pin);
|
||||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||
if (((portNum != 2) && (portNum != 11))
|
||||
|| (userFunc == NULL)
|
||||
) {
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.println("NOT_A_PORT det");
|
||||
#endif
|
||||
|
||||
return;
|
||||
}
|
||||
// map pin to PCIR register
|
||||
uint8_t portIndex = (portNum == 2)?(0):(2);
|
||||
#else
|
||||
if ((portNum == NOT_A_PORT) || (userFunc == NULL)) {
|
||||
return;
|
||||
}
|
||||
// map pin to PCIR register
|
||||
uint8_t portIndex = portNum - 2;
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.print("portNum = ");
|
||||
Serial.print(portNum, DEC);
|
||||
Serial.print("; ");
|
||||
#endif
|
||||
|
||||
PCintPort &port = PCintPort::pcIntPorts[portIndex];
|
||||
port.addPin(mode,digitalPinToBitMask(pin),userFunc);
|
||||
}
|
||||
|
||||
void PCintPort::detachInterrupt(uint8_t pin)
|
||||
{
|
||||
uint8_t portNum = digitalPinToPort(pin);
|
||||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||
if ((portNum != 2) && (portNum != 11)) {
|
||||
//Serial.println("NOT_A_PORT det");
|
||||
return;
|
||||
}
|
||||
uint8_t portIndex = (portNum == 2)?(0):(2);
|
||||
PCintPort& port = PCintPort::pcIntPorts[portIndex];
|
||||
|
||||
#else
|
||||
if (portNum == NOT_A_PORT) {
|
||||
//Serial.println("NOT_A_PORT det");
|
||||
return;
|
||||
}
|
||||
PCintPort& port = PCintPort::pcIntPorts[portNum - 2];
|
||||
#endif
|
||||
|
||||
port.delPin(digitalPinToBitMask(pin));
|
||||
}
|
||||
|
||||
// common code for isr handler. "port" is the PCINT number.
|
||||
// there isn't really a good way to back-map ports and masks to pins.
|
||||
void PCintPort::PCint() {
|
||||
#ifndef DISABLE_PCINT_MULTI_SERVICE
|
||||
uint8_t pcifr;
|
||||
do {
|
||||
#endif
|
||||
uint8_t curr = portInputReg;
|
||||
uint8_t mask = curr ^ PCintLast;
|
||||
// get the pin states for the indicated port.
|
||||
// mask is pins that have changed. screen out non pcint pins.
|
||||
if ((mask &= pcmask) == 0) {
|
||||
return;
|
||||
}
|
||||
PCintLast = curr;
|
||||
PCintPin** t = pcIntPins;
|
||||
while (*t) {
|
||||
PCintPin& p = **t;
|
||||
if (p.PCIntMask & mask) { // a changed bit
|
||||
// Trigger interrupt if mode is CHANGE, or if mode is RISING and
|
||||
// the bit is currently high, or if mode is FALLING and bit is low.
|
||||
if (p.PCintMode == CHANGE
|
||||
|| ((p.PCintMode == FALLING) && !(curr & p.PCIntMask))
|
||||
|| ((p.PCintMode == RISING) && (curr & p.PCIntMask))
|
||||
) {
|
||||
p.PCintFunc();
|
||||
}
|
||||
}
|
||||
t++;
|
||||
}
|
||||
#ifndef DISABLE_PCINT_MULTI_SERVICE
|
||||
pcifr = PCIFR & PCICRbit;
|
||||
PCIFR = pcifr; // clear the interrupt if we will process is (no effect if bit is zero)
|
||||
} while(pcifr);
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifndef NO_PORTB_PINCHANGES
|
||||
ISR(PCINT0_vect) {
|
||||
PCintPort::pcIntPorts[0].PCint();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef NO_PORTC_PINCHANGES
|
||||
ISR(PCINT1_vect) {
|
||||
PCintPort::pcIntPorts[1].PCint();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef NO_PORTD_PINCHANGES
|
||||
ISR(PCINT2_vect) {
|
||||
PCintPort::pcIntPorts[2].PCint();
|
||||
}
|
||||
#endif
|
||||
|
||||
136
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h
Normal file
136
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h
Normal file
@ -0,0 +1,136 @@
|
||||
/*
|
||||
PinChangeInt.h
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PinChangeInt_h
|
||||
#define PinChangeInt_h
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#include "PinChangeIntConfig.h"
|
||||
#ifndef Pins_Arduino_h
|
||||
#include "pins_arduino.h"
|
||||
#endif
|
||||
// This library was inspired by and derived from "johnboiles" (it seems) Main.PCInt Arduino Playground example
|
||||
// see: http://www.arduino.cc/playground/Main/PcInt
|
||||
|
||||
|
||||
// Interrupt management for PCI
|
||||
/*
|
||||
* an extension to the interrupt support for arduino.
|
||||
* add pin change interrupts to the external interrupts, giving a way
|
||||
* for users to have interrupts drive off of any pin.
|
||||
* Refer to avr-gcc header files, arduino source and atmega datasheet.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Theory: all IO pins on AtmegaX(168/328/1280/2560) are covered by Pin Change Interrupts.
|
||||
* The PCINT corresponding to the pin must be enabled and masked, and
|
||||
* an ISR routine provided. Since PCINTs are per port, not per pin, the ISR
|
||||
* must use some logic to actually implement a per-pin interrupt service.
|
||||
*/
|
||||
|
||||
/* Pin to interrupt map ON ATmega168/328:
|
||||
* D0-D7 = PCINT 16-23 = PCIR2 = PD = PCIE2 = pcmsk2
|
||||
* D8-D13 = PCINT 0-5 = PCIR0 = PB = PCIE0 = pcmsk0
|
||||
* A0-A5 (D14-D19) = PCINT 8-13 = PCIR1 = PC = PCIE1 = pcmsk1
|
||||
*/
|
||||
|
||||
|
||||
/* Pin to interrupt map ON ATmega1280/2560:
|
||||
* D50-D53 = PCINT 3-0 = PCIR0 = PB = PCIE0 = pcmsk0
|
||||
* D10-D13 = PCINT 4-7 = PCIR0 = PB = PCIE0 = pcmsk0
|
||||
* A8-A15 (D62-D69) = PCINT 16-23 = PCIR2 = Pk = PCIE2 = pcmsk2
|
||||
|
||||
* *******D0 = PCINT 8 = PCIR1 = PE = PCIE1 = pcmsk1**********
|
||||
* *******D14-D15 = PCINT 10-9 = PJ = PCIE1 = pcmsk1*********
|
||||
********NOTE:PCINT 8-15 does NOT available in this library******
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
Please make any configuration changes in the accompanying PinChangeIntConfig.h file.
|
||||
This will help avoid having to reset your config in the event of changes to the
|
||||
library code (just don't replace that file when you update).
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#ifndef MAX_PIN_CHANGE_PINS
|
||||
#define MAX_PIN_CHANGE_PINS 8
|
||||
#endif
|
||||
|
||||
// You can reduce the memory footprint of this handler by declaring that there will be no pin change interrupts
|
||||
// on any of the three ports.
|
||||
// define NO_PORTB_PINCHANGES to indicate that port b will not be used for pin change interrupts
|
||||
// define NO_PORTC_PINCHANGES to indicate that port c will not be used for pin change interrupts
|
||||
// define NO_PORTD_PINCHANGES to indicate that port d will not be used for pin change interrupts
|
||||
// If only a single port remains, the handler will be declared inline reducing the size and latency
|
||||
// of the handler.
|
||||
|
||||
// if their is only one PCInt vector in use the code can be inlined
|
||||
// reducing latecncy and code size
|
||||
#define INLINE_PCINT
|
||||
#if ((defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES)) || \
|
||||
(defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \
|
||||
(defined(NO_PORTD_PINCHANGES) && defined(NO_PORTB_PINCHANGES)))
|
||||
#undef INLINE_PCINT
|
||||
#define INLINE_PCINT inline
|
||||
#endif
|
||||
|
||||
// Provide drop in compatibility with johnboiles PCInt project at
|
||||
// http://www.arduino.cc/playground/Main/PcInt
|
||||
#define PCdetachInterrupt(pin) PCintPort::detachInterrupt(pin)
|
||||
#define PCattachInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, userFunc,mode)
|
||||
|
||||
|
||||
|
||||
typedef void (*PCIntvoidFuncPtr)(void);
|
||||
|
||||
class PCintPort {
|
||||
PCintPort(int index,volatile uint8_t& maskReg) :
|
||||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||
portInputReg(*portInputRegister((index == 0)?(2):(index + 9))),
|
||||
|
||||
#else
|
||||
portInputReg(*portInputRegister(index + 2)),
|
||||
#endif
|
||||
|
||||
pcmask(maskReg),
|
||||
PCICRbit(1 << index),
|
||||
PCintLast(0) {
|
||||
for (int i = 0; i < 9; i++) {
|
||||
pcIntPins[i] = NULL;
|
||||
}
|
||||
}
|
||||
public:
|
||||
static void attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode);
|
||||
static void detachInterrupt(uint8_t pin);
|
||||
INLINE_PCINT void PCint();
|
||||
static PCintPort pcIntPorts[];
|
||||
|
||||
protected:
|
||||
class PCintPin {
|
||||
public:
|
||||
PCintPin() :
|
||||
PCintFunc((PCIntvoidFuncPtr)NULL),
|
||||
PCintMode(0) {}
|
||||
PCIntvoidFuncPtr PCintFunc;
|
||||
uint8_t PCintMode;
|
||||
uint8_t PCIntMask;
|
||||
static PCintPin pinDataAlloc[MAX_PIN_CHANGE_PINS];
|
||||
};
|
||||
void addPin(uint8_t mode,uint8_t mask,PCIntvoidFuncPtr userFunc);
|
||||
void delPin(uint8_t mask);
|
||||
volatile uint8_t& portInputReg;
|
||||
volatile uint8_t& pcmask;
|
||||
const uint8_t PCICRbit;
|
||||
uint8_t PCintLast;
|
||||
PCintPin* pcIntPins[9]; // extra entry is a barrier
|
||||
};
|
||||
#endif
|
||||
@ -0,0 +1,20 @@
|
||||
|
||||
// This file is used to separate the changes you make to personalize the
|
||||
// Pin Change Interrupt library from any future changes to the library itself.
|
||||
// Ideally it would reside in the folder of the current sketch, but I have not
|
||||
// figured out how such a file can be included from a library.
|
||||
// Nothing is required to be defined within this file since default values are
|
||||
// defined in the primary PinChangeInt.h file.
|
||||
|
||||
// Uncomment the line below to limit the handler to servicing a single interrupt per invocation.
|
||||
//#define DISABLE_PCINT_MULTI_SERVICE
|
||||
|
||||
// Define the value MAX_PIN_CHANGE_PINS to limit the number of pins that may be
|
||||
// used for pin change interrupts. This value determines the number of pin change
|
||||
// interrupts supported for all ports.
|
||||
//#define MAX_PIN_CHANGE_PINS 2
|
||||
|
||||
// declare ports without pin change interrupts used
|
||||
//#define NO_PORTB_PINCHANGES
|
||||
//#define NO_PORTC_PINCHANGES
|
||||
//#define NO_PORTD_PINCHANGES
|
||||
46
src/dcaitirobot/embedded/lib/README
Normal file
46
src/dcaitirobot/embedded/lib/README
Normal file
@ -0,0 +1,46 @@
|
||||
|
||||
This directory is intended for project specific (private) libraries.
|
||||
PlatformIO will compile them to static libraries and link into executable file.
|
||||
|
||||
The source code of each library should be placed in a an own separate directory
|
||||
("lib/your_library_name/[here are source files]").
|
||||
|
||||
For example, see a structure of the following two libraries `Foo` and `Bar`:
|
||||
|
||||
|--lib
|
||||
| |
|
||||
| |--Bar
|
||||
| | |--docs
|
||||
| | |--examples
|
||||
| | |--src
|
||||
| | |- Bar.c
|
||||
| | |- Bar.h
|
||||
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||
| |
|
||||
| |--Foo
|
||||
| | |- Foo.c
|
||||
| | |- Foo.h
|
||||
| |
|
||||
| |- README --> THIS FILE
|
||||
|
|
||||
|- platformio.ini
|
||||
|--src
|
||||
|- main.c
|
||||
|
||||
and a contents of `src/main.c`:
|
||||
```
|
||||
#include <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
int main (void)
|
||||
{
|
||||
...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
PlatformIO Library Dependency Finder will find automatically dependent
|
||||
libraries scanning project source files.
|
||||
|
||||
More information about PlatformIO Library Dependency Finder
|
||||
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||
@ -13,5 +13,3 @@ platform = atmelavr
|
||||
board = diecimilaatmega328
|
||||
framework = arduino
|
||||
monitor_speed = 115200
|
||||
lib_deps =
|
||||
arduino-libraries
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
#include "communication/UValue.hpp"
|
||||
#include "misc/Utils.hpp"
|
||||
#include "constants.h"
|
||||
#include "MotorWheel.h"
|
||||
|
||||
uint8_t UARTCom::data[MAX_PACKET_LENGTH];
|
||||
uint8_t currentParseIndex;
|
||||
@ -170,7 +171,7 @@ void handleRequest (uint8_t* d)
|
||||
break;
|
||||
}
|
||||
case DRIVE_ENCODER: {
|
||||
UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
|
||||
UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition());
|
||||
break;
|
||||
}
|
||||
case PID_PARAMETER: {
|
||||
|
||||
@ -1,5 +1,7 @@
|
||||
#include "communication/UCommands.hpp"
|
||||
#include "misc/Utils.hpp"
|
||||
#include "MotorWheel.h"
|
||||
|
||||
extern MotorWheel wheelLeft;
|
||||
extern MotorWheel wheelRight;
|
||||
|
||||
|
||||
@ -19,20 +19,24 @@ void UValue::sendEncoderValue (U_Component cp, float value)
|
||||
UARTCom::send(VALUE, 4, cp, switched_b);
|
||||
}
|
||||
|
||||
void UValue::sendBothEncoderValues (float value_right, float value_left)
|
||||
void UValue::sendBothEncoderValues (float speed_right, float speed_left, float pos_right, float pos_left)
|
||||
{
|
||||
union {
|
||||
float f[2];
|
||||
uint8_t b[8];
|
||||
float f[4];
|
||||
uint8_t b[16];
|
||||
} u;
|
||||
u.f[0] = value_right;
|
||||
u.f[1] = value_left;
|
||||
u.f[0] = speed_right;
|
||||
u.f[1] = speed_left;
|
||||
u.f[2] = pos_right;
|
||||
u.f[3] = pos_left;
|
||||
|
||||
uint8_t switched_byte_array[8];
|
||||
uint8_t switched_byte_array[16];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
switched_byte_array[i] = u.b[3 - i];
|
||||
switched_byte_array[i+4] = u.b[7 - i];
|
||||
switched_byte_array[i+8] = u.b[11 - i];
|
||||
switched_byte_array[i+12] = u.b[15 - i];
|
||||
}
|
||||
|
||||
UARTCom::send(VALUE, 8, ALL_MOTORS, switched_byte_array);
|
||||
UARTCom::send(VALUE, 16, ALL_MOTORS, switched_byte_array);
|
||||
}
|
||||
14
src/dcaitirobot/launch/frames.gv
Normal file
14
src/dcaitirobot/launch/frames.gv
Normal file
@ -0,0 +1,14 @@
|
||||
digraph G {
|
||||
"base_link" -> "velodyne"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "base_footprint"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "camera_link"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"base_link" -> "chassis"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"camera_link" -> "camera_link_optical"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "caster_wheel_back"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "caster_wheel_front"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
"chassis" -> "laser_frame"[label=" Broadcaster: default_authority\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
|
||||
edge [style=invis];
|
||||
subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
|
||||
"Recorded at time: 1688907915.1191485"[ shape=plaintext ] ;
|
||||
}->"base_link";
|
||||
}
|
||||
BIN
src/dcaitirobot/launch/frames.pdf
Normal file
BIN
src/dcaitirobot/launch/frames.pdf
Normal file
Binary file not shown.
50
src/dcaitirobot/launch/launch_slam.py
Normal file
50
src/dcaitirobot/launch/launch_slam.py
Normal file
@ -0,0 +1,50 @@
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
slam_params = os.path.join(
|
||||
str(Path(__file__).absolute().parent.parent), # <-- Replace with your package name
|
||||
'config',
|
||||
'slam.yml'
|
||||
)
|
||||
|
||||
slam_toolbox = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('slam_toolbox'),'launch','online_async_launch.py'
|
||||
)]),
|
||||
launch_arguments={
|
||||
'use_sim_time': 'true',
|
||||
'slam_params_file': slam_params}.items()
|
||||
)
|
||||
|
||||
node_fix_lidar = Node(
|
||||
package="dcaitirobot",
|
||||
executable="fixlidar",
|
||||
name="fixlidar",
|
||||
)
|
||||
|
||||
node_scale_odom = Node(
|
||||
package="dcaitirobot",
|
||||
executable="scaleodom",
|
||||
name="scaleodom",
|
||||
)
|
||||
|
||||
tfs1 = Node(package = "tf2_ros",
|
||||
executable = "static_transform_publisher",
|
||||
arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "velodyne"])
|
||||
|
||||
tfs2 = Node(package = "tf2_ros",
|
||||
executable = "static_transform_publisher",
|
||||
arguments = ["0", "0", "0" ,"0", "0", "0", "base_link", "base_footprint"])
|
||||
|
||||
|
||||
return LaunchDescription([
|
||||
slam_toolbox, tfs1, tfs2, node_fix_lidar, node_scale_odom
|
||||
])
|
||||
@ -28,17 +28,17 @@ def generate_launch_description():
|
||||
)
|
||||
)
|
||||
),
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
str(
|
||||
(
|
||||
Path(get_package_share_directory("slam_toolbox")).absolute()
|
||||
/ "launch"
|
||||
/ "online_async_launch.py"
|
||||
)
|
||||
)
|
||||
),
|
||||
launch_arguments=[("use_sim_time", "True")],
|
||||
),
|
||||
# IncludeLaunchDescription(
|
||||
# PythonLaunchDescriptionSource(
|
||||
# str(
|
||||
# (
|
||||
# Path(get_package_share_directory("slam_toolbox")).absolute()
|
||||
# / "launch"
|
||||
# / "online_async_launch.py"
|
||||
# )
|
||||
# )
|
||||
# ),
|
||||
# launch_arguments=[("use_sim_time", "True")],
|
||||
# ),
|
||||
]
|
||||
)
|
||||
|
||||
@ -12,7 +12,8 @@ setup(
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name), glob('launch/*.launch.py'))
|
||||
(os.path.join('share', package_name), glob('launch/*.py')),
|
||||
(os.path.join('share', 'config'), glob('config/*.yml')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
@ -27,6 +28,8 @@ setup(
|
||||
'twistcalc = dcaitirobot.twist:main',
|
||||
'serial_comms = dcaitirobot.serial_comms:main',
|
||||
'joy = dcaitirobot.joy:main',
|
||||
"fixlidar = dcaitirobot.handle_nan_scan:main",
|
||||
"scaleodom = dcaitirobot.scale_odom:main"
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@ -34,11 +34,11 @@ public:
|
||||
|
||||
void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms);
|
||||
void sendEmptyMsg();
|
||||
void readEncoderValues(double &val_1, double &val_2);
|
||||
void readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
|
||||
void setMotorValues(float val_1, float val_2);
|
||||
void setPidValues(float k_p, float k_d, float k_i, float k_o);
|
||||
const std::vector<uint8_t> buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector<uint8_t> payload);
|
||||
void readSpeed(double &left_speed, double &right_speed);
|
||||
void readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
|
||||
|
||||
bool connected() const { return serial_conn_.isOpen(); }
|
||||
|
||||
|
||||
@ -20,12 +20,12 @@ void ArduinoComms::sendEmptyMsg()
|
||||
{
|
||||
}
|
||||
|
||||
void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
|
||||
void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
|
||||
{
|
||||
std::vector<uint8_t> payload = {DRIVE_ENCODER};
|
||||
std::vector<uint8_t> msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload);
|
||||
sendMsg(msg);
|
||||
readSpeed(left_speed, right_speed);
|
||||
readSpeedandPosition(left_speed, right_speed, left_pos, right_pos);
|
||||
}
|
||||
|
||||
void ArduinoComms::setMotorValues(float left_speed, float right_speed)
|
||||
@ -48,7 +48,7 @@ void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o)
|
||||
ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r";
|
||||
}
|
||||
|
||||
void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
|
||||
void ArduinoComms::readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
|
||||
{
|
||||
std::vector<uint8_t> buffer;
|
||||
int read_bytes = 0;
|
||||
@ -90,13 +90,21 @@ void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
|
||||
if (frame_type == VALUE && frame_component == BOTH_MOTORS)
|
||||
{
|
||||
std::vector<uint8_t> left_speed_bytes(payload.begin(), payload.begin() + 4);
|
||||
std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.end());
|
||||
std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.begin()+8);
|
||||
std::vector<uint8_t> left_pos_bytes(payload.begin() + 8, payload.begin()+12);
|
||||
std::vector<uint8_t> right_pos_bytes(payload.begin() + 12, payload.end());
|
||||
std::reverse(left_speed_bytes.begin(), left_speed_bytes.end());
|
||||
std::reverse(right_speed_bytes.begin(), right_speed_bytes.end());
|
||||
std::reverse(left_pos_bytes.begin(), left_pos_bytes.end());
|
||||
std::reverse(right_pos_bytes.begin(), right_pos_bytes.end());
|
||||
float left_speed_float = *reinterpret_cast<float*>(&left_speed_bytes[0]);
|
||||
float right_speed_float = *reinterpret_cast<float*>(&right_speed_bytes[0]);
|
||||
float left_pos_float = *reinterpret_cast<float*>(&left_pos_bytes[0]);
|
||||
float right_pos_float = *reinterpret_cast<float*>(&right_pos_bytes[0]);
|
||||
left_speed = left_speed_float;
|
||||
right_speed = right_speed_float;
|
||||
left_pos = left_pos_float;
|
||||
right_pos = right_pos_float;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -7,6 +7,11 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include <math.h>
|
||||
|
||||
#define RADSTORPM(a)(a*60.0/(2.0*M_PI))
|
||||
#define RPMSTORADS(a)(a*2.0*M_PI/60.0)
|
||||
#define REVSTORAD(a)(a*2.0*M_PI)
|
||||
#define RADSTOREV(a)(a/(2.0*M_PI))
|
||||
|
||||
namespace diffdrive_arduino
|
||||
{
|
||||
|
||||
@ -106,18 +111,15 @@ return_type DiffDriveArduino::read()
|
||||
return return_type::ERROR;
|
||||
}
|
||||
|
||||
arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel);
|
||||
// write l_wheel_.cmd to string and print via RCLCPP_INFO
|
||||
std::string msg = "Left Wheel Read: " + std::to_string(l_wheel_.vel);
|
||||
RCLCPP_INFO(logger_, msg.c_str());
|
||||
arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
|
||||
// convert rpm to rad/s
|
||||
l_wheel_.vel = (l_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
|
||||
r_wheel_.vel = (r_wheel_.vel / 60) * 2 * M_PI * 0.049 ;
|
||||
l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
|
||||
r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
|
||||
|
||||
|
||||
// Force the wheel position
|
||||
l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
|
||||
r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
|
||||
// convert rev to rad
|
||||
l_wheel_.pos = REVSTORAD(l_wheel_.pos);
|
||||
r_wheel_.pos = REVSTORAD(r_wheel_.pos);
|
||||
return return_type::OK;
|
||||
}
|
||||
|
||||
@ -128,9 +130,7 @@ return_type DiffDriveArduino::write()
|
||||
return return_type::ERROR;
|
||||
}
|
||||
// convert rad/s to rpm and write to motors
|
||||
arduino_.setMotorValues(l_wheel_.cmd * 30 / M_PI , r_wheel_.cmd * 30 / M_PI );
|
||||
std::string msg = "Left Wheel Command: " + std::to_string(l_wheel_.cmd* 30 / M_PI);
|
||||
RCLCPP_INFO(logger_, msg.c_str());
|
||||
arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd));
|
||||
|
||||
return return_type::OK;
|
||||
}
|
||||
|
||||
1
src/xacro
Submodule
1
src/xacro
Submodule
Submodule src/xacro added at 344e25b49f
Reference in New Issue
Block a user