Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ETHERCAT_HARDWARE__WG_SOFT_PROCESSOR_H
00036 #define ETHERCAT_HARDWARE__WG_SOFT_PROCESSOR_H
00037
00038 #include <ros/ros.h>
00039 #include "ethercat_hardware/ethercat_com.h"
00040 #include "ethercat_hardware/wg_mailbox.h"
00041 #include "ethercat_hardware/SoftProcessorFirmwareWrite.h"
00042 #include "ethercat_hardware/SoftProcessorFirmwareRead.h"
00043 #include "ethercat_hardware/SoftProcessorReset.h"
00044
00045 #include <vector>
00046 #include <ostream>
00047 #include <string>
00048
00049 namespace ethercat_hardware
00050 {
00051
00052
00057 class WGSoftProcessor
00058 {
00059 public:
00060 WGSoftProcessor();
00061 bool initialize(EthercatCom *com);
00062
00063 struct Info
00064 {
00065 Info() : mbx_(NULL), iram_address_(-1), ctrl_address_(-1) {}
00066 Info( WGMailbox *mbx,
00067 const std::string &actuator_name,
00068 const std::string &processor_name,
00069 unsigned iram_address, unsigned ctrl_address ) :
00070 mbx_(mbx), actuator_name_(actuator_name), processor_name_(processor_name),
00071 iram_address_(iram_address), ctrl_address_(ctrl_address) {}
00072 WGMailbox *mbx_;
00073 std::string actuator_name_;
00074 std::string processor_name_;
00075 unsigned iram_address_;
00076 unsigned ctrl_address_;
00077 };
00078
00079 void add(WGMailbox *mbx, const std::string &actuator_name, const std::string &processor_name,
00080 unsigned iram_address, unsigned ctrl_address);
00081
00082 protected:
00083
00084 static const unsigned IRAM_INSTRUCTION_LENGTH = 1024;
00085
00086 std::vector<Info> processors_;
00087
00088 bool writeFirmwareCB(ethercat_hardware::SoftProcessorFirmwareWrite::Request &request,
00089 ethercat_hardware::SoftProcessorFirmwareWrite::Response &response);
00090 bool readFirmwareCB(ethercat_hardware::SoftProcessorFirmwareRead::Request &request,
00091 ethercat_hardware::SoftProcessorFirmwareRead::Response &response);
00092 bool resetCB(ethercat_hardware::SoftProcessorReset::Request &request,
00093 ethercat_hardware::SoftProcessorReset::Response &response);
00094
00095 ros::ServiceServer read_firmware_service_;
00096 ros::ServiceServer write_firmware_service_;
00097 ros::ServiceServer reset_service_;
00098
00100 bool assertReset(const Info &info, std::ostream &err_msg);
00101
00103 bool releaseReset(const Info &info, std::ostream &err_msg);
00104
00106 const WGSoftProcessor::Info* get(const std::string &actuator_name, const std::string &processor_name, std::ostream &err_out) const;
00107
00108 EthercatCom *com_;
00109 };
00110
00111
00112 };
00113
00114 #endif //ETHERCAT_HARDWARE__WG_SOFT_PROCESSOR_H