i2cpwm_board  0.5.1
ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface
i2cpwm_controller.cpp
Go to the documentation of this file.
1 
169 #include <stdio.h>
170 #include <stdlib.h>
171 #include <string.h>
172 #include <math.h>
173 #include <fcntl.h>
174 #include <errno.h>
175 #include <unistd.h>
176 #include <sys/ioctl.h>
177 #include <sys/types.h>
178 #include <sys/stat.h>
179 #include <linux/i2c-dev.h>
180 
181 #include <ros/ros.h>
182 #include <ros/console.h>
183 
184 // messages used for any service with no parameters
185 #include <std_srvs/Empty.h>
186 // messages used for drive movement topic
187 #include <geometry_msgs/Twist.h>
188 
189 // messages used for the absolute and proportional movement topics
190 #include "i2cpwm_board/Servo.h"
191 #include "i2cpwm_board/ServoArray.h"
192 // messages used for the servo setup service
193 #include "i2cpwm_board/ServoConfig.h"
194 #include "i2cpwm_board/ServoConfigArray.h"
195 // request/response of the servo setup service
196 #include "i2cpwm_board/ServosConfig.h"
197 // request/response of the drive mode service
198 #include "i2cpwm_board/DriveMode.h"
199 #include "i2cpwm_board/Position.h"
200 #include "i2cpwm_board/PositionArray.h"
201 // request/response of the integer parameter services
202 #include "i2cpwm_board/IntValue.h"
203 
204 
206 
207 typedef struct _servo_config {
208  int center;
209  int range;
210  int direction;
211  int mode_pos;
212 } servo_config;
213 
214 typedef struct _drive_mode {
215  int mode;
216  float rpm;
217  float radius;
218  float track;
219  float scale;
220 } drive_mode;
221 
222 enum drive_modes {
223  MODE_UNDEFINED = 0,
224  MODE_ACKERMAN = 1,
225  MODE_DIFFERENTIAL = 2,
226  MODE_MECANUM = 3,
227  MODE_INVALID = 4
228 };
229 
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,
236  POSITION_INVALID = 5
237 };
238 
239 #define _BASE_ADDR 0x40
240 #ifndef _PI
241 #define _PI 3.14159265358979323846
242 #endif
243 #define _CONST(s) ((char*)(s))
244 
245 enum pwm_regs {
246  // Registers/etc.
247  __MODE1 = 0x00,
248  __MODE2 = 0x01,
249  __SUBADR1 = 0x02, // enable sub address 1 support
250  __SUBADR2 = 0x03, // enable sub address 2 support
251  __SUBADR3 = 0x04, // enable sub address 2 support
252  __PRESCALE = 0xFE,
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,
261  __RESTART = 0x80,
262  __SLEEP = 0x10, // enable low power mode
263  __ALLCALL = 0x01,
264  __INVRT = 0x10, // invert the output control logic
265  __OUTDRV = 0x04
266 };
267 
268 #define MAX_BOARDS 62
269 #define MAX_SERVOS (16*MAX_BOARDS)
270 
271 servo_config _servo_configs[MAX_SERVOS]; // we can support up to 62 boards (1..62), each with 16 PWM devices (1..16)
272 drive_mode _active_drive; // used when converting Twist geometry to PWM values and which servos are for motion
273 int _last_servo = -1;
274 
275 int _pwm_boards[MAX_BOARDS]; // we can support up to 62 boards (1..62)
276 int _active_board = 0; // used to determine if I2C SLAVE change is needed
277 int _controller_io_handle; // linux file handle for I2C
278 
279 int _pwm_frequency = 50; // frequency determines the size of a pulse width; higher numbers make RC servos buzz
280 
281 
283 
284 
285 
286 //* ------------------------------------------------------------------------------------------------------------------------------------
287 // local private methods
288 //* ------------------------------------------------------------------------------------------------------------------------------------
289 
290 static float _abs (float v1)
291 {
292  if (v1 < 0)
293  return (0 - v1);
294  return v1;
295 }
296 
297 static float _min (float v1, float v2)
298 {
299  if (v1 > v2)
300  return v2;
301  return v1;
302 }
303 
304 static float _max (float v1, float v2)
305 {
306  if (v1 < v2)
307  return v2;
308  return v1;
309 }
310 
311 static float _absmin (float v1, float v2)
312 {
313  float a1, a2;
314  float sign = 1.0;
315  // if (v1 < 0)
316  // sign = -1.0;
317  a1 = _abs(v1);
318  a2 = _abs(v2);
319  if (a1 > a2)
320  return (sign * a2);
321  return v1;
322 }
323 
324 static float _absmax (float v1, float v2)
325 {
326  float a1, a2;
327  float sign = 1.0;
328  // if (v1 < 0)
329  // sign = -1.0;
330  a1 = _abs(v1);
331  a2 = _abs(v2);
332  if (a1 < a2)
333  return (sign * a2);
334  return v1;
335 }
336 
337 
338 
349 static int _smoothing (float speed)
350 {
351  /* if smoothing is desired, then remove the commented code */
352  // speed = (cos(_PI*(((float)1.0 - speed))) + 1) / 2;
353  return speed;
354 }
355 
356 
363 static float _convert_mps_to_proportional (float speed)
364 {
365  /* we use the drive mouter output rpm and wheel radius to compute the conversion */
366 
367  float initial, max_rate; // the max m/s is ((rpm/60) * (2*PI*radius))
368 
369  initial = speed;
370 
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);
373  return 0.0;
374  }
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);
377  return 0.0;
378  }
379 
380  max_rate = (_active_drive.radius * _PI * 2) * (_active_drive.rpm / 60.0);
381 
382  speed = speed / max_rate;
383  // speed = _absmin (speed, 1.0);
384 
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);
386  return speed;
387 }
388 
389 
390 
398 static void _set_pwm_frequency (int freq)
399 {
400  int prescale;
401  char oldmode, newmode;
402  int res;
403 
404  _pwm_frequency = freq; // save to global
405 
406  ROS_DEBUG("_set_pwm_frequency prescale");
407  float prescaleval = 25000000.0; // 25MHz
408  prescaleval /= 4096.0;
409  prescaleval /= (float)freq;
410  prescaleval -= 1.0;
411  //ROS_INFO("Estimated pre-scale: %6.4f", prescaleval);
412  prescale = floor(prescaleval + 0.5);
413  // ROS_INFO("Final pre-scale: %d", prescale);
414 
415 
416  ROS_INFO("Setting PWM frequency to %d Hz", freq);
417 
418  nanosleep ((const struct timespec[]){{1, 000000L}}, NULL);
419 
420 
421  oldmode = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);
422  newmode = (oldmode & 0x7F) | 0x10; // sleep
423 
424  if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, newmode)) // go to sleep
425  ROS_ERROR("Unable to set PWM controller to sleep mode");
426 
427  if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __PRESCALE, (int)(floor(prescale))))
428  ROS_ERROR("Unable to set PWM controller prescale");
429 
430  if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode))
431  ROS_ERROR("Unable to set PWM controller to active mode");
432 
433  nanosleep((const struct timespec[]){{0, 5000000L}}, NULL); //sleep 5microsec,
434 
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");
437 }
438 
439 
440 
449 static void _set_pwm_interval_all (int start, int end)
450 {
451  // the public API is ONE based and hardware is ZERO based
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);
454  return;
455  }
456  int board = _active_board - 1;
457 
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);
466 }
467 
468 
469 
476 static void _set_active_board (int board)
477 {
478  char mode1res;
479 
480  if ((board<1) || (board>62)) {
481  ROS_ERROR("Invalid board number %d :: board numbers must be between 1 and 62", board);
482  return;
483  }
484  if (_active_board != board) {
485  _active_board = board; // save to global
486 
487  // the public API is ONE based and hardware is ZERO based
488  board--;
489 
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));
492  return; /* exit(1) */ /* additional ERROR HANDLING information is available with 'errno' */
493  }
494 
495  if (_pwm_boards[board]<0) {
496  _pwm_boards[board] = 1;
497 
498  /* this is guess but I believe the following needs to be done on each board only once */
499 
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");
502 
503  if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, __ALLCALL))
504  ROS_ERROR ("Failed to enable ALLCALL for PWM channels");
505 
506  nanosleep ((const struct timespec[]){{0, 5000000L}}, NULL); //sleep 5microsec, wait for osci
507 
508 
509  mode1res = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);
510  mode1res = mode1res & ~__SLEEP; // # wake up (reset sleep)
511 
512  if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, mode1res))
513  ROS_ERROR ("Failed to recover from low power mode");
514 
515  nanosleep((const struct timespec[]){{0, 5000000L}}, NULL); //sleep 5microsec, wait for osci
516 
517  // the first time we activate a board, we mark it and set all of its servo channels to 0
518  _set_pwm_interval_all (0, 0);
519  }
520  }
521 }
522 
523 
524 
534 static void _set_pwm_interval (int servo, int start, int end)
535 {
536  ROS_DEBUG("_set_pwm_interval enter");
537 
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);
540  return;
541  }
542 
543  int board = ((int)(servo/16))+1;
544  _set_active_board(board);
545 
546  servo = servo % 16;
547 
548  board = _active_board - 1;
549 
550  ROS_DEBUG("_set_pwm_interval board=%d", board);
551  // the public API is ONE based and hardware is ZERO based
552 
553  int channel = servo - 1;
554 
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);
563 }
564 
565 
566 
575 static void _set_pwm_interval_proportional (int servo, float value)
576 {
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);
579  return;
580  }
581 
582  servo_config* configp = &(_servo_configs[servo-1]);
583 
584  if ((configp->center < 0) ||(configp->range < 0)) {
585  ROS_ERROR("Missing servo configuration for servo[%d]", servo);
586  return;
587  }
588 
589  int pos = (configp->direction * (((float)(configp->range) / 2) * value)) + configp->center;
590 
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);
593  return;
594  }
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);
597 }
598 
599 
600 
610 static void _config_servo (int servo, int center, int range, int direction)
611 {
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);
614  return;
615  }
616 
617  if ((center < 0) || (center > 4096))
618  ROS_ERROR("Invalid center value %d :: center values must be between 0 and 4096", center);
619 
620  if ((center < 0) || (center > 4096))
621  ROS_ERROR("Invalid range value %d :: range values must be between 0 and 4096", range);
622 
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));
625 
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;
630 
631  if (servo > _last_servo) // used for internal optimizations
632  _last_servo = servo;
633 
634  ROS_INFO("Servo #%d configured: center=%d, range=%d, direction=%d", servo, center, range, direction);
635 }
636 
637 
638 static int _config_servo_position (int servo, int position)
639 {
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);
642  return -1;
643  }
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);
646  return -1;
647  }
648  _servo_configs[servo-1].mode_pos = position;
649  ROS_INFO("Servo #%d configured: position=%d", servo, position);
650  return 0;
651 }
652 
653 
654 static int _config_drive_mode (std::string mode, float rpm, float radius, float track, float scale)
655 {
656  int mode_val = MODE_UNDEFINED;
657 
658  // assumes the parameter was provided in the proper case
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;
665  else {
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());
668  return -1;
669  }
670 
671  if (rpm <= 0.0) {
672  ROS_ERROR("Invalid RPM %6.4f :: the motor's output RPM must be greater than 0.0", rpm);
673  return -1;
674  }
675 
676  if (radius <= 0.0) {
677  ROS_ERROR("Invalid radius %6.4f :: the wheel radius must be greater than 0.0 meters", radius);
678  return -1;
679  }
680 
681  if (track <= 0.0) {
682  ROS_ERROR("Invalid track %6.4f :: the axel track must be greater than 0.0 meters", track);
683  return -1;
684  }
685 
686  if (scale <= 0.0) {
687  ROS_ERROR("Invalid scale %6.4f :: the scalar for Twist messages must be greater than 0.0", scale);
688  return -1;
689  }
690 
691  _active_drive.mode = mode_val;
692  _active_drive.rpm = rpm;
693  _active_drive.radius = radius; // the service takes the radius in meters
694  _active_drive.track = track; // the service takes the track in meters
695  _active_drive.scale = scale;
696 
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);
698  return 0;
699 }
700 
701 
702 
710 static void _init (char* filename)
711 {
712  int res;
713  char mode1res;
714  int i;
715 
716  /* initialize all of the global data objects */
717 
718  for (i=0; i<MAX_BOARDS;i++)
719  _pwm_boards[i] = -1;
720  _active_board = -1;
721 
722  for (i=0; i<(MAX_SERVOS);i++) {
723  // these values have not useful meaning
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;
728  }
729  _last_servo = -1;
730 
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;
736 
737 
738  if ((_controller_io_handle = open (filename, O_RDWR)) < 0) {
739  ROS_FATAL ("Failed to open I2C bus %s", filename);
740  return; /* exit(1) */ /* additional ERROR HANDLING information is available with 'errno' */
741  }
742 }
743 
744 
745 
746 // ------------------------------------------------------------------------------------------------------------------------------------
750 // ------------------------------------------------------------------------------------------------------------------------------------
751 
797 void servos_absolute (const i2cpwm_board::ServoArray::ConstPtr& msg)
798 {
799  /* this subscription works on the active_board */
800 
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;
804 
805  if ((value < 0) || (value > 4096)) {
806  ROS_ERROR("Invalid PWM value %d :: PWM values must be between 0 and 4096", value);
807  continue;
808  }
809  _set_pwm_interval (servo, 0, value);
810  ROS_DEBUG("servo[%d] = %d", servo, value);
811  }
812 }
813 
814 
867 void servos_proportional (const i2cpwm_board::ServoArray::ConstPtr& msg)
868 {
869  /* this subscription works on the active_board */
870 
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);
875  }
876 }
877 
878 
879 
880 
976 void servos_drive (const geometry_msgs::Twist::ConstPtr& msg)
977 {
978  /* this subscription works on the active_board */
979 
980  int i;
981  float delta, range, ratio;
982  float temp_x, temp_y, temp_r;
983  float dir_x, dir_y, dir_r;
984  float speed[4];
985 
986  /* msg is a pointer to a Twist message: msg->linear and msg->angular each of which have members .x .y .z */
987  /* the subscriber uses the maths from: http://robotsforroboticists.com/drive-kinematics/ */
988 
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);
991 
992  if (_active_drive.mode == MODE_UNDEFINED) {
993  ROS_ERROR("drive mode not set");
994  return;
995  }
996  if ((_active_drive.mode < MODE_UNDEFINED) || (_active_drive.mode >= MODE_INVALID)) {
997  ROS_ERROR("unrecognized drive mode set %d", _active_drive.mode);
998  return;
999  }
1000 
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);
1004 
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);
1008 
1009  // temp_x = _smoothing (temp_x);
1010  // temp_y = _smoothing (temp_y);
1011  // temp_r = _smoothing (temp_r) / 2;
1012 
1013  /* the delta is the angular velocity * half the drive track */
1014  delta = (temp_r * (_active_drive.track / 2));
1015 
1016  ratio = _convert_mps_to_proportional(temp_y + delta);
1017  if (ratio > 1.0)
1018  temp_y /= ratio;
1019 
1020 
1021  switch (_active_drive.mode) {
1022 
1023  case MODE_ACKERMAN:
1024  /*
1025  with ackerman drive, steering is handled by a separate servo
1026  we drive assigned servos exclusively by the linear.x
1027  */
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;
1032 
1033  ROS_DEBUG("ackerman drive mode speed=%6.4f", speed[0]);
1034  break;
1035 
1036  case MODE_DIFFERENTIAL:
1037  /*
1038  with differential drive, steering is handled by the relative speed of left and right servos
1039  we drive assigned servos by mixing linear.x and angular.z
1040  we compute the delta for left and right components
1041  we use the sign of the angular velocity to determine which is the faster / slower
1042  */
1043 
1044  /* the delta is the angular velocity * half the drive track */
1045 
1046  if (dir_r > 0) { // turning right
1047  speed[0] = (temp_y + delta) * dir_y;
1048  speed[1] = (temp_y - delta) * dir_y;
1049  } else { // turning left
1050  speed[0] = (temp_y - delta) * dir_y;
1051  speed[1] = (temp_y + delta) * dir_y;
1052  }
1053 
1054  ROS_DEBUG("computed differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
1055 
1056  /* if any of the results are greater that 1.0, we need to scale all the results down */
1057  range = _max (_abs(speed[0]), _abs(speed[1]));
1058 
1059  ratio = _convert_mps_to_proportional(range);
1060  if (ratio > 1.0) {
1061  speed[0] /= ratio;
1062  speed[1] /= ratio;
1063  }
1064  ROS_DEBUG("adjusted differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
1065 
1066  speed[0] = _convert_mps_to_proportional(speed[0]);
1067  speed[1] = _convert_mps_to_proportional(speed[1]);
1068 
1069  ROS_DEBUG("differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
1070  break;
1071 
1072  case MODE_MECANUM:
1073  /*
1074  with mecanum drive, steering is handled by the relative speed of left and right servos
1075  with mecanum drive, lateral motion is handled by the rotation of front and rear servos
1076  we drive assigned servos by mixing linear.x and angular.z and linear.y
1077  */
1078 
1079  if (dir_r > 0) { // turning right
1080  speed[0] = speed[2] = (temp_y + delta) * dir_y;
1081  speed[1] = speed[3] = (temp_y - delta) * dir_y;
1082  } else { // turning left
1083  speed[0] = speed[2] = (temp_y - delta) * dir_y;
1084  speed[1] = speed[3] = (temp_y + delta) * dir_y;
1085  }
1086 
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]);
1092 
1093  range = _max (_max (_max (_abs(speed[0]), _abs(speed[1])), _abs(speed[2])), _abs(speed[3]));
1094  ratio = _convert_mps_to_proportional(range);
1095  if (ratio > 1.0) {
1096  speed[0] /= ratio;
1097  speed[1] /= ratio;
1098  speed[2] /= ratio;
1099  speed[3] /= ratio;
1100  }
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]);
1102 
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]);
1107 
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]);
1109  break;
1110 
1111  default:
1112  break;
1113 
1114  }
1115 
1116  /* find all drive servos and set their new speed */
1117  for (i=0; i<(_last_servo); i++) {
1118  // we use 'fall thru' on the switch statement to allow all necessary servos to be controlled
1119  switch (_active_drive.mode) {
1120  case MODE_MECANUM:
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]);
1128  case MODE_ACKERMAN:
1129  if (_servo_configs[i].mode_pos == POSITION_RIGHTFRONT)
1130  _set_pwm_interval_proportional (i+1, speed[1]);
1131  }
1132  }
1133 }
1134 
1135 
1136 // ------------------------------------------------------------------------------------------------------------------------------------
1141 // services
1142 // ------------------------------------------------------------------------------------------------------------------------------------
1143 
1172 bool set_pwm_frequency (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)
1173 {
1174  int freq;
1175  freq = req.value;
1176  if ((freq<12) || (freq>1024)) {
1177  ROS_ERROR("Invalid PWM frequency %d :: PWM frequencies should be between 12 and 1024", freq);
1178  freq = 50; // most analog RC servos are designed for 20ms pulses.
1179  res.error = freq;
1180  }
1181  _set_pwm_frequency (freq); // I think we must reset frequency when we change boards
1182  res.error = freq;
1183  return true;
1184 }
1185 
1186 
1228 bool config_servos (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)
1229 {
1230  /* this service works on the active_board */
1231  int i;
1232 
1233  res.error = 0;
1234 
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);
1237  res.error = -1;
1238  return true;
1239  }
1240 
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;
1246 
1247  _config_servo (servo, center, range, direction);
1248  }
1249 
1250  return true;
1251 }
1252 
1253 
1327 bool config_drive_mode (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)
1328 {
1329  res.error = 0;
1330 
1331  int i;
1332 
1333  if ((res.error = _config_drive_mode (req.mode, req.rpm, req.radius, req.track, req.scale)))
1334  return true;
1335 
1336  for (i=0;i<req.servos.size();i++) {
1337  int servo = req.servos[i].servo;
1338  int position = req.servos[i].position;
1339 
1340  if (_config_servo_position (servo, position) != 0) {
1341  res.error = servo; /* this needs to be more specific and indicate a bad server ID was provided */
1342  continue;
1343  }
1344  }
1345 
1346  return true;
1347 }
1348 
1349 
1350 
1370 bool stop_servos (std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
1371 {
1372  int save_active = _active_board;
1373  int i;
1374 
1375  for (i=0; i<MAX_BOARDS; i++) {
1376  if (_pwm_boards[i] > 0) {
1377  _set_active_board (i+1); // API is ONE based
1378  _set_pwm_interval_all (0, 0);
1379  }
1380  }
1381  _set_active_board (save_active); // restore last active board
1382  return true;
1383 }
1384 
1385 
1386 
1387 
1388 
1389 static std::string _get_string_param (XmlRpc::XmlRpcValue obj, std::string param_name)
1390 {
1391  XmlRpc::XmlRpcValue &item = obj[param_name];
1392  if (item.getType() == XmlRpc::XmlRpcValue::TypeString)
1393  return item;
1394 
1395  ROS_WARN("invalid paramter type for %s - expected TypeString", param_name.c_str());
1396  return 0;
1397 }
1398 
1399 
1400 static int _get_int_param (XmlRpc::XmlRpcValue obj, std::string param_name)
1401 {
1402  XmlRpc::XmlRpcValue &item = obj[param_name];
1403  if (item.getType() == XmlRpc::XmlRpcValue::TypeInt)
1404  return item;
1405 
1406  ROS_WARN("invalid paramter type for %s - expected TypeInt", param_name.c_str());
1407  return 0;
1408 }
1409 
1410 
1411 static double _get_float_param (XmlRpc::XmlRpcValue obj, std::string param_name)
1412 {
1413  XmlRpc::XmlRpcValue &item = obj[param_name];
1414  if (item.getType() == XmlRpc::XmlRpcValue::TypeDouble)
1415  return item;
1416 
1417  ROS_WARN("invalid paramter type for %s - expected TypeDouble", param_name.c_str());
1418  return 0;
1419 }
1420 
1421 
1422 static int _load_params (void)
1423 {
1424  ros::NodeHandle nhp; // not currently private namespace
1425 
1426  /*
1427  pwm_frequency: 50
1428  */
1429 
1430  int pwm;
1431  nhp.param ("pwm_frequency", pwm, 50);
1432 
1433 
1434  /*
1435  // note: servos are numbered sequntially with '1' being the first servo on board #1, '17' is the first servo on board #2
1436 
1437  servo_config:
1438  - {id: 1, center: 333, direction: -1, range: 100}
1439  - {id: 2, center: 336, direction: 1, range: 108}
1440 
1441  */
1442  // attempt to load configuration for servos
1443  if(nhp.hasParam ("servo_config")) {
1444  XmlRpc::XmlRpcValue servos;
1445  nhp.getParam ("servo_config", servos);
1446 
1447  if(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
1448  ROS_DEBUG("Retrieving members from 'servo_config' in namespace(%s)", nhp.getNamespace().c_str());
1449 
1450  for(int32_t i = 0; i < servos.size(); i++) {
1451  XmlRpc::XmlRpcValue servo;
1452  servo = servos[i]; // get the data from the iterator
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());
1455 
1456  // get the servo settings
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");
1462 
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);
1469  }
1470  else
1471  ROS_WARN("Parameter servo id=%d is out of bounds", id);
1472  }
1473  else
1474  ROS_WARN("Invalid parameters for servo id=%d'", id);
1475  }
1476  else
1477  ROS_WARN("Invalid type %d for member of 'servo_config' - expected TypeStruct(%d)", servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);
1478  }
1479  }
1480  else
1481  ROS_WARN("Invalid type %d for 'servo_config' - expected TypeArray(%d)", servos.getType(), XmlRpc::XmlRpcValue::TypeArray);
1482  }
1483  else
1484  ROS_DEBUG("Parameter Server namespace[%s] does not contain 'servo_config", nhp.getNamespace().c_str());
1485 
1486 
1487  /*
1488  drive_config:
1489  mode: mecanum
1490  radius: 0.062
1491  rpm: 60.0
1492  scale: 0.3
1493  track: 0.2
1494  servos:
1495  - {id: 1, position: 1}
1496  - {id: 2, position: 2}
1497  - {id: 3, position: 3}
1498  - {id: 4, position: 4}
1499  */
1500 
1501  // attempt to load configuration for drive mode
1502  if(nhp.hasParam ("drive_config")) {
1503  XmlRpc::XmlRpcValue drive;
1504  nhp.getParam ("drive_config", drive);
1505 
1506  if(drive.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
1507  ROS_DEBUG("Retrieving members from 'drive_config' in namespace(%s)", nhp.getNamespace().c_str());
1508 
1509  // get the drive mode settings
1510  std::string mode;
1511  float radius, rpm, scale, track;
1512  int id, position;
1513 
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");
1519 
1520  _config_drive_mode (mode, rpm, radius, track, scale);
1521 
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());
1525 
1526  for(int32_t i = 0; i < servos.size(); i++) {
1527  XmlRpc::XmlRpcValue servo;
1528  servo = servos[i]; // get the data from the iterator
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());
1531 
1532  // get the servo position settings
1533  int id, position;
1534  id = _get_int_param (servo, "id");
1535  position = _get_int_param (servo, "position");
1536 
1537  if (id && position)
1538  _config_servo_position (id, position); // had its own error reporting
1539  }
1540  else
1541  ROS_WARN("Invalid type %d for member %d of 'drive_config/servos' - expected TypeStruct(%d)", i, servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);
1542  }
1543  }
1544  else
1545  ROS_WARN("Invalid type %d for 'drive_config/servos' - expected TypeArray(%d)", servos.getType(), XmlRpc::XmlRpcValue::TypeArray);
1546  }
1547  else
1548  ROS_WARN("Invalid type %d for 'drive_config' - expected TypeStruct(%d)", drive.getType(), XmlRpc::XmlRpcValue::TypeStruct);
1549  }
1550  else
1551  ROS_DEBUG("Parameter Server namespace[%s] does not contain 'drive_config", nhp.getNamespace().c_str());
1552 }
1553 
1554 
1555 // ------------------------------------------------------------------------------------------------------------------------------------
1557 // main
1558 // ------------------------------------------------------------------------------------------------------------------------------------
1559 
1560 int main (int argc, char **argv)
1561 {
1562 
1563  _controller_io_handle = 0;
1564  _pwm_frequency = 50; // set the initial pulse frequency to 50 Hz which is standard for RC servos
1565 
1566 
1567  _init (_CONST("/dev/i2c-1")); // default I2C device on RPi2 and RPi3 = "/dev/i2c-1";
1568 
1569  _set_active_board (1);
1570 
1571  _set_pwm_frequency (50);
1572 
1573 
1574  ros::init (argc, argv, "i2cpwm_controller");
1575 
1576  ros::NodeHandle n;
1577 
1578  ros::ServiceServer freq_srv = n.advertiseService ("set_pwm_frequency", set_pwm_frequency);
1579  ros::ServiceServer config_srv = n.advertiseService ("config_servos", config_servos); // 'config' will setup the necessary properties of continuous servos and is helpful for standard servos
1580  ros::ServiceServer mode_srv = n.advertiseService ("config_drive_mode", config_drive_mode); // 'mode' specifies which servos are used for motion and which behavior will be applied when driving
1581  ros::ServiceServer stop_srv = n.advertiseService ("stop_servos", stop_servos); // the 'stop' service can be used at any time
1582 
1583  ros::Subscriber abs_sub = n.subscribe ("servos_absolute", 500, servos_absolute); // the 'absolute' topic will be used for standard servo motion and testing of continuous servos
1584  ros::Subscriber rel_sub = n.subscribe ("servos_proportional", 500, servos_proportional); // the 'proportion' topic will be used for standard servos and continuous rotation aka drive servos
1585  ros::Subscriber drive_sub = n.subscribe ("servos_drive", 500, servos_drive); // the 'drive' topic will be used for continuous rotation aka drive servos controlled by Twist messages
1586 
1587  _load_params();
1588 
1589  ros::spin();
1590 
1591  close (_controller_io_handle);
1592 
1593  return 0;
1594 }
1595 
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