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

Functions

void servos_absolute (const i2cpwm_board::ServoArray::ConstPtr &msg)
 subscriber topic to move servos in a physical position More...
 
void servos_proportional (const i2cpwm_board::ServoArray::ConstPtr &msg)
 subscriber topic to move servos in the range of ±1.0 More...
 
void servos_drive (const geometry_msgs::Twist::ConstPtr &msg)
 subscriber topic to move servos based on a drive mode More...
 

Detailed Description

Function Documentation

void servos_absolute ( const i2cpwm_board::ServoArray::ConstPtr &  msg)

#include <src/i2cpwm_controller.cpp>

subscriber topic to move servos in a physical position

Subscriber for the servos_absolute topic which processes one or more servos and sets their physical pulse value.

When working with a continuous rotation servo, the topic is used to find the center position of a servo by sending successive values until a stopped position is identified. The topic is also used to find the range of a servo - the fastest forward and fasted reverse values. The difference between these two is the servo's range. Due to variences in servos, each will likely have a slightly different center value.

When working with a fixed 180 degree rotation servo, the topic is used to find the center position of a servo by sending successive values until a the desired middle is identified. The topic is also used to find the range of a servo - the maximum clockwise and anti clockwise positions. The difference between these two is the servo's range. If the servo rotates slightly more in one direction from center than the other, then '2X' the lesser value should be used as the range to preserve the middle stop position.

Parameters
msga 'ServoArray' message (array of one or more 'Servo') where the servo:value is the pulse position/speed

i2cpwm_board::ServoArray

# the ServoArray message is commonly used message in this package to
# handle multiple assignments of values to servos. the purpose of
# the value is dependent on the topic or service being called
Servo[] servos

i2cpwm_board::Servo

# the Servo message is commonly used message in this package to
# assign a value to a specific servo. the purpose of the value is
# dependent on the topic or service being called
int16 servo
float32 value

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 # the follow message sets the PWM value of the first two servos to 250 and 350 respectively.
6 # depending on center value of each servo, these values may casue forward or backward rotation
7 
8 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 250}, {servo: 2, value: 350}]}"
9 
10 # the following messages are an example of finding a continuous servo's center
11 # in this example the center is found to be 333
12 
13 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 300}]}"
14 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 350}]}"
15 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 320}]}"
16 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 330}]}"
17 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 335}]}"
18 rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 333}]}"

Definition at line 797 of file i2cpwm_controller.cpp.

void servos_proportional ( const i2cpwm_board::ServoArray::ConstPtr &  msg)

#include <src/i2cpwm_controller.cpp>

subscriber topic to move servos in the range of ±1.0

Subscriber for controlling servos using proportional values. This topic processes one or more servos and sets their physical pulse value based on each servos physical range proportional to a range of ±1.0.

When working with a continuous rotation servo, the topic is used to adjust the speed of the servo.

When working with a fixed 180 degree rotation servo, the topic is used to adjust the position of the servo.

This topic requires the use of the config_servos() service. Once the configuration of the servos - center position, direction of rotation, and PWM range - has been set, these data are to convert the proportional value to a physical PWM value specific to each servo.

Parameters
msga 'ServoArray' message (array of one or more 'Servo') where the servo:value is a relative position/speed

i2cpwm_board::ServoArray Message

# the ServoArray message is commonly used message in this package to
# handle multiple assignments of values to servos. the purpose of
# the value is dependent on the topic or service being called
Servo[] servos

i2cpwm_board::Servo Message

# the Servo message is commonly used message in this package to
# assign a value to a specific servo. the purpose of the value is
# dependent on the topic or service being called
int16 servo
float32 value

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 # drive both servos forward at 40% of maximum speed
14 
15 rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 0.40}, {servo: 2, value: 0.40}]}"
16 
17 # additionally configure one 180 degree servo (±90) used for a robot arm - this servo was determine to have a ragee of ±188
18 
19 rosservice call /config_servos "servos: [{servo: 9, center: 344, range: 376, direction: -1}]"
20 
21 # drive the arm servo to its 45 degree position and then to its -45 degree position
22 
23 rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 9, value: 0.50}]}"
24 rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 9, value: -0.50}]}"

