SHARE
TWEET

smart_motor_node

a guest Aug 14th, 2014 227 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <csignal>
  2. #include <cstdio>
  3. #include "ros/ros.h"
  4. #include "toro/motorInterface.h"
  5. #include <math.h>
  6. #include <toro/Sweep.h>
  7.  
  8. CMotorInterface motorInterface;
  9.  
  10. void sweep_callback(const toro::Sweep::ConstPtr&/*toro::State::ConstPtr&*/ sweep_command)
  11. {
  12.         // Start the motor sweeping
  13.         motorInterface.setStart(sweep_command->start);
  14.         motorInterface.setEnd(sweep_command->end);
  15.         motorInterface.setSpeed(sweep_command->speed);
  16.         motorInterface.startSweeping();
  17.        
  18.         double startTime = ros::Time::now().toSec();
  19.         double endTime = startTime+sweep_command->duration;
  20.        
  21.         ros::Rate loop_rate(10); //set loop rate to 10 hz
  22.        
  23.         while (ros::ok() && ros::Time::now().toSec()<endTime)
  24.         {
  25.                 //wait to eventually end sweep
  26.                 loop_rate.sleep();
  27.         }
  28.         //now that time has run out, or user has killed the program, stop sweeping
  29.         motorInterface.stopSweeping();
  30.  
  31. }
  32.  
  33. int main(int argc, char **argv)
  34. {
  35.  
  36.         //smart motor interface stuff
  37.         const char *port = "/dev/ttyUSB0";
  38.  
  39.         ros::init(argc, argv, "smart_motor_node");
  40.         ros::NodeHandle n;
  41.  
  42.         //connect to smart motor
  43.         ROS_INFO("opening serial port %s", port);
  44.         try
  45.         {
  46.                 motorInterface.open(port);
  47.         } catch (ESMSerial &exception)
  48.         {
  49.                 ROS_INFO("Failed to open smart motor serial port");
  50.                 return(0);
  51.         }
  52.         //home smart motor
  53.         int position, oldPosition, positionSame;
  54.         try
  55.         {
  56.                 ROS_INFO("Homing smart motor");
  57.                 if (!motorInterface.homeMotor())
  58.                 {
  59.                         ROS_INFO("Failed to home smart motor");
  60.                         return(0);
  61.                 }
  62.                 // Wait for position to settle
  63.                 positionSame = oldPosition = 0;
  64.                 do
  65.                 {
  66.                         position = motorInterface.getPosition();
  67.                         ROS_INFO("position: %d, oldPosition: %d", position, oldPosition);
  68.                         if (oldPosition == position)
  69.                         {
  70.                                 positionSame++;
  71.                         } else
  72.                         {
  73.                                 positionSame = 0;
  74.                         }
  75.                         oldPosition = position;
  76.                 } while (positionSame < 10);
  77.         } catch (ESMSerial &exception)
  78.         {
  79.                 ROS_INFO("Failed to home smart motor");
  80.                 return(0);
  81.         }
  82.         ROS_INFO("Final position: %d", position);
  83.  
  84.         //set up a custom message subscriber
  85.         ros::Subscriber sweep_subscriber = n.subscribe<toro::Sweep>("/sweep", 1, sweep_callback);
  86.        
  87.         ros::spin();
  88. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top