PhidgetServo.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "ros/ros.h"
00031 #include <stdio.h>
00032 #include <phidget21.h>
00033 
00034 #include "corobot_msgs/ServoPosition.h"
00035 #include "corobot_msgs/ServoType.h"
00036 #include <diagnostic_updater/diagnostic_updater.h>
00037 #include <diagnostic_updater/publisher.h>
00038 #include <corobot_diagnostics/diagnostics.h>
00039 
00040 
00041 //Declare a servo handle
00042 CPhidgetAdvancedServoHandle servo = 0;
00043 
00044 // Servo position publisher
00045 ros::Publisher position_pub;
00046 
00047 // used for diagnostics purpose
00048 int servoError = 0; 
00049 
00050 
00054 int PositionChangeHandler(CPhidgetAdvancedServoHandle phid, void *userPtr, int index, double position)
00055 {
00056         if(position_pub)
00057         {
00058                 corobot_msgs::ServoPosition position_msg;
00059                 position_msg.index = index;
00060                 position_msg.position = position;
00061                 position_pub.publish(position_msg);
00062         }
00063         return 0;
00064 }
00065 
00071 void setPositionCallback(const corobot_msgs::ServoPosition &msg)
00072 {
00073         ROS_INFO("phidget_servo, servo: %d, angle: %f", msg.index, msg.position);
00074         int err = CPhidgetAdvancedServo_setPosition(servo, msg.index, msg.position);
00075         if (err != 0)
00076         {
00077                 ROS_ERROR("Could not set the servo motor number %d to the position %f",msg.index, msg.position);
00078                 servoError = 2;
00079         }
00080 }
00081 
00085 int ErrorHandler(CPhidgetHandle SERV, void *userptr, int ErrorCode, const char *Description)
00086 {
00087         ROS_ERROR("Error Phidget Servo motor %d : %s",ErrorCode, Description);
00088         return 0;
00089 }
00090 
00091 
00092 
00098 void setTypeCallback(const corobot_msgs::ServoType &msg)
00099 {       
00100         CPhidgetAdvancedServo_setServoType(servo, msg.index, (CPhidget_ServoType)msg.type);
00101 }
00102 
00103 
00107 void servo_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00108 {
00109         if (!servoError)  
00110                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "intialized");
00111         else if(servoError == 1)
00112         {
00113                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Phidget servo controller cannot be initialized");
00114                 stat.addf("Recommendation", PHIDGET_SERVO_ERROR_CONNECTION);
00115         }
00116         else if(servoError == 2)
00117         {
00118                 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "The arm cannot be moved");
00119                 stat.addf("Recommendation", ERROR_MOVING_ARM);
00120         }
00121 
00122 }
00123 
00124 
00125 int main(int argc, char* argv[])
00126 {
00127         ros::init(argc, argv, "phidget_servomotors");
00128         ros::NodeHandle n;
00129         ros::NodeHandle n_private("~");
00130         int serialNumber = -1;
00131         double minAccel, maxVel;
00132         int err;
00133 
00134         //create an updater that will send information on the diagnostics topics
00135         diagnostic_updater::Updater updater;
00136         updater.setHardwareIDf("Phidget");
00137         //function that will be executed with updater.update()
00138         updater.add("Servo", servo_diagnostic); 
00139 
00140 
00141         //create the servo object
00142         err = CPhidgetAdvancedServo_create(&servo);
00143         if (err != 0)
00144         {
00145                 ROS_ERROR("error create the Phidget servo controller device");
00146                 servoError = 1;
00147         }
00148 
00149         //Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
00150         CPhidget_set_OnError_Handler((CPhidgetHandle)servo, ErrorHandler, NULL);
00151 
00152         //Registers a callback that will run when the motor position is changed.
00153         //Requires the handle for the Phidget, the function that will be called, and an arbitrary pointer that will be supplied to the callback function (may be NULL).
00154         CPhidgetAdvancedServo_set_OnPositionChange_Handler(servo,PositionChangeHandler,NULL);
00155 
00156         //serial number of the phidget servo board, -1 if not specified
00157         n_private.param("serialNumber", serialNumber, -1); 
00158 
00159         //open the servo for device connections
00160         err = CPhidget_open((CPhidgetHandle)servo, serialNumber);
00161         if (err != 0)
00162         {
00163                 ROS_ERROR("error opening the Phidget servo controller device");
00164                 servoError = 1;
00165         }
00166         if((err = CPhidget_waitForAttachment((CPhidgetHandle)servo, 10000)))
00167         {
00168                 ROS_ERROR("Problem waiting for attachment of Phidget servo controller");
00169                 servoError = 1;
00170         }
00171 
00172         //Engage every servo to make sure they are powered on, setup the acceleration value and setup the default position value. 
00173         for(int i=0;i<8;i++)
00174         {
00175                 err = CPhidgetAdvancedServo_getAccelerationMin(servo, i, &minAccel);
00176                 if (err != 0)
00177                         ROS_ERROR("error getting the acceleration min of the servo motor number %d",i);
00178                 err = CPhidgetAdvancedServo_setAcceleration(servo, i, minAccel*2);
00179                 if (err != 0)
00180                         ROS_ERROR("error setting the acceleration of the servo motor number %d",i);
00181                 err = CPhidgetAdvancedServo_getVelocityMax(servo, 0, &maxVel);
00182                 if (err != 0)
00183                         ROS_ERROR("error getting the max velocity of the servo motor number %d",i);
00184                 err = CPhidgetAdvancedServo_setVelocityLimit(servo, 0, maxVel/2);
00185                 if (err != 0)
00186                         ROS_ERROR("error setting the lvelocity limit of the servo motor number %d",i);
00187                 err = CPhidgetAdvancedServo_setEngaged(servo,i,1);
00188                 if (err != 0)
00189                         ROS_ERROR("error engaging the servo motor number %d",i);
00190         }       
00191         
00192         //setup the default type of servo motors. It corresponds to 2 old corobot arms connected to the phidget servo board
00193         err = CPhidgetAdvancedServo_setServoType(servo, 0, PHIDGET_SERVO_HITEC_HS645MG);
00194         if (err != 0)
00195                 ROS_ERROR("error setting up the type of the servo motor number: 0");
00196         err = CPhidgetAdvancedServo_setServoType(servo, 1, PHIDGET_SERVO_HITEC_HS645MG);
00197         if (err != 0)
00198                 ROS_ERROR("error setting up the type of the servo motor number: 1");
00199         err = CPhidgetAdvancedServo_setServoType(servo, 2, PHIDGET_SERVO_HITEC_HS422);
00200         if (err != 0)
00201                 ROS_ERROR("error setting up the type of the servo motor number: 2");
00202         err = CPhidgetAdvancedServo_setServoType(servo, 3, PHIDGET_SERVO_HITEC_HS422);
00203         if (err != 0)
00204                 ROS_ERROR("error setting up the type of the servo motor number: 3");
00205         err = CPhidgetAdvancedServo_setServoType(servo, 4, PHIDGET_SERVO_HITEC_HS645MG);
00206         if (err != 0)
00207                 ROS_ERROR("error setting up the type of the servo motor number: 4");
00208         err = CPhidgetAdvancedServo_setServoType(servo, 5, PHIDGET_SERVO_HITEC_HS645MG);
00209         if (err != 0)
00210                 ROS_ERROR("error setting up the type of the servo motor number: 5");
00211         err = CPhidgetAdvancedServo_setServoType(servo, 6, PHIDGET_SERVO_HITEC_HS422);
00212         if (err != 0)
00213                 ROS_ERROR("error setting up the type of the servo motor number: 6");
00214         err = CPhidgetAdvancedServo_setServoType(servo, 7, PHIDGET_SERVO_HITEC_HS422);
00215         if (err != 0)
00216                 ROS_ERROR("error setting up the type of the servo motor number: 7");
00217 
00218 
00219 
00220         //Declare every topics
00221         position_pub = n.advertise<corobot_msgs::ServoPosition>("phidgetServo_getPosition", 100);
00222 
00223         //Subscribe to every necessary topics
00224         ros::Subscriber position_sub = n.subscribe("phidgetServo_setPosition",100, &setPositionCallback);
00225         ros::Subscriber type_sub = n.subscribe("phidgetServo_setType",100, &setTypeCallback);
00226         
00227         while (ros::ok())
00228             {
00229                 updater.update();
00230                 ros::spinOnce();
00231             }
00232 
00233 
00234         //Go back to the default position and disengage every servo motor before closing the servo handle
00235         for(int i=0;i<8;i++)
00236         {
00237                 err = CPhidgetAdvancedServo_setPosition (servo, i, 0.00);
00238                 err += CPhidgetAdvancedServo_setEngaged(servo,i,0);
00239                 if (err != 0)
00240                         ROS_ERROR("error closing the servo motor number : %d", i);
00241         }
00242 
00243         CPhidget_close((CPhidgetHandle)servo);
00244         CPhidget_delete((CPhidgetHandle)servo);
00245 
00246 }


phidget_servo
Author(s):
autogenerated on Sun Oct 5 2014 23:18:01