Definition at line 867 of file i2cpwm_controller.cpp.

void servos_drive ( const geometry_msgs::Twist::ConstPtr &  msg)

#include <src/i2cpwm_controller.cpp>

subscriber topic to move servos based on a drive mode

Subscriber for controlling a group of servos in concert based on a geometry_msgs::Twist message.

This topic requires the use the config_servos() service to configure the servos for proportional control and the use of the config_drive_mode() to speccify the desired type of drive and to assign individual servos to the positions in the drive system.

Parameters
msga geometry Twist message

geometry_msgs::Twist

# This expresses velocity in free space broken into its linear and angular parts.
Vector3 linear
Vector3 angular

geometry_msgs::Vector3

# This represents a vector in free space.
# It is only meant to represent a direction. Therefore, it does not
# make sense to apply a translation to it (e.g., when applying a
# generic rigid transformation to a Vector3, tf2 will only apply the
# rotation). If you want your data to be translatable too, use the
# geometry_msgs/Point message instead.
float64 x
float64 y
float64 z

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 # a typical high-speed servo is capable of 0.16-0.2 sec/60deg or aproximately 50-65 RPM.
9 # using wheel with 11 cm diameter results in a circumference of 0.35 meters
10 # the theoretical maximum speed of this combination is 0.30 m/s - 0.40 m/s
11 
12 
13 # differential drive example
14 # this example uses 2 servos
15 # the first servo is the left and the second servo is the right
16 
17 # configure drive mode for two RC servos attached to 110mm diameter wheels
18 
19 rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \
20  {servo: 2, center: 336, range: 108, direction: 1}]"
21 
22 rosservice call /config_drive_mode "{mode: differential, rpm: 60.0, radius: 5.5, track: 5, scale: 1.0, \
23  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}]}"
24 
25 # moving forward at 0.35 m/s (or best speed)
26 
27 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, 0.0]}"
28 
29 # left 45 degrees per second turn while moving forward at 0.35 m/s (or best speed)
30 
31 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, -0.7854]}"
32 
33 # pivoting clockwise at 90 degrees per second
34 
35 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.0, 0.0], angular: [0.0, 0.0, 1.5708]}"
36 
37 
38 # a mecanum drive example
39 # this example assumes there are 4 servos
40 # the servos are 1, 2, 3, & 4 and correspond to left-front, right-front, left-rear, and right-rear
41 
42 # configure drive mode for four servos
43 
44 rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: 1}, \
45  {servo: 2, center: 336, range: 108, direction: -1}, \
46  {servo: 3, center: 337, range: 102, direction: -1}, \
47  {servo: 4, center: 330, range: 100, direction: -1}]"
48 
49 rosservice call /config_drive_mode "{mode: mecanum, rpm: 60.0, radius: 5.5, track: 5, scale: 1.0, \
50  servos: [{servo: 1, value: 2}, {servo: 2, value: 1}, \
51  {servo: 3, value: 3}, {servo: 4, value: 4}]}"
52 
53 # moving forward at full speed
54 
55 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35.0, 0.0, 0.0], angular: [0.0, 0.0, 0.0]}"
56 
57 # moving forward and to the right while always facing forward
58 
59 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.15, 0.0], angular: [0.0, 0.0, 0.0]}"
60 
61 # right turn while moving forward
62 
63 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, 0.7854]}"
64 
65 # pivoting clockwise
66 
67 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.0, 0.0], angular: [0.0, 0.0, 1.5708]}"
68 
69 # moving sideways to the right
70 
71 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.2, 0.0], angular: [0.0, 0.0, 0.0]}"
72 
73 # moving sideways to the left
74 
75 rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, -0.2, 0.0], angular: [0.0, 0.0, 0.0]}"

Definition at line 976 of file i2cpwm_controller.cpp.