i2cpwm_board  0.5.1
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
Functions
Services interfaces provided by this package

Functions

bool set_pwm_frequency (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)
 service to set set PWM frequency More...
 
bool config_servos (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)
 store configuration data for servos on the active board More...
 
bool config_drive_mode (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)
 set drive mode and drive servos More...
 
bool stop_servos (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
 service to stop all servos on all boards More...
 

Detailed Description

Function Documentation

bool set_pwm_frequency ( i2cpwm_board::IntValue::Request &  req,
i2cpwm_board::IntValue::Response &  res 
)

#include <src/i2cpwm_controller.cpp>

service to set set PWM frequency

The PWM boards drive LED and servos using pulse width modulation. The 12 bit interface means values are 0..4096. The size of the minimum width is determined by the frequency. is service is needed when using a board configured other than with the default I2C address and when using multiple boards.

If using the set_active_board() service, it must be used before using other services or topics from this package.

Warning: Changing the frequency will affect any active servos.

Parameters
[in]reqan Int16 value for the requested pulse frequency
[out]resthe return value will be the new active frequency
Returns
true

i2cpwm_board::IntValue

# there are a few services whic take a single integer as input
# these services share the IntValue service definition
int16 value
---
int16 error

Example

1 # Analog RC servos are most often designed for 20ms pulses. This is achieved with a frequency of 50Hz.
2 # This software defaults to 50Hz. Use the set_pwm_frequncy() to change this frequency value.
3 # It may be necessary or convenient to change the PWM frequency if using DC motors connected to PWM controllers.
4 # It may also be convenient if using PWM to control LEDs.
5 
6 rosservice call /set_pwm_frequency "{value: 50}"

Definition at line 1172 of file i2cpwm_controller.cpp.

bool config_servos ( i2cpwm_board::ServosConfig::Request &  req,
i2cpwm_board::ServosConfig::Response &  res 
)

#include <src/i2cpwm_controller.cpp>

store configuration data for servos on the active board

A service to set each servo's center value, direction of rotation (1 for forward and -1 for reverse motion), the center or nul value, range, and direction of one or more servos. and range between full left and right or maximun forward and backward speed.

Setting these data are required before sending messages to the servos_proportional() topic as well as the servos_drive() topic.

If more than one PWM board is present, the set_active_board() service is used to switch between boards prior to configuring servos.

Parameters
[in]reqan array of 'ServoConfig' which consists of a servo number (one based), center(0..4096), range(0..4096), and direction (±1).
[out]resinteger non-zero if an error occured
Returns
true

i2cpwm_board::ServosConfig

# the servos_config service is used to assign useful configuration data to servos
# the tollerance of electronis varies in RC servos so it is important to calibate
# each servo, indicating its PWM value for direction of rotation, centering, and
# range which is used to scale servo motion a standard ±1000 scale
ServoConfig[] servos
---
int16 error

i2cpwm_board::ServoConfig

# the ServoConfig message is used to assign specific configuration
# data to a servo. the data is needed to normalize servos to
# common values to isolate variations from the rest of the user's
# robot motion code.
int16 servo
int16 center
int16 range
int16 direction

Example

1 # this example makes the following assumptions about the hardware
2 # the servos have been connected to the PWM board starting with the first connector and proceeding in order
3 # any drive servos used for the left side servos are mounted opposite of those for the right side
4 
5 # this example uses 2 servos
6 # the first servo is the left and the second servo is the right
7 
8 # configure two continuous rotation servos associated with the drive system - these servos were determined to have a ragee of ±50
9 
10 rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \
11  {servo: 2, center: 336, range: 108, direction: 1}]"
12 
13 # additionally configure one 180 degree servo (±90) used for a robot arm - this servo was determine to have a ragee of ±188
14 
15 rosservice call /config_servos "servos: [{servo: 9, center: 344, range: 376, direction: -1}]"

Definition at line 1228 of file i2cpwm_controller.cpp.

