176 #include <sys/ioctl.h> 177 #include <sys/types.h> 178 #include <sys/stat.h> 179 #include <linux/i2c-dev.h> 182 #include <ros/console.h> 185 #include <std_srvs/Empty.h> 187 #include <geometry_msgs/Twist.h> 190 #include "i2cpwm_board/Servo.h" 191 #include "i2cpwm_board/ServoArray.h" 193 #include "i2cpwm_board/ServoConfig.h" 194 #include "i2cpwm_board/ServoConfigArray.h" 196 #include "i2cpwm_board/ServosConfig.h" 198 #include "i2cpwm_board/DriveMode.h" 199 #include "i2cpwm_board/Position.h" 200 #include "i2cpwm_board/PositionArray.h" 202 #include "i2cpwm_board/IntValue.h" 207 typedef struct _servo_config {
214 typedef struct _drive_mode {
225 MODE_DIFFERENTIAL = 2,
230 enum drive_mode_positions {
231 POSITION_UNDEFINED = 0,
232 POSITION_LEFTFRONT = 1,
233 POSITION_RIGHTFRONT = 2,
234 POSITION_LEFTREAR = 3,
235 POSITION_RIGHTREAR = 4,
239 #define _BASE_ADDR 0x40 241 #define _PI 3.14159265358979323846 243 #define _CONST(s) ((char*)(s)) 253 __CHANNEL_ON_L = 0x06,
254 __CHANNEL_ON_H = 0x07,
255 __CHANNEL_OFF_L = 0x08,
256 __CHANNEL_OFF_H = 0x09,
257 __ALL_CHANNELS_ON_L = 0xFA,
258 __ALL_CHANNELS_ON_H = 0xFB,
259 __ALL_CHANNELS_OFF_L = 0xFC,
260 __ALL_CHANNELS_OFF_H = 0xFD,
268 #define MAX_BOARDS 62 269 #define MAX_SERVOS (16*MAX_BOARDS) 271 servo_config _servo_configs[MAX_SERVOS];
272 drive_mode _active_drive;
273 int _last_servo = -1;
275 int _pwm_boards[MAX_BOARDS];
276 int _active_board = 0;
277 int _controller_io_handle;
279 int _pwm_frequency = 50;
290 static float _abs (
float v1)
297 static float _min (
float v1,
float v2)
304 static float _max (
float v1,
float v2)
311 static float _absmin (
float v1,
float v2)
324 static float _absmax (
float v1,
float v2)
349 static int _smoothing (
float speed)
363 static float _convert_mps_to_proportional (
float speed)
367 float initial, max_rate;
371 if (_active_drive.rpm <= 0.0) {
372 ROS_ERROR(
"Invalid active drive mode RPM %6.4f :: RPM must be greater than 0", _active_drive.rpm);
375 if (_active_drive.radius <= 0.0) {
376 ROS_ERROR(
"Invalid active drive mode radius %6.4f :: wheel radius must be greater than 0", _active_drive.radius);
380 max_rate = (_active_drive.radius * _PI * 2) * (_active_drive.rpm / 60.0);
382 speed = speed / max_rate;
385 ROS_DEBUG(
"%6.4f = convert_mps_to_proportional ( speed(%6.4f) / ((radus(%6.4f) * pi(%6.4f) * 2) * (rpm(%6.4f) / 60.0)) )", speed, initial, _active_drive.radius, _PI, _active_drive.rpm);
398 static void _set_pwm_frequency (
int freq)
401 char oldmode, newmode;
404 _pwm_frequency = freq;
406 ROS_DEBUG(
"_set_pwm_frequency prescale");
407 float prescaleval = 25000000.0;
408 prescaleval /= 4096.0;
409 prescaleval /= (float)freq;
412 prescale = floor(prescaleval + 0.5);
416 ROS_INFO(
"Setting PWM frequency to %d Hz", freq);
418 nanosleep ((
const struct timespec[]){{1, 000000L}}, NULL);
421 oldmode = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);
422 newmode = (oldmode & 0x7F) | 0x10;
424 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, newmode))
425 ROS_ERROR(
"Unable to set PWM controller to sleep mode");
427 if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __PRESCALE, (
int)(floor(prescale))))
428 ROS_ERROR(
"Unable to set PWM controller prescale");
430 if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode))
431 ROS_ERROR(
"Unable to set PWM controller to active mode");
433 nanosleep((
const struct timespec[]){{0, 5000000L}}, NULL);
435 if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode | 0x80))
436 ROS_ERROR(
"Unable to restore PWM controller to active mode");
449 static void _set_pwm_interval_all (
int start,
int end)
452 if ((_active_board<1) || (_active_board>62)) {
453 ROS_ERROR(
"Internal error - invalid active board number %d :: PWM board numbers must be between 1 and 62", _active_board);
456 int board = _active_board - 1;
458 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_ON_L, start & 0xFF))
459 ROS_ERROR (
"Error setting PWM start low byte for all servos on board %d", _active_board);
460 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_ON_H, start >> 8))
461 ROS_ERROR (
"Error setting PWM start high byte for all servos on board %d", _active_board);
462 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_OFF_L, end & 0xFF))
463 ROS_ERROR (
"Error setting PWM end low byte for all servos on board %d", _active_board);
464 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_OFF_H, end >> 8))
465 ROS_ERROR (
"Error setting PWM end high byte for all servos on board %d", _active_board);
476 static void _set_active_board (
int board)
480 if ((board<1) || (board>62)) {
481 ROS_ERROR(
"Invalid board number %d :: board numbers must be between 1 and 62", board);
484 if (_active_board != board) {
485 _active_board = board;
490 if (0 > ioctl (_controller_io_handle, I2C_SLAVE, (_BASE_ADDR+(board)))) {
491 ROS_FATAL (
"Failed to acquire bus access and/or talk to I2C slave at address 0x%02X", (_BASE_ADDR+board));
495 if (_pwm_boards[board]<0) {
496 _pwm_boards[board] = 1;
500 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE2, __OUTDRV))
501 ROS_ERROR (
"Failed to enable PWM outputs for totem-pole structure");
503 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, __ALLCALL))
504 ROS_ERROR (
"Failed to enable ALLCALL for PWM channels");
506 nanosleep ((
const struct timespec[]){{0, 5000000L}}, NULL);
509 mode1res = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);
510 mode1res = mode1res & ~__SLEEP;
512 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, mode1res))
513 ROS_ERROR (
"Failed to recover from low power mode");
515 nanosleep((
const struct timespec[]){{0, 5000000L}}, NULL);
518 _set_pwm_interval_all (0, 0);
534 static void _set_pwm_interval (
int servo,
int start,
int end)
536 ROS_DEBUG(
"_set_pwm_interval enter");
538 if ((servo<1) || (servo>(MAX_SERVOS))) {
539 ROS_ERROR(
"Invalid servo number %d :: servo numbers must be between 1 and %d", servo, MAX_BOARDS);
543 int board = ((int)(servo/16))+1;
544 _set_active_board(board);
548 board = _active_board - 1;
550 ROS_DEBUG(
"_set_pwm_interval board=%d", board);
553 int channel = servo - 1;
555 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_ON_L+4*channel, start & 0xFF))
556 ROS_ERROR (
"Error setting PWM start low byte on servo %d on board %d", servo, _active_board);
557 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_ON_H+4*channel, start >> 8))
558 ROS_ERROR (
"Error setting PWM start high byte on servo %d on board %d", servo, _active_board);
559 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_OFF_L+4*channel, end & 0xFF))
560 ROS_ERROR (
"Error setting PWM end low byte on servo %d on board %d", servo, _active_board);
561 if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_OFF_H+4*channel, end >> 8))
562 ROS_ERROR (
"Error setting PWM end high byte on servo %d on board %d", servo, _active_board);
575 static void _set_pwm_interval_proportional (
int servo,
float value)
577 if ((value < -1.0) || (value > 1.0)) {
578 ROS_ERROR(
"Invalid proportion value %f :: proportion values must be between -1.0 and 1.0", value);
582 servo_config* configp = &(_servo_configs[servo-1]);
584 if ((configp->center < 0) ||(configp->range < 0)) {
585 ROS_ERROR(
"Missing servo configuration for servo[%d]", servo);
589 int pos = (configp->direction * (((float)(configp->range) / 2) * value)) + configp->center;
591 if ((pos < 0) || (pos > 4096)) {
592 ROS_ERROR(
"Invalid computed position servo[%d] = (direction(%d) * ((range(%d) / 2) * value(%6.4f))) + %d = %d", servo, configp->direction, configp->range, value, configp->center, pos);
595 _set_pwm_interval (servo, 0, pos);
596 ROS_DEBUG(
"servo[%d] = (direction(%d) * ((range(%d) / 2) * value(%6.4f))) + %d = %d", servo, configp->direction, configp->range, value, configp->center, pos);
610 static void _config_servo (
int servo,
int center,
int range,
int direction)
612 if ((servo < 1) || (servo > (MAX_SERVOS))) {
613 ROS_ERROR(
"Invalid servo number %d :: servo numbers must be between 1 and %d", servo, MAX_SERVOS);
617 if ((center < 0) || (center > 4096))
618 ROS_ERROR(
"Invalid center value %d :: center values must be between 0 and 4096", center);
620 if ((center < 0) || (center > 4096))
621 ROS_ERROR(
"Invalid range value %d :: range values must be between 0 and 4096", range);
623 if (((center - (range/2)) < 0) || (((range/2) + center) > 4096))
624 ROS_ERROR(
"Invalid range center combination %d ± %d :: range/2 ± center must be between 0 and 4096", center, (range/2));
626 _servo_configs[servo-1].center = center;
627 _servo_configs[servo-1].range = range;
628 _servo_configs[servo-1].direction = direction;
629 _servo_configs[servo-1].mode_pos = POSITION_UNDEFINED;
631 if (servo > _last_servo)
634 ROS_INFO(
"Servo #%d configured: center=%d, range=%d, direction=%d", servo, center, range, direction);
638 static int _config_servo_position (
int servo,
int position)
640 if ((servo < 1) || (servo > (MAX_SERVOS))) {
641 ROS_ERROR(
"Invalid servo number %d :: servo numbers must be between 1 and %d", servo, MAX_SERVOS);
644 if ((position < POSITION_UNDEFINED) || (position > POSITION_RIGHTREAR)) {
645 ROS_ERROR(
"Invalid drive mode position %d :: positions are 0 = non-drive, 1 = left front, 2 = right front, 3 = left rear, and 4 = right rear", position);
648 _servo_configs[servo-1].mode_pos = position;
649 ROS_INFO(
"Servo #%d configured: position=%d", servo, position);
654 static int _config_drive_mode (std::string mode,
float rpm,
float radius,
float track,
float scale)
656 int mode_val = MODE_UNDEFINED;
659 if (0 == strcmp (mode.c_str(), _CONST(
"ackerman")))
660 mode_val = MODE_ACKERMAN;
661 else if (0 == strcmp (mode.c_str(), _CONST(
"differential")))
662 mode_val = MODE_DIFFERENTIAL;
663 else if (0 == strcmp (mode.c_str(), _CONST(
"mecanum")))
664 mode_val = MODE_MECANUM;
666 mode_val = MODE_INVALID;
667 ROS_ERROR(
"Invalid drive mode %s :: drive mode must be one of ackerman, differential, or mecanum", mode.c_str());
672 ROS_ERROR(
"Invalid RPM %6.4f :: the motor's output RPM must be greater than 0.0", rpm);
677 ROS_ERROR(
"Invalid radius %6.4f :: the wheel radius must be greater than 0.0 meters", radius);
682 ROS_ERROR(
"Invalid track %6.4f :: the axel track must be greater than 0.0 meters", track);
687 ROS_ERROR(
"Invalid scale %6.4f :: the scalar for Twist messages must be greater than 0.0", scale);
691 _active_drive.mode = mode_val;
692 _active_drive.rpm = rpm;
693 _active_drive.radius = radius;
694 _active_drive.track = track;
695 _active_drive.scale = scale;
697 ROS_INFO(
"Drive mode configured: mode=%s, rpm=%6.4f, radius=%6.4f, track=%6.4f, scale=%6.4f", mode.c_str(), rpm, radius, track, scale);
710 static void _init (
char* filename)
718 for (i=0; i<MAX_BOARDS;i++)
722 for (i=0; i<(MAX_SERVOS);i++) {
724 _servo_configs[i].center = -1;
725 _servo_configs[i].range = -1;
726 _servo_configs[i].direction = 1;
727 _servo_configs[i].mode_pos = -1;
731 _active_drive.mode = MODE_UNDEFINED;
732 _active_drive.rpm = -1.0;
733 _active_drive.radius = -1.0;
734 _active_drive.track = -1.0;
735 _active_drive.scale = -1.0;
738 if ((_controller_io_handle = open (filename, O_RDWR)) < 0) {
739 ROS_FATAL (
"Failed to open I2C bus %s", filename);
801 for(std::vector<i2cpwm_board::Servo>::const_iterator sp = msg->servos.begin(); sp != msg->servos.end(); ++sp) {
802 int servo = sp->servo;
803 int value = sp->value;
805 if ((value < 0) || (value > 4096)) {
806 ROS_ERROR(
"Invalid PWM value %d :: PWM values must be between 0 and 4096", value);
809 _set_pwm_interval (servo, 0, value);
810 ROS_DEBUG(
"servo[%d] = %d", servo, value);
871 for(std::vector<i2cpwm_board::Servo>::const_iterator sp = msg->servos.begin(); sp != msg->servos.end(); ++sp) {
872 int servo = sp->servo;
873 float value = sp->value;
874 _set_pwm_interval_proportional (servo, value);
981 float delta, range, ratio;
982 float temp_x, temp_y, temp_r;
983 float dir_x, dir_y, dir_r;
989 ROS_DEBUG(
"servos_drive Twist = [%5.2f %5.2f %5.2f] [%5.2f %5.2f %5.2f]",
990 msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);
992 if (_active_drive.mode == MODE_UNDEFINED) {
993 ROS_ERROR(
"drive mode not set");
996 if ((_active_drive.mode < MODE_UNDEFINED) || (_active_drive.mode >= MODE_INVALID)) {
997 ROS_ERROR(
"unrecognized drive mode set %d", _active_drive.mode);
1001 dir_x = ((msg->linear.x < 0) ? -1 : 1);
1002 dir_y = ((msg->linear.y < 0) ? -1 : 1);
1003 dir_r = ((msg->angular.z < 0) ? -1 : 1);
1005 temp_x = _active_drive.scale * _abs(msg->linear.x);
1006 temp_y = _active_drive.scale * _abs(msg->linear.y);
1007 temp_r = _abs(msg->angular.z);
1014 delta = (temp_r * (_active_drive.track / 2));
1016 ratio = _convert_mps_to_proportional(temp_y + delta);
1021 switch (_active_drive.mode) {
1028 speed[0] = temp_x * dir_x;
1029 speed[0] = _convert_mps_to_proportional(speed[0]);
1030 if (_abs(speed[0]) > 1.0)
1031 speed[0] = 1.0 * dir_x;
1033 ROS_DEBUG(
"ackerman drive mode speed=%6.4f", speed[0]);
1036 case MODE_DIFFERENTIAL:
1047 speed[0] = (temp_y + delta) * dir_y;
1048 speed[1] = (temp_y - delta) * dir_y;
1050 speed[0] = (temp_y - delta) * dir_y;
1051 speed[1] = (temp_y + delta) * dir_y;
1054 ROS_DEBUG(
"computed differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
1057 range = _max (_abs(speed[0]), _abs(speed[1]));
1059 ratio = _convert_mps_to_proportional(range);
1064 ROS_DEBUG(
"adjusted differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
1066 speed[0] = _convert_mps_to_proportional(speed[0]);
1067 speed[1] = _convert_mps_to_proportional(speed[1]);
1069 ROS_DEBUG(
"differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
1080 speed[0] = speed[2] = (temp_y + delta) * dir_y;
1081 speed[1] = speed[3] = (temp_y - delta) * dir_y;
1083 speed[0] = speed[2] = (temp_y - delta) * dir_y;
1084 speed[1] = speed[3] = (temp_y + delta) * dir_y;
1087 speed[0] += temp_x * dir_x;
1088 speed[3] += temp_x * dir_x;
1089 speed[1] -= temp_x * dir_x;
1090 speed[2] -= temp_x * dir_x;
1091 ROS_DEBUG(
"computed mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f", speed[0], speed[1], speed[2], speed[3]);
1093 range = _max (_max (_max (_abs(speed[0]), _abs(speed[1])), _abs(speed[2])), _abs(speed[3]));
1094 ratio = _convert_mps_to_proportional(range);
1101 ROS_DEBUG(
"adjusted mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f", speed[0], speed[1], speed[2], speed[3]);
1103 speed[0] = _convert_mps_to_proportional(speed[0]);
1104 speed[1] = _convert_mps_to_proportional(speed[1]);
1105 speed[2] = _convert_mps_to_proportional(speed[2]);
1106 speed[3] = _convert_mps_to_proportional(speed[3]);
1108 ROS_DEBUG(
"mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f", speed[0], speed[1], speed[2], speed[3]);
1117 for (i=0; i<(_last_servo); i++) {
1119 switch (_active_drive.mode) {
1121 if (_servo_configs[i].mode_pos == POSITION_LEFTREAR)
1122 _set_pwm_interval_proportional (i+1, speed[2]);
1123 if (_servo_configs[i].mode_pos == POSITION_RIGHTREAR)
1124 _set_pwm_interval_proportional (i+1, speed[3]);
1125 case MODE_DIFFERENTIAL:
1126 if (_servo_configs[i].mode_pos == POSITION_LEFTFRONT)
1127 _set_pwm_interval_proportional (i+1, speed[0]);
1129 if (_servo_configs[i].mode_pos == POSITION_RIGHTFRONT)
1130 _set_pwm_interval_proportional (i+1, speed[1]);
1172 bool set_pwm_frequency (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)
1176 if ((freq<12) || (freq>1024)) {
1177 ROS_ERROR(
"Invalid PWM frequency %d :: PWM frequencies should be between 12 and 1024", freq);
1181 _set_pwm_frequency (freq);
1228 bool config_servos (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)
1235 if ((_active_board<1) || (_active_board>62)) {
1236 ROS_ERROR(
"Internal error - invalid board number %d :: PWM board numbers must be between 1 and 62", _active_board);
1241 for (i=0;i<req.servos.size();i++) {
1242 int servo = req.servos[i].servo;
1243 int center = req.servos[i].center;
1244 int range = req.servos[i].range;
1245 int direction = req.servos[i].direction;
1247 _config_servo (servo, center, range, direction);
1327 bool config_drive_mode (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)
1333 if ((res.error = _config_drive_mode (req.mode, req.rpm, req.radius, req.track, req.scale)))
1336 for (i=0;i<req.servos.size();i++) {
1337 int servo = req.servos[i].servo;
1338 int position = req.servos[i].position;
1340 if (_config_servo_position (servo, position) != 0) {
1370 bool stop_servos (std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
1372 int save_active = _active_board;
1375 for (i=0; i<MAX_BOARDS; i++) {
1376 if (_pwm_boards[i] > 0) {
1377 _set_active_board (i+1);
1378 _set_pwm_interval_all (0, 0);
1381 _set_active_board (save_active);
1389 static std::string _get_string_param (XmlRpc::XmlRpcValue obj, std::string param_name)
1391 XmlRpc::XmlRpcValue &item = obj[param_name];
1392 if (item.getType() == XmlRpc::XmlRpcValue::TypeString)
1395 ROS_WARN(
"invalid paramter type for %s - expected TypeString", param_name.c_str());
1400 static int _get_int_param (XmlRpc::XmlRpcValue obj, std::string param_name)
1402 XmlRpc::XmlRpcValue &item = obj[param_name];
1403 if (item.getType() == XmlRpc::XmlRpcValue::TypeInt)
1406 ROS_WARN(
"invalid paramter type for %s - expected TypeInt", param_name.c_str());
1411 static double _get_float_param (XmlRpc::XmlRpcValue obj, std::string param_name)
1413 XmlRpc::XmlRpcValue &item = obj[param_name];
1414 if (item.getType() == XmlRpc::XmlRpcValue::TypeDouble)
1417 ROS_WARN(
"invalid paramter type for %s - expected TypeDouble", param_name.c_str());
1422 static int _load_params (
void)
1424 ros::NodeHandle nhp;
1431 nhp.param (
"pwm_frequency", pwm, 50);
1443 if(nhp.hasParam (
"servo_config")) {
1444 XmlRpc::XmlRpcValue servos;
1445 nhp.getParam (
"servo_config", servos);
1447 if(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
1448 ROS_DEBUG(
"Retrieving members from 'servo_config' in namespace(%s)", nhp.getNamespace().c_str());
1450 for(int32_t i = 0; i < servos.size(); i++) {
1451 XmlRpc::XmlRpcValue servo;
1453 if(servo.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
1454 ROS_DEBUG(
"Retrieving items from 'servo_config' member %d in namespace(%s)", i, nhp.getNamespace().c_str());
1457 int id = 0, center = 0, direction = 0, range = 0;
1458 id = _get_int_param (servo,
"id");
1459 center = _get_int_param (servo,
"center");
1460 direction = _get_int_param (servo,
"direction");
1461 range = _get_int_param (servo,
"range");
1463 if (
id && center && direction && range) {
1464 if ((
id >= 1) && (
id <= MAX_SERVOS)) {
1465 int board = ((int)(
id / 16)) + 1;
1466 _set_active_board (board);
1467 _set_pwm_frequency (pwm);
1468 _config_servo (
id, center, range, direction);
1471 ROS_WARN(
"Parameter servo id=%d is out of bounds",
id);
1474 ROS_WARN(
"Invalid parameters for servo id=%d'",
id);
1477 ROS_WARN(
"Invalid type %d for member of 'servo_config' - expected TypeStruct(%d)", servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);
1481 ROS_WARN(
"Invalid type %d for 'servo_config' - expected TypeArray(%d)", servos.getType(), XmlRpc::XmlRpcValue::TypeArray);
1484 ROS_DEBUG(
"Parameter Server namespace[%s] does not contain 'servo_config", nhp.getNamespace().c_str());
1502 if(nhp.hasParam (
"drive_config")) {
1503 XmlRpc::XmlRpcValue drive;
1504 nhp.getParam (
"drive_config", drive);
1506 if(drive.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
1507 ROS_DEBUG(
"Retrieving members from 'drive_config' in namespace(%s)", nhp.getNamespace().c_str());
1511 float radius, rpm, scale, track;
1514 mode = _get_string_param (drive,
"mode");
1515 rpm = _get_float_param (drive,
"rpm");
1516 radius = _get_float_param (drive,
"radius");
1517 track = _get_float_param (drive,
"track");
1518 scale = _get_float_param (drive,
"scale");
1520 _config_drive_mode (mode, rpm, radius, track, scale);
1522 XmlRpc::XmlRpcValue &servos = drive[
"servos"];
1523 if(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
1524 ROS_DEBUG(
"Retrieving members from 'drive_config/servos' in namespace(%s)", nhp.getNamespace().c_str());
1526 for(int32_t i = 0; i < servos.size(); i++) {
1527 XmlRpc::XmlRpcValue servo;
1529 if(servo.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
1530 ROS_DEBUG(
"Retrieving items from 'drive_config/servos' member %d in namespace(%s)", i, nhp.getNamespace().c_str());
1534 id = _get_int_param (servo,
"id");
1535 position = _get_int_param (servo,
"position");
1538 _config_servo_position (
id, position);
1541 ROS_WARN(
"Invalid type %d for member %d of 'drive_config/servos' - expected TypeStruct(%d)", i, servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);
1545 ROS_WARN(
"Invalid type %d for 'drive_config/servos' - expected TypeArray(%d)", servos.getType(), XmlRpc::XmlRpcValue::TypeArray);
1548 ROS_WARN(
"Invalid type %d for 'drive_config' - expected TypeStruct(%d)", drive.getType(), XmlRpc::XmlRpcValue::TypeStruct);
1551 ROS_DEBUG(
"Parameter Server namespace[%s] does not contain 'drive_config", nhp.getNamespace().c_str());
1560 int main (
int argc,
char **argv)
1563 _controller_io_handle = 0;
1564 _pwm_frequency = 50;
1567 _init (_CONST(
"/dev/i2c-1"));
1569 _set_active_board (1);
1571 _set_pwm_frequency (50);
1574 ros::init (argc, argv,
"i2cpwm_controller");
1578 ros::ServiceServer freq_srv = n.advertiseService (
"set_pwm_frequency",
set_pwm_frequency);
1579 ros::ServiceServer config_srv = n.advertiseService (
"config_servos",
config_servos);
1580 ros::ServiceServer mode_srv = n.advertiseService (
"config_drive_mode",
config_drive_mode);
1581 ros::ServiceServer stop_srv = n.advertiseService (
"stop_servos",
stop_servos);
1583 ros::Subscriber abs_sub = n.subscribe (
"servos_absolute", 500,
servos_absolute);
1585 ros::Subscriber drive_sub = n.subscribe (
"servos_drive", 500,
servos_drive);
1591 close (_controller_io_handle);
bool set_pwm_frequency(i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)
service to set set PWM frequency
bool config_servos(i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)
store configuration data for servos on the active board
bool stop_servos(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
service to stop all servos on all boards
bool config_drive_mode(i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)
set drive mode and drive servos
void servos_drive(const geometry_msgs::Twist::ConstPtr &msg)
subscriber topic to move servos based on a drive mode
void servos_absolute(const i2cpwm_board::ServoArray::ConstPtr &msg)
subscriber topic to move servos in a physical position
void servos_proportional(const i2cpwm_board::ServoArray::ConstPtr &msg)
subscriber topic to move servos in the range of ±1.0