$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #ifndef RIQ_HAND_ETHERCAT_HARDWARE__RIQ_HAND_STATE_H 00036 #define RIQ_HAND_ETHERCAT_HARDWARE__RIQ_HAND_STATE_H 00037 00038 #include <stdint.h> 00039 00040 namespace riq_hand_ethercat_hardware 00041 { 00042 00043 00044 struct RIQGripperStatus 00045 { 00046 union { 00047 uint8_t raw_; 00048 struct { 00049 uint8_t initialization_ : 1; // When set to 0 will reset gripper, setting to 1 will initialize gripper 00050 uint8_t mode_ : 2; // Gripper grasp mode (Cylindrical, Pinch, Spheriod, Scissors) 00051 uint8_t grip_ : 2; // Current mode for gripper, Open,Close,Stop,Changing 00052 uint8_t status_ : 2; // Action status (Faulted, In progress, illegal/undefined, successfull) 00053 uint8_t reserved_ : 1; 00054 } __attribute__ ((__packed__)); 00055 } __attribute__ ((__packed__)); 00056 00057 enum {RESETTED=0, INITALIAZATION_COMPLETE=1}; 00058 enum {CYLINDRICAL_MODE=0, PINCH_MODE=1, SPHERIOD_MODE=2, SCISSORS_MODE=3}; 00059 enum {GRIP_STOPPED=0, GRIP_OPENED=1, GRIP_CLOSED=2, GRIP_INITIALIZING_OR_CHANGING=3}; 00060 enum {ACTION_FAULTED=0, ACTION_IN_PROGRESS=1, ACTION_ILLEGAL_UNDEFINED=2, ACTION_SUCCESSFUL=3}; 00061 00062 bool inScissorsMode() const {return (mode_ == SCISSORS_MODE);} 00063 00064 static const char* modeString(unsigned mode); 00065 static const char* gripString(unsigned grip); 00066 static const char* statusString(unsigned status); 00067 00068 const char* modeString() const {return modeString(mode_);} 00069 const char* gripString() const {return gripString(grip_);} 00070 const char* statusString() const {return statusString(status_);} 00071 00072 } __attribute__ ((__packed__)); 00073 00074 00075 00076 struct RIQObjectStatus 00077 { 00078 union { 00079 uint8_t raw_; 00080 struct { 00081 uint8_t detected_ : 2; 00082 uint8_t thumb_detected_ : 1; // Set to 1 when thumb detected object 00083 uint8_t right_finger_detected_ : 1; 00084 uint8_t left_finger_detected_ : 1; 00085 uint8_t scissors_detected_ : 1; 00086 uint8_t reserved_ : 2; 00087 } __attribute__ ((__packed__)); 00088 } __attribute__ ((__packed__)); 00089 00090 enum {OBJECT_NOT_DETECTED=0, 00091 OBJECT_DETECTED_1_FINGER=1, 00092 OBJECT_DETECTED_2_FINGERS=2, 00093 OBJECT_DETECTED_ALL_FINGERS=3}; 00094 00095 static const char* detectedString(unsigned detected, bool in_scissors_mode=false); 00096 static const char* wasDetectedString(bool was_detected); 00097 00098 const char* detectedString(bool in_scissors_mode=false) const {return detectedString(detected_, in_scissors_mode);} 00099 00100 } __attribute__ ((__packed__)); 00101 00102 00103 00104 struct RIQHandStateEcat 00105 { 00106 RIQGripperStatus gripper_status_; 00107 RIQObjectStatus object_status_; 00108 uint8_t reserved_; 00109 uint8_t fault_status_; 00110 uint8_t thumb_position_; 00111 uint8_t right_finger_position_; 00112 uint8_t left_finger_position_; 00113 uint8_t scissors_position_; 00114 uint8_t thumb_current_; 00115 uint8_t right_finger_current_; 00116 uint8_t left_finger_current_; 00117 uint8_t scissors_current_; 00118 00119 enum {NO_FAULT = 0x00, 00120 FAULT_NEED_INITIALIZATION_TO_COMPLETE=0x11, 00121 FAULT_NEED_MODE_CHANGE_TO_COMPLETE =0x12, 00122 FAULT_COMMUNICATION_TIMEOUT =0x21, 00123 FAULT_INSUFFICIENT_SUPPLY_VOLTAGE =0x22, 00124 FAULT_CHANGING_MODE_INTERFERENCE_SCISSORS = 0x31, 00125 FAULT_OPENING_INTERFERENCE_FINGERS = 0x32, 00126 FAULT_OPENING_INTERFERENCE_SCISSORS = 0x33, 00127 FAULT_CLOSING_INTERFERENCE_FINGERS = 0x34, 00128 FAULT_CLOSING_INTERFERENCE_SCISSORS = 0x35, 00129 FAULT_MAJOR_SCISSORS_DISPLACEMENT = 0x41, 00130 FAULT_MAJOR_FINGER_DISPLACEMENT = 0x42}; 00131 00133 static const char* faultString(unsigned fault); 00134 00136 const char* faultString() const {return faultString(fault_status_);} 00137 00139 static double convertPosition(unsigned position); 00140 00142 static double convertCurrent(unsigned current); 00143 00144 } __attribute__ ((__packed__)); 00145 00146 }; //namespace riq_hand_ethercat_hardware 00147 00148 00149 #endif /* RIQ_HAND_ETHERCAT_HARDWARE__RIQ_HAND_STATE_H */