bool config_drive_mode ( i2cpwm_board::DriveMode::Request &  req,
i2cpwm_board::DriveMode::Response &  res 
)

#include <src/i2cpwm_controller.cpp>

set drive mode and drive servos

A service to set the desired drive mode. It must be called before messages are handled by the servos_drive() topic. Setting these data are required before sending messages to the servos_proportional() topic.

The drive mode consists of a string value for the type of drive desired: ackerman, differential, or mecanum. For each mode, the drive servos must be specified.

Parameters
req[in] DriveMode configuration data
res[out] non-zero on error
Returns
true

The DriveMode input requires drive system details included: wheel RPM, wheel radius, and track width. ROS uses meters for measurements. The values of radius and track are expected in meters.

A scale factor is available if necessary to compensate for linear vector values.

The mode string is one of the following drive systems:

  1. 'ackerman' - (automobile steering) requires minimum of one servo for drive and uses some other servo for stearing..
  2. 'differential' - requires multiples of two servos, designated as left and right.
  3. 'mecanum' - requires multiples of four servos, designated as left-front, right-front, left-rear, and right-rear.

The servo message is used for indicating which servos are used for the drive system. The message consists of 'servo' number, and data 'value'.

The 'value' field indicates the positon the corresponding servo within the drive system. The applicable servos are assigned positions as follows:

positon ackerman differential mecanum
position 1 corresponds to drive left left-front
position 2 corresponds to right right-front
position 3 corresponds to left-rear
position 4 corresponds to right-rear

i2cpwm_board::DriveMode

# the drive_mode service is used to assigned servos to various drive modes
# the drive modes determine how the assigned servos respond to geometry_msgs::Twist messages
# drive modes are one of: ackerman, differential, or mecanum
# to accurately convert velocity in m/s the controller needs to know three values:
# the RPM - the maximum output speed available from the drive motors
# the radius - the drive wheel radius in meters
# the track - the distance between the left and right wheels in meters
# use the scale value to adjust incoming Twist values as needed to match the servo/motor capability
string mode
float32 rpm
float32 radius
float32 track
float32 scale
Position[] servos
---
int16 error

i2cpwm_board::Position

# the position message is used when configuring drive mode to
# assign a servo to a specific wheel positon in the drive system
# postions are specific toe the desired drive mode
# ackerman has only one position
# 1 = drive
# differential has two positons
# 1 = left, 2 = right
# mecanum has four positions
# 1 = front left, 2 = front right, 3 = rear left, 4 = rear right
# multiple servos/motors may be used for each positon
int16 servo
int16 position

Example

1 # this example makes the following assumptions about the hardware
2 # the servos have been connected to the PWM board starting with the first connector and proceeding in order
3 # any drive servos used for the left side servos are mounted opposite of those for the right side
4 
5 # ROS used m/s (meters per second) as th standard unit for velocity.
6 # The geometry_msgs::Twist linear and angular vectors are in m/s and radians/s respectively.
7 
8 # differential drive example
9 # this example uses 2 servos
10 # the first servo is the left and the second servo is the right
11 
12 rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \
13  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}]}"
14 
15 # this example uses 4 servos
16 # there are two servos for the left and two fors the right
17 
18 rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \
19  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}, \
20  {servo: 3, value: 1}, {servo: 4, value: 2}]}"
21 
22 # mecanum drive example
23 # this example uses 4 servos
24 
25 rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \
26  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}, \
27  {servo: 3, value: 1}, {servo: 4, value: 2}]}"

Definition at line 1327 of file i2cpwm_controller.cpp.

bool stop_servos ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
)

#include <src/i2cpwm_controller.cpp>

service to stop all servos on all boards

A service to stop all of the servos on all of the PWM boards and set their power state to off / coast.

This is different from setting each servo to its center value. A centered servo is still under power and it's in a brake state.

Parameters
reqis empty
resis empty
Returns
true

Example

1 # stop all servos on all boards and setting them to coast rather than brake
2 
3 rosservice call /stop_servos

Definition at line 1370 of file i2cpwm_controller.cpp.