ssc32.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include "ssc32.hpp"
00003 
00004 namespace Servo
00005 {
00006     //Constructor
00007     SSC32::SSC32(){
00008         nrOfPorts = 32;
00009         }
00010     
00011     //Destructor
00012         SSC32::~SSC32() {}
00013 
00014     void SSC32::initSerial()
00015     {
00016         // Set baud rate
00017         cfsetispeed(&port_settings, BAUDRATE);
00018         cfsetospeed(&port_settings, BAUDRATE);
00019         
00020         // Initialise the timeout structure
00021         timeout.tv_sec = TIMEOUT_SEC;
00022 
00023         // Process the configuration file
00024         /*FILE *cFile;
00025         cFile = fopen("config.txt","r");
00026         int m,n,o,p,q,r;
00027         nrOfPorts = 0;
00028         int res = fscanf (cFile, "%d %d %d %d %d %d", &m, &n, &o, &p, &q, &r);
00029         while(res == 6)
00030         {
00031             servoLimits[nrOfPorts][0] = m;
00032             servoLimits[nrOfPorts][1] = n;
00033             servoLimits[nrOfPorts][2] = o;
00034             servoLimits[nrOfPorts][3] = p;
00035             servoLimits[nrOfPorts][4] = q;
00036             servoLimits[nrOfPorts][5] = r;
00037             res = fscanf (cFile, "%d %d %d %d %d %d", &m, &n, &o, &p, &q, &r);
00038             nrOfPorts ++;
00039         }
00040         std::cout << "number of ports: " << nrOfPorts << std::endl;*/
00041 
00042     }
00043     
00044     bool SSC32::startSerial(std::string COM_PORT_)
00045     {
00046         const char *COM_PORT;
00047         COM_PORT = COM_PORT_.c_str();
00048         int flags;
00049         printf("COM_PORT =  %s \n",COM_PORT);
00050         // Try to open the serial port
00051         fd = open(COM_PORT, O_RDWR | O_NONBLOCK, S_IRUSR | S_IWUSR);
00052         //fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
00053         if(fd == -1)  //Unable to open the serial port
00054         {
00055             std::cout << "(SSC-32) Unable to open " << COM_PORT<< "\n" << std::endl;
00056             return false;
00057         }
00058 
00059         if (tcflush(fd, TCIFLUSH) < 0)
00060         {
00061                 close(fd);
00062                 fd = -1;
00063                 return false;
00064         }
00065         if (tcgetattr(fd, &port_settings) < 0)
00066         {
00067                 close(fd);
00068                 fd = -1;
00069                 return false;
00070         }
00071 
00072         cfmakeraw(&port_settings);
00073         cfsetispeed(&port_settings, BAUDRATE);
00074         cfsetospeed(&port_settings, BAUDRATE);
00075 
00076         if (tcsetattr(fd, TCSAFLUSH, &port_settings) < 0)
00077         {
00078                 perror("corobot_open():tcsetattr():");
00079                 close(fd);
00080                 fd = -1;
00081                 return false;
00082         }
00083 
00084             /* We know the robot is there; switch to blocking */
00085             /* ok, we got data, so now set NONBLOCK, and continue */
00086         if ((flags = fcntl(fd, F_GETFL)) < 0)
00087         {
00088                 perror("corobot_comm_open():fcntl():");
00089                 close(fd);
00090                 fd = -1;
00091                 return false;
00092         }
00093         if (fcntl(fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
00094         {
00095                 perror("corobot_comm_open():fcntl()");
00096                 close(fd);
00097                 fd = -1;
00098                 return false;
00099         }
00100         else
00101         {
00102             std::cout << "(SSC-32) Opened " << COM_PORT << "\n" << std::endl;
00103             return true;
00104         }
00105 
00106     }
00107     
00108     void SSC32::stopSerial()
00109     {   
00110         // Close the serial port
00111         SendMessage(0,0,0);
00112         SendMessage(1,0,0);
00113         
00114         close(fd);
00115     }
00116 
00117     bool SSC32::SendMessage(int port, int position, int speed)
00118     {
00119         // Check port, position and speed limits
00120         if(port >= nrOfPorts || port < 0)
00121         {
00122             std::cout << "(SSC-32) Port number not valid: " << port << std::endl;
00123             return false;
00124         }
00125         
00126         /*if(position < servoLimits[port][1]) position = servoLimits[port][1];
00127         else if(position > servoLimits[port][2]) position = servoLimits[port][2];
00128 
00129         if(speed > servoLimits[port][3]) speed = servoLimits[port][3];
00130         else if(speed < 0) speed = 0;
00131         
00132         // Scale position and speed 
00133         position = (position - servoLimits[port][1]) / (servoLimits[port][2] - servoLimits[port][1]) * (servoLimits[port][5] - servoLimits[port][4]) + servoLimits[port][4];
00134 
00135         speed = (speed) / (servoLimits[port][3]) * (servoLimits[port][5] - servoLimits[port][4]) + servoLimits[port][4];*/
00136         
00137         // Convert port, position and speed to string
00138         std::ostringstream temp, temp2, temp3; 
00139         temp << port; 
00140         Port = temp.str();
00141 
00142         temp2 << position; 
00143         Position = temp2.str();
00144 
00145         temp3 << speed; 
00146         Speed = temp3.str(); 
00147         
00148         // Compose actual command
00149         if(speed == -1)
00150                 command= "#" + Port + " P" + Position + " \r";  //Using T, you can use T = 0 to stop the servo output.
00151         else
00152                 command= "#" + Port + " P" + Position + " T" + Speed + " \r";  //Using T, you can use T = 0 to stop the servo output.
00153         Output = command.c_str();
00154         
00155         // Write command to port
00156         int err = write(fd,Output,strlen(Output));
00157         std::cout << "(SSC32) " << command << "\n" << std::endl;
00158 
00159         if(err == -1)
00160                 ROS_ERROR("Error sending command to SSC32 controller");
00161         else if(err == 0)
00162                 ROS_WARN("The command was not send to the SSC32 controller");
00163         else if(err != ((int) strlen(Output)))
00164                 ROS_WARN("Only a part of the command was sent to the SSC32 controller, %d bits out of %d", err, (int) strlen(Output));
00165         else
00166                 ROS_DEBUG("Command sent successfully to the SSC32 Controller");
00167         return true;
00168     }
00169 
00170     bool SSC32::CheckIfFinished()
00171     {   
00172             char someData; 
00173             int err = write(fd,"Q \r",strlen("Q \r"));
00174             err = read(fd,&someData,1);
00175             if (char(someData) == '.') 
00176         {
00177             std::cout << "(SSC32) Servos have finished rotating " << "\n" << std::endl;
00178             return true;
00179         }
00180             return false;
00181     }
00182 }//namespace
00183 


corobot_ssc32
Author(s):
autogenerated on Wed Aug 26 2015 11:09:37