fix robot control issues
This commit is contained in:
NuoDaJia
@@ -14,7 +14,7 @@
|
||||
|
||||
#ifndef ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
|
||||
#define ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
|
||||
|
||||
#include<thread>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
@@ -64,7 +64,7 @@ public:
|
||||
|
||||
ETHERCAT_DRIVER_PUBLIC
|
||||
hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
|
||||
|
||||
void watch_state();
|
||||
private:
|
||||
std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
|
||||
std::string & urdf, std::string component_name, std::string component_type);
|
||||
@@ -86,6 +86,7 @@ private:
|
||||
ethercat_interface::EcMaster master_;
|
||||
std::mutex ec_mutex_;
|
||||
bool activated_;
|
||||
|
||||
};
|
||||
} // namespace ethercat_driver
|
||||
|
||||
|
||||
@@ -13,7 +13,6 @@
|
||||
// limitations under the License.
|
||||
|
||||
#include "ethercat_driver/ethercat_driver.hpp"
|
||||
|
||||
#include <tinyxml2.h>
|
||||
#include <string>
|
||||
#include <regex>
|
||||
@@ -258,7 +257,29 @@ EthercatDriver::export_command_interfaces()
|
||||
}
|
||||
return command_interfaces;
|
||||
}
|
||||
|
||||
void EthercatDriver::watch_state(){
|
||||
int print_cnt=0;
|
||||
while(true){
|
||||
int xpos=master_.get_state(nullptr);
|
||||
if(xpos>-1){
|
||||
for (auto i = 0ul; i < ec_modules_.size(); i++) {
|
||||
uint16_t pos=std::stod(ec_module_parameters_[i]["position"]);
|
||||
master_.set_state(pos,"OP");
|
||||
}
|
||||
}
|
||||
if(xpos>-1){
|
||||
print_cnt+=1;
|
||||
if(print_cnt%10==0){
|
||||
std::cout<<"check state:"<<xpos<<" error"<<std::endl;
|
||||
}
|
||||
}
|
||||
else{
|
||||
std::cout<<"check state: all OK!!!!"<<std::endl;
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
}
|
||||
CallbackReturn EthercatDriver::on_activate(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/)
|
||||
{
|
||||
@@ -281,14 +302,18 @@ CallbackReturn EthercatDriver::on_activate(
|
||||
}
|
||||
|
||||
// start EC and wait until state operative
|
||||
|
||||
control_frequency_=100.0f;
|
||||
std::cout<<"force set freq"<<std::endl;
|
||||
master_.setCtrlFrequency(control_frequency_);
|
||||
|
||||
std::cout << "DRV: ----------abc-------------" << master_.getInterval() << std::endl;
|
||||
for (auto i = 0ul; i < ec_modules_.size(); i++) {
|
||||
uint16_t pos=std::stod(ec_module_parameters_[i]["position"]);
|
||||
master_.addSlave(
|
||||
std::stod(ec_module_parameters_[i]["alias"]),
|
||||
std::stod(ec_module_parameters_[i]["position"]),
|
||||
pos,
|
||||
ec_modules_[i].get());
|
||||
////std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
/////std::cout<<"delay 100ms"<<std::endl;
|
||||
}
|
||||
|
||||
// configure SDO
|
||||
@@ -323,14 +348,18 @@ CallbackReturn EthercatDriver::on_activate(
|
||||
t.tv_sec++;
|
||||
|
||||
bool running = true;
|
||||
///int dev_num=0;
|
||||
int try_num=0;
|
||||
while (running) {
|
||||
//while (try_num<5) {
|
||||
|
||||
try_num+=1;
|
||||
// wait until next shot
|
||||
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
|
||||
// update EtherCAT bus
|
||||
|
||||
master_.update();
|
||||
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
|
||||
|
||||
// check if operational
|
||||
bool isAllInit = true;
|
||||
for (auto & module : ec_modules_) {
|
||||
@@ -339,6 +368,11 @@ CallbackReturn EthercatDriver::on_activate(
|
||||
if (isAllInit) {
|
||||
running = false;
|
||||
}
|
||||
std::cout<<"start watch state thread......"<<std::endl;
|
||||
std::thread ws([this]() {
|
||||
this->watch_state();
|
||||
});
|
||||
ws.detach();
|
||||
// calculate next shot. carry over nanoseconds into microseconds.
|
||||
t.tv_nsec += master_.getInterval();
|
||||
while (t.tv_nsec >= 1000000000) {
|
||||
@@ -346,7 +380,7 @@ CallbackReturn EthercatDriver::on_activate(
|
||||
t.tv_sec++;
|
||||
}
|
||||
}
|
||||
|
||||
//////std::cout<<"###################:"<<running<<std::endl;
|
||||
RCLCPP_INFO(
|
||||
rclcpp::get_logger("EthercatDriver"), "System Successfully started!");
|
||||
|
||||
|
||||
@@ -0,0 +1,97 @@
|
||||
/* config.h. Generated from config.h.in by configure. */
|
||||
/* config.h.in. Generated from configure.ac by autoheader. */
|
||||
|
||||
/* Debug interfaces enabled */
|
||||
/* #undef EC_DEBUG_IF */
|
||||
|
||||
/* Debug ring enabled */
|
||||
/* #undef EC_DEBUG_RING */
|
||||
|
||||
/* EoE support enabled */
|
||||
/* #undef EC_EOE */
|
||||
|
||||
/* Use CPU timestamp counter */
|
||||
/* #undef EC_HAVE_CYCLES */
|
||||
|
||||
/* Use vendor id / product code wildcards */
|
||||
/* #undef EC_IDENT_WILDCARDS */
|
||||
|
||||
/* Max. number of Ethernet devices per master */
|
||||
#define EC_MAX_NUM_DEVICES 1
|
||||
|
||||
/* Read alias adresses from register */
|
||||
/* #undef EC_REGALIAS */
|
||||
|
||||
/* RTDM interface enabled */
|
||||
/* #undef EC_RTDM */
|
||||
|
||||
/* Output to syslog in RT context */
|
||||
#define EC_RT_SYSLOG 1
|
||||
|
||||
/* Assign SII to PDI */
|
||||
#define EC_SII_ASSIGN 1
|
||||
|
||||
/* Use hrtimer for scheduling */
|
||||
/* #undef EC_USE_HRTIMER */
|
||||
|
||||
/* Define to 1 if you have the <dlfcn.h> header file. */
|
||||
#define HAVE_DLFCN_H 1
|
||||
|
||||
/* Define to 1 if you have the <inttypes.h> header file. */
|
||||
#define HAVE_INTTYPES_H 1
|
||||
|
||||
/* Define to 1 if you have the <stdint.h> header file. */
|
||||
#define HAVE_STDINT_H 1
|
||||
|
||||
/* Define to 1 if you have the <stdio.h> header file. */
|
||||
#define HAVE_STDIO_H 1
|
||||
|
||||
/* Define to 1 if you have the <stdlib.h> header file. */
|
||||
#define HAVE_STDLIB_H 1
|
||||
|
||||
/* Define to 1 if you have the <strings.h> header file. */
|
||||
#define HAVE_STRINGS_H 1
|
||||
|
||||
/* Define to 1 if you have the <string.h> header file. */
|
||||
#define HAVE_STRING_H 1
|
||||
|
||||
/* Define to 1 if you have the <sys/stat.h> header file. */
|
||||
#define HAVE_SYS_STAT_H 1
|
||||
|
||||
/* Define to 1 if you have the <sys/types.h> header file. */
|
||||
#define HAVE_SYS_TYPES_H 1
|
||||
|
||||
/* Define to 1 if you have the <unistd.h> header file. */
|
||||
#define HAVE_UNISTD_H 1
|
||||
|
||||
/* Define to the sub-directory where libtool stores uninstalled libraries. */
|
||||
#define LT_OBJDIR ".libs/"
|
||||
|
||||
/* Name of package */
|
||||
#define PACKAGE "ethercat"
|
||||
|
||||
/* Define to the address where bug reports for this package should be sent. */
|
||||
#define PACKAGE_BUGREPORT "fp@igh.de"
|
||||
|
||||
/* Define to the full name of this package. */
|
||||
#define PACKAGE_NAME "ethercat"
|
||||
|
||||
/* Define to the full name and version of this package. */
|
||||
#define PACKAGE_STRING "ethercat 1.5.3"
|
||||
|
||||
/* Define to the one symbol short name of this package. */
|
||||
#define PACKAGE_TARNAME "ethercat"
|
||||
|
||||
/* Define to the home page for this package. */
|
||||
#define PACKAGE_URL ""
|
||||
|
||||
/* Define to the version of this package. */
|
||||
#define PACKAGE_VERSION "1.5.3"
|
||||
|
||||
/* Define to 1 if all of the C90 standard headers exist (not just the ones
|
||||
required in a freestanding environment). This macro is provided for
|
||||
backward compatibility; new code need not use it. */
|
||||
#define STDC_HEADERS 1
|
||||
|
||||
/* Version number of package */
|
||||
#define VERSION "1.5.3"
|
||||
@@ -33,7 +33,8 @@ class EcMaster
|
||||
public:
|
||||
explicit EcMaster(const int master = 0);
|
||||
virtual ~EcMaster();
|
||||
|
||||
int set_state(int pos,std::string sta);
|
||||
int get_state(uint16_t* res);
|
||||
/** \brief add a slave device to the master
|
||||
* alias and position can be found by running the following command
|
||||
* /opt/etherlab/bin$ sudo ./ethercat slaves
|
||||
@@ -82,7 +83,9 @@ public:
|
||||
|
||||
void setCtrlFrequency(double frequency)
|
||||
{
|
||||
std::cout << "set control frequency to " << frequency << std::endl;
|
||||
interval_ = 1000000000.0 / frequency;
|
||||
std::cout << "new interval_ " << interval_<< std::endl;
|
||||
}
|
||||
|
||||
uint32_t getInterval() {return interval_;}
|
||||
|
||||
@@ -0,0 +1,305 @@
|
||||
/******************************************************************************
|
||||
*
|
||||
* Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH
|
||||
*
|
||||
* This file is part of the IgH EtherCAT master.
|
||||
*
|
||||
* The file is free software; you can redistribute it and/or modify it under
|
||||
* the terms of the GNU Lesser General Public License as published by the
|
||||
* Free Software Foundation; version 2.1 of the License.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but WITHOUT
|
||||
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
||||
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
|
||||
* License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public License
|
||||
* along with this file. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*****************************************************************************/
|
||||
|
||||
/** \file
|
||||
* Global definitions and macros.
|
||||
*/
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#ifndef __EC_MASTER_GLOBALS_H__
|
||||
#define __EC_MASTER_GLOBALS_H__
|
||||
|
||||
#include "../globals.h"
|
||||
#include "../include/ecrt.h"
|
||||
|
||||
/******************************************************************************
|
||||
* EtherCAT master
|
||||
*****************************************************************************/
|
||||
|
||||
/** Datagram timeout in microseconds. */
|
||||
#define EC_IO_TIMEOUT 500
|
||||
|
||||
/** Time to send a byte in nanoseconds.
|
||||
*
|
||||
* t_ns = 1 / (100 MBit/s / 8 bit/byte) = 80 ns/byte
|
||||
*/
|
||||
#define EC_BYTE_TRANSMISSION_TIME_NS 80
|
||||
|
||||
/** Number of state machine retries on datagram timeout. */
|
||||
#define EC_FSM_RETRIES 3
|
||||
|
||||
/** Seconds to wait before fetching SDO dictionary
|
||||
after slave entered PREOP state. */
|
||||
#define EC_WAIT_SDO_DICT 3
|
||||
|
||||
/** Minimum size of a buffer used with ec_state_string(). */
|
||||
#define EC_STATE_STRING_SIZE 32
|
||||
|
||||
/** Maximum SII size in words, to avoid infinite reading. */
|
||||
#define EC_MAX_SII_SIZE 4096
|
||||
|
||||
/** Number of statistic rate intervals to maintain. */
|
||||
#define EC_RATE_COUNT 3
|
||||
|
||||
/******************************************************************************
|
||||
* EtherCAT protocol
|
||||
*****************************************************************************/
|
||||
|
||||
/** Size of an EtherCAT frame header. */
|
||||
#define EC_FRAME_HEADER_SIZE 2
|
||||
|
||||
/** Size of an EtherCAT datagram header. */
|
||||
#define EC_DATAGRAM_HEADER_SIZE 10
|
||||
|
||||
/** Size of an EtherCAT datagram footer. */
|
||||
#define EC_DATAGRAM_FOOTER_SIZE 2
|
||||
|
||||
/** Size of the EtherCAT address field. */
|
||||
#define EC_ADDR_LEN 4
|
||||
|
||||
/** Resulting maximum data size of a single datagram in a frame. */
|
||||
#define EC_MAX_DATA_SIZE (ETH_DATA_LEN - EC_FRAME_HEADER_SIZE \
|
||||
- EC_DATAGRAM_HEADER_SIZE - EC_DATAGRAM_FOOTER_SIZE)
|
||||
|
||||
/** Mailbox header size. */
|
||||
#define EC_MBOX_HEADER_SIZE 6
|
||||
|
||||
/** Word offset of first SII category. */
|
||||
#define EC_FIRST_SII_CATEGORY_OFFSET 0x40
|
||||
|
||||
/** Size of a sync manager configuration page. */
|
||||
#define EC_SYNC_PAGE_SIZE 8
|
||||
|
||||
/** Maximum number of FMMUs per slave. */
|
||||
#define EC_MAX_FMMUS 16
|
||||
|
||||
/** Size of an FMMU configuration page. */
|
||||
#define EC_FMMU_PAGE_SIZE 16
|
||||
|
||||
/** Number of DC sync signals. */
|
||||
#define EC_SYNC_SIGNAL_COUNT 2
|
||||
|
||||
/** Size of the datagram description string.
|
||||
*
|
||||
* This is also used as the maximum lenth of EoE device names.
|
||||
**/
|
||||
#define EC_DATAGRAM_NAME_SIZE 20
|
||||
|
||||
/** Slave state mask.
|
||||
*
|
||||
* Apply this mask to a slave state byte to get the slave state without
|
||||
* the error flag.
|
||||
*/
|
||||
#define EC_SLAVE_STATE_MASK 0x0F
|
||||
|
||||
/** State of an EtherCAT slave.
|
||||
*/
|
||||
typedef enum {
|
||||
EC_SLAVE_STATE_UNKNOWN = 0x00,
|
||||
/**< unknown state */
|
||||
EC_SLAVE_STATE_INIT = 0x01,
|
||||
/**< INIT state (no mailbox communication, no IO) */
|
||||
EC_SLAVE_STATE_PREOP = 0x02,
|
||||
/**< PREOP state (mailbox communication, no IO) */
|
||||
EC_SLAVE_STATE_BOOT = 0x03,
|
||||
/**< Bootstrap state (mailbox communication, firmware update) */
|
||||
EC_SLAVE_STATE_SAFEOP = 0x04,
|
||||
/**< SAFEOP (mailbox communication and input update) */
|
||||
EC_SLAVE_STATE_OP = 0x08,
|
||||
/**< OP (mailbox communication and input/output update) */
|
||||
EC_SLAVE_STATE_ACK_ERR = 0x10
|
||||
/**< Acknowledge/Error bit (no actual state) */
|
||||
} ec_slave_state_t;
|
||||
|
||||
/** Supported mailbox protocols.
|
||||
*/
|
||||
enum {
|
||||
EC_MBOX_AOE = 0x01, /**< ADS over EtherCAT */
|
||||
EC_MBOX_EOE = 0x02, /**< Ethernet over EtherCAT */
|
||||
EC_MBOX_COE = 0x04, /**< CANopen over EtherCAT */
|
||||
EC_MBOX_FOE = 0x08, /**< File-Access over EtherCAT */
|
||||
EC_MBOX_SOE = 0x10, /**< Servo-Profile over EtherCAT */
|
||||
EC_MBOX_VOE = 0x20 /**< Vendor specific */
|
||||
};
|
||||
|
||||
/** Slave information interface CANopen over EtherCAT details flags.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t enable_sdo : 1; /**< Enable SDO access. */
|
||||
uint8_t enable_sdo_info : 1; /**< SDO information service available. */
|
||||
uint8_t enable_pdo_assign : 1; /**< PDO mapping configurable. */
|
||||
uint8_t enable_pdo_configuration : 1; /**< PDO configuration possible. */
|
||||
uint8_t enable_upload_at_startup : 1; /**< ?. */
|
||||
uint8_t enable_sdo_complete_access : 1; /**< Complete access possible. */
|
||||
} ec_sii_coe_details_t;
|
||||
|
||||
/** Slave information interface general flags.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t enable_safeop : 1; /**< ?. */
|
||||
uint8_t enable_not_lrw : 1; /**< Slave does not support LRW. */
|
||||
} ec_sii_general_flags_t;
|
||||
|
||||
/** EtherCAT slave distributed clocks range.
|
||||
*/
|
||||
typedef enum {
|
||||
EC_DC_32, /**< 32 bit. */
|
||||
EC_DC_64 /*< 64 bit for system time, system time offset and
|
||||
port 0 receive time. */
|
||||
} ec_slave_dc_range_t;
|
||||
|
||||
/** EtherCAT slave sync signal configuration.
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t cycle_time; /**< Cycle time [ns]. */
|
||||
int32_t shift_time; /**< Shift time [ns]. */
|
||||
} ec_sync_signal_t;
|
||||
|
||||
/** Access states for SDO entries.
|
||||
*
|
||||
* The access rights are managed per AL state.
|
||||
*/
|
||||
enum {
|
||||
EC_SDO_ENTRY_ACCESS_PREOP, /**< Access rights in PREOP. */
|
||||
EC_SDO_ENTRY_ACCESS_SAFEOP, /**< Access rights in SAFEOP. */
|
||||
EC_SDO_ENTRY_ACCESS_OP, /**< Access rights in OP. */
|
||||
EC_SDO_ENTRY_ACCESS_COUNT /**< Number of states. */
|
||||
};
|
||||
|
||||
/** Master devices.
|
||||
*/
|
||||
typedef enum {
|
||||
EC_DEVICE_MAIN, /**< Main device. */
|
||||
EC_DEVICE_BACKUP /**< Backup device */
|
||||
} ec_device_index_t;
|
||||
|
||||
extern const char *ec_device_names[2]; // only main and backup!
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Convenience macro for printing EtherCAT-specific information to syslog.
|
||||
*
|
||||
* This will print the message in \a fmt with a prefixed "EtherCAT: ".
|
||||
*
|
||||
* \param fmt format string (like in printf())
|
||||
* \param args arguments (optional)
|
||||
*/
|
||||
#define EC_INFO(fmt, args...) \
|
||||
printk(KERN_INFO "EtherCAT: " fmt, ##args)
|
||||
|
||||
/** Convenience macro for printing EtherCAT-specific errors to syslog.
|
||||
*
|
||||
* This will print the message in \a fmt with a prefixed "EtherCAT ERROR: ".
|
||||
*
|
||||
* \param fmt format string (like in printf())
|
||||
* \param args arguments (optional)
|
||||
*/
|
||||
#define EC_ERR(fmt, args...) \
|
||||
printk(KERN_ERR "EtherCAT ERROR: " fmt, ##args)
|
||||
|
||||
/** Convenience macro for printing EtherCAT-specific warnings to syslog.
|
||||
*
|
||||
* This will print the message in \a fmt with a prefixed "EtherCAT WARNING: ".
|
||||
*
|
||||
* \param fmt format string (like in printf())
|
||||
* \param args arguments (optional)
|
||||
*/
|
||||
#define EC_WARN(fmt, args...) \
|
||||
printk(KERN_WARNING "EtherCAT WARNING: " fmt, ##args)
|
||||
|
||||
/** Convenience macro for printing EtherCAT debug messages to syslog.
|
||||
*
|
||||
* This will print the message in \a fmt with a prefixed "EtherCAT DEBUG: ".
|
||||
*
|
||||
* \param fmt format string (like in printf())
|
||||
* \param args arguments (optional)
|
||||
*/
|
||||
#define EC_DBG(fmt, args...) \
|
||||
printk(KERN_DEBUG "EtherCAT DEBUG: " fmt, ##args)
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Absolute value.
|
||||
*/
|
||||
#define EC_ABS(X) ((X) >= 0 ? (X) : -(X))
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
extern char *ec_master_version_str;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
unsigned int ec_master_count(void);
|
||||
void ec_print_data(const uint8_t *, size_t);
|
||||
void ec_print_data_diff(const uint8_t *, const uint8_t *, size_t);
|
||||
size_t ec_state_string(uint8_t, char *, uint8_t);
|
||||
ssize_t ec_mac_print(const uint8_t *, char *);
|
||||
int ec_mac_is_zero(const uint8_t *);
|
||||
|
||||
ec_master_t *ecrt_request_master_err(unsigned int);
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Code/Message pair.
|
||||
*
|
||||
* Some EtherCAT datagrams support reading a status code to display a certain
|
||||
* message. This type allows to map a code to a message string.
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t code; /**< Code. */
|
||||
const char *message; /**< Message belonging to \a code. */
|
||||
} ec_code_msg_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Generic request state.
|
||||
*
|
||||
* \attention If ever changing this, please be sure to adjust the \a
|
||||
* state_table in master/sdo_request.c.
|
||||
*/
|
||||
typedef enum {
|
||||
EC_INT_REQUEST_INIT,
|
||||
EC_INT_REQUEST_QUEUED,
|
||||
EC_INT_REQUEST_BUSY,
|
||||
EC_INT_REQUEST_SUCCESS,
|
||||
EC_INT_REQUEST_FAILURE
|
||||
} ec_internal_request_state_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
extern const ec_request_state_t ec_request_state_translation_table[];
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Origin type.
|
||||
*/
|
||||
typedef enum {
|
||||
EC_ORIG_INTERNAL, /**< Internal. */
|
||||
EC_ORIG_EXTERNAL /**< External. */
|
||||
} ec_origin_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct ec_slave ec_slave_t; /**< \see ec_slave. */
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,823 @@
|
||||
/******************************************************************************
|
||||
*
|
||||
* Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH
|
||||
*
|
||||
* This file is part of the IgH EtherCAT master.
|
||||
*
|
||||
* The file is free software; you can redistribute it and/or modify it under
|
||||
* the terms of the GNU Lesser General Public License as published by the
|
||||
* Free Software Foundation; version 2.1 of the License.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but WITHOUT
|
||||
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
||||
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
|
||||
* License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public License
|
||||
* along with this file. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*****************************************************************************/
|
||||
|
||||
/**
|
||||
\file
|
||||
EtherCAT master character device IOCTL commands.
|
||||
*/
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#ifndef __EC_IOCTL_H__
|
||||
#define __EC_IOCTL_H__
|
||||
|
||||
#include <linux/ioctl.h>
|
||||
|
||||
#include "globals.h"
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** \cond */
|
||||
|
||||
#define EC_IOCTL_TYPE 0xa4
|
||||
|
||||
#define EC_IO(nr) _IO(EC_IOCTL_TYPE, nr)
|
||||
#define EC_IOR(nr, type) _IOR(EC_IOCTL_TYPE, nr, type)
|
||||
#define EC_IOW(nr, type) _IOW(EC_IOCTL_TYPE, nr, type)
|
||||
#define EC_IOWR(nr, type) _IOWR(EC_IOCTL_TYPE, nr, type)
|
||||
|
||||
/** EtherCAT master ioctl() version magic.
|
||||
*
|
||||
* Increment this when changing the ioctl interface!
|
||||
*/
|
||||
#define EC_IOCTL_VERSION_MAGIC 33
|
||||
|
||||
// Command-line tool
|
||||
#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t)
|
||||
#define EC_IOCTL_MASTER EC_IOR(0x01, ec_ioctl_master_t)
|
||||
#define EC_IOCTL_SLAVE EC_IOWR(0x02, ec_ioctl_slave_t)
|
||||
#define EC_IOCTL_SLAVE_SYNC EC_IOWR(0x03, ec_ioctl_slave_sync_t)
|
||||
#define EC_IOCTL_SLAVE_SYNC_PDO EC_IOWR(0x04, ec_ioctl_slave_sync_pdo_t)
|
||||
#define EC_IOCTL_SLAVE_SYNC_PDO_ENTRY EC_IOWR(0x05, ec_ioctl_slave_sync_pdo_entry_t)
|
||||
#define EC_IOCTL_DOMAIN EC_IOWR(0x06, ec_ioctl_domain_t)
|
||||
#define EC_IOCTL_DOMAIN_FMMU EC_IOWR(0x07, ec_ioctl_domain_fmmu_t)
|
||||
#define EC_IOCTL_DOMAIN_DATA EC_IOWR(0x08, ec_ioctl_domain_data_t)
|
||||
#define EC_IOCTL_MASTER_DEBUG EC_IO(0x09)
|
||||
#define EC_IOCTL_MASTER_RESCAN EC_IO(0x0a)
|
||||
#define EC_IOCTL_SLAVE_STATE EC_IOW(0x0b, ec_ioctl_slave_state_t)
|
||||
#define EC_IOCTL_SLAVE_SDO EC_IOWR(0x0c, ec_ioctl_slave_sdo_t)
|
||||
#define EC_IOCTL_SLAVE_SDO_ENTRY EC_IOWR(0x0d, ec_ioctl_slave_sdo_entry_t)
|
||||
#define EC_IOCTL_SLAVE_SDO_UPLOAD EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t)
|
||||
#define EC_IOCTL_SLAVE_SDO_DOWNLOAD EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t)
|
||||
#define EC_IOCTL_SLAVE_SII_READ EC_IOWR(0x10, ec_ioctl_slave_sii_t)
|
||||
#define EC_IOCTL_SLAVE_SII_WRITE EC_IOW(0x11, ec_ioctl_slave_sii_t)
|
||||
#define EC_IOCTL_SLAVE_REG_READ EC_IOWR(0x12, ec_ioctl_slave_reg_t)
|
||||
#define EC_IOCTL_SLAVE_REG_WRITE EC_IOW(0x13, ec_ioctl_slave_reg_t)
|
||||
#define EC_IOCTL_SLAVE_FOE_READ EC_IOWR(0x14, ec_ioctl_slave_foe_t)
|
||||
#define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x15, ec_ioctl_slave_foe_t)
|
||||
#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x16, ec_ioctl_slave_soe_read_t)
|
||||
#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x17, ec_ioctl_slave_soe_write_t)
|
||||
#define EC_IOCTL_CONFIG EC_IOWR(0x18, ec_ioctl_config_t)
|
||||
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x19, ec_ioctl_config_pdo_t)
|
||||
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x1a, ec_ioctl_config_pdo_entry_t)
|
||||
#define EC_IOCTL_CONFIG_SDO EC_IOWR(0x1b, ec_ioctl_config_sdo_t)
|
||||
#define EC_IOCTL_CONFIG_IDN EC_IOWR(0x1c, ec_ioctl_config_idn_t)
|
||||
#define EC_IOCTL_CONFIG_FLAG EC_IOWR(0x1d, ec_ioctl_config_flag_t)
|
||||
#ifdef EC_EOE
|
||||
#define EC_IOCTL_EOE_HANDLER EC_IOWR(0x1e, ec_ioctl_eoe_handler_t)
|
||||
#endif
|
||||
|
||||
// Application interface
|
||||
#define EC_IOCTL_REQUEST EC_IO(0x1f)
|
||||
#define EC_IOCTL_CREATE_DOMAIN EC_IO(0x20)
|
||||
#define EC_IOCTL_CREATE_SLAVE_CONFIG EC_IOWR(0x21, ec_ioctl_config_t)
|
||||
#define EC_IOCTL_SELECT_REF_CLOCK EC_IOW(0x22, uint32_t)
|
||||
#define EC_IOCTL_ACTIVATE EC_IOR(0x23, ec_ioctl_master_activate_t)
|
||||
#define EC_IOCTL_DEACTIVATE EC_IO(0x24)
|
||||
#define EC_IOCTL_SEND EC_IO(0x25)
|
||||
#define EC_IOCTL_RECEIVE EC_IO(0x26)
|
||||
#define EC_IOCTL_MASTER_STATE EC_IOR(0x27, ec_master_state_t)
|
||||
#define EC_IOCTL_MASTER_LINK_STATE EC_IOWR(0x28, ec_ioctl_link_state_t)
|
||||
#define EC_IOCTL_APP_TIME EC_IOW(0x29, uint64_t)
|
||||
#define EC_IOCTL_SYNC_REF EC_IO(0x2a)
|
||||
#define EC_IOCTL_SYNC_REF_TO EC_IOW(0x2b, uint64_t)
|
||||
#define EC_IOCTL_SYNC_SLAVES EC_IO(0x2c)
|
||||
#define EC_IOCTL_REF_CLOCK_TIME EC_IOR(0x2d, uint32_t)
|
||||
#define EC_IOCTL_SYNC_MON_QUEUE EC_IO(0x2e)
|
||||
#define EC_IOCTL_SYNC_MON_PROCESS EC_IOR(0x2f, uint32_t)
|
||||
#define EC_IOCTL_RESET EC_IO(0x30)
|
||||
#define EC_IOCTL_SC_SYNC EC_IOW(0x31, ec_ioctl_config_t)
|
||||
#define EC_IOCTL_SC_WATCHDOG EC_IOW(0x32, ec_ioctl_config_t)
|
||||
#define EC_IOCTL_SC_ADD_PDO EC_IOW(0x33, ec_ioctl_config_pdo_t)
|
||||
#define EC_IOCTL_SC_CLEAR_PDOS EC_IOW(0x34, ec_ioctl_config_pdo_t)
|
||||
#define EC_IOCTL_SC_ADD_ENTRY EC_IOW(0x35, ec_ioctl_add_pdo_entry_t)
|
||||
#define EC_IOCTL_SC_CLEAR_ENTRIES EC_IOW(0x36, ec_ioctl_config_pdo_t)
|
||||
#define EC_IOCTL_SC_REG_PDO_ENTRY EC_IOWR(0x37, ec_ioctl_reg_pdo_entry_t)
|
||||
#define EC_IOCTL_SC_REG_PDO_POS EC_IOWR(0x38, ec_ioctl_reg_pdo_pos_t)
|
||||
#define EC_IOCTL_SC_DC EC_IOW(0x39, ec_ioctl_config_t)
|
||||
#define EC_IOCTL_SC_SDO EC_IOW(0x3a, ec_ioctl_sc_sdo_t)
|
||||
#define EC_IOCTL_SC_EMERG_SIZE EC_IOW(0x3b, ec_ioctl_sc_emerg_t)
|
||||
#define EC_IOCTL_SC_EMERG_POP EC_IOWR(0x3c, ec_ioctl_sc_emerg_t)
|
||||
#define EC_IOCTL_SC_EMERG_CLEAR EC_IOW(0x3d, ec_ioctl_sc_emerg_t)
|
||||
#define EC_IOCTL_SC_EMERG_OVERRUNS EC_IOWR(0x3e, ec_ioctl_sc_emerg_t)
|
||||
#define EC_IOCTL_SC_SDO_REQUEST EC_IOWR(0x3f, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SC_SOE_REQUEST EC_IOWR(0x40, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_SC_REG_REQUEST EC_IOWR(0x41, ec_ioctl_reg_request_t)
|
||||
#define EC_IOCTL_SC_VOE EC_IOWR(0x42, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_SC_STATE EC_IOWR(0x43, ec_ioctl_sc_state_t)
|
||||
#define EC_IOCTL_SC_IDN EC_IOW(0x44, ec_ioctl_sc_idn_t)
|
||||
#define EC_IOCTL_SC_FLAG EC_IOW(0x45, ec_ioctl_sc_flag_t)
|
||||
#define EC_IOCTL_DOMAIN_SIZE EC_IO(0x46)
|
||||
#define EC_IOCTL_DOMAIN_OFFSET EC_IO(0x47)
|
||||
#define EC_IOCTL_DOMAIN_PROCESS EC_IO(0x48)
|
||||
#define EC_IOCTL_DOMAIN_QUEUE EC_IO(0x49)
|
||||
#define EC_IOCTL_DOMAIN_STATE EC_IOWR(0x4a, ec_ioctl_domain_state_t)
|
||||
#define EC_IOCTL_SDO_REQUEST_INDEX EC_IOWR(0x4b, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SDO_REQUEST_TIMEOUT EC_IOWR(0x4c, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SDO_REQUEST_STATE EC_IOWR(0x4d, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SDO_REQUEST_READ EC_IOWR(0x4e, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SDO_REQUEST_WRITE EC_IOWR(0x4f, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SDO_REQUEST_DATA EC_IOWR(0x50, ec_ioctl_sdo_request_t)
|
||||
#define EC_IOCTL_SOE_REQUEST_IDN EC_IOWR(0x51, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_SOE_REQUEST_TIMEOUT EC_IOWR(0x52, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_SOE_REQUEST_STATE EC_IOWR(0x53, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_SOE_REQUEST_READ EC_IOWR(0x54, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_SOE_REQUEST_WRITE EC_IOWR(0x55, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_SOE_REQUEST_DATA EC_IOWR(0x56, ec_ioctl_soe_request_t)
|
||||
#define EC_IOCTL_REG_REQUEST_DATA EC_IOWR(0x57, ec_ioctl_reg_request_t)
|
||||
#define EC_IOCTL_REG_REQUEST_STATE EC_IOWR(0x58, ec_ioctl_reg_request_t)
|
||||
#define EC_IOCTL_REG_REQUEST_WRITE EC_IOWR(0x59, ec_ioctl_reg_request_t)
|
||||
#define EC_IOCTL_REG_REQUEST_READ EC_IOWR(0x5a, ec_ioctl_reg_request_t)
|
||||
#define EC_IOCTL_VOE_SEND_HEADER EC_IOW(0x5b, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_VOE_REC_HEADER EC_IOWR(0x5c, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_VOE_READ EC_IOW(0x5d, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_VOE_READ_NOSYNC EC_IOW(0x5e, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_VOE_WRITE EC_IOWR(0x5f, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_VOE_EXEC EC_IOWR(0x60, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_VOE_DATA EC_IOWR(0x61, ec_ioctl_voe_t)
|
||||
#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x62, size_t)
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#define EC_IOCTL_STRING_SIZE 64
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
uint32_t ioctl_version_magic;
|
||||
uint32_t master_count;
|
||||
} ec_ioctl_module_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
uint32_t slave_count;
|
||||
uint32_t config_count;
|
||||
uint32_t domain_count;
|
||||
uint32_t eoe_handler_count;
|
||||
uint8_t phase;
|
||||
uint8_t active;
|
||||
uint8_t scan_busy;
|
||||
struct ec_ioctl_device {
|
||||
uint8_t address[6];
|
||||
uint8_t attached;
|
||||
uint8_t link_state;
|
||||
uint64_t tx_count;
|
||||
uint64_t rx_count;
|
||||
uint64_t tx_bytes;
|
||||
uint64_t rx_bytes;
|
||||
uint64_t tx_errors;
|
||||
int32_t tx_frame_rates[EC_RATE_COUNT];
|
||||
int32_t rx_frame_rates[EC_RATE_COUNT];
|
||||
int32_t tx_byte_rates[EC_RATE_COUNT];
|
||||
int32_t rx_byte_rates[EC_RATE_COUNT];
|
||||
} devices[EC_MAX_NUM_DEVICES];
|
||||
uint32_t num_devices;
|
||||
uint64_t tx_count;
|
||||
uint64_t rx_count;
|
||||
uint64_t tx_bytes;
|
||||
uint64_t rx_bytes;
|
||||
int32_t tx_frame_rates[EC_RATE_COUNT];
|
||||
int32_t rx_frame_rates[EC_RATE_COUNT];
|
||||
int32_t tx_byte_rates[EC_RATE_COUNT];
|
||||
int32_t rx_byte_rates[EC_RATE_COUNT];
|
||||
int32_t loss_rates[EC_RATE_COUNT];
|
||||
uint64_t app_time;
|
||||
uint64_t dc_ref_time;
|
||||
uint16_t ref_clock;
|
||||
} ec_ioctl_master_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// input
|
||||
uint16_t position;
|
||||
|
||||
// outputs
|
||||
unsigned int device_index;
|
||||
uint32_t vendor_id;
|
||||
uint32_t product_code;
|
||||
uint32_t revision_number;
|
||||
uint32_t serial_number;
|
||||
uint16_t alias;
|
||||
uint16_t boot_rx_mailbox_offset;
|
||||
uint16_t boot_rx_mailbox_size;
|
||||
uint16_t boot_tx_mailbox_offset;
|
||||
uint16_t boot_tx_mailbox_size;
|
||||
uint16_t std_rx_mailbox_offset;
|
||||
uint16_t std_rx_mailbox_size;
|
||||
uint16_t std_tx_mailbox_offset;
|
||||
uint16_t std_tx_mailbox_size;
|
||||
uint16_t mailbox_protocols;
|
||||
uint8_t has_general_category;
|
||||
ec_sii_coe_details_t coe_details;
|
||||
ec_sii_general_flags_t general_flags;
|
||||
int16_t current_on_ebus;
|
||||
struct {
|
||||
ec_slave_port_desc_t desc;
|
||||
ec_slave_port_link_t link;
|
||||
uint32_t receive_time;
|
||||
uint16_t next_slave;
|
||||
uint32_t delay_to_next_dc;
|
||||
} ports[EC_MAX_PORTS];
|
||||
uint8_t fmmu_bit;
|
||||
uint8_t dc_supported;
|
||||
ec_slave_dc_range_t dc_range;
|
||||
uint8_t has_dc_system_time;
|
||||
uint32_t transmission_delay;
|
||||
uint8_t al_state;
|
||||
uint8_t error_flag;
|
||||
uint8_t sync_count;
|
||||
uint16_t sdo_count;
|
||||
uint32_t sii_nwords;
|
||||
char group[EC_IOCTL_STRING_SIZE];
|
||||
char image[EC_IOCTL_STRING_SIZE];
|
||||
char order[EC_IOCTL_STRING_SIZE];
|
||||
char name[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_slave_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint32_t sync_index;
|
||||
|
||||
// outputs
|
||||
uint16_t physical_start_address;
|
||||
uint16_t default_size;
|
||||
uint8_t control_register;
|
||||
uint8_t enable;
|
||||
uint8_t pdo_count;
|
||||
} ec_ioctl_slave_sync_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint32_t sync_index;
|
||||
uint32_t pdo_pos;
|
||||
|
||||
// outputs
|
||||
uint16_t index;
|
||||
uint8_t entry_count;
|
||||
int8_t name[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_slave_sync_pdo_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint32_t sync_index;
|
||||
uint32_t pdo_pos;
|
||||
uint32_t entry_pos;
|
||||
|
||||
// outputs
|
||||
uint16_t index;
|
||||
uint8_t subindex;
|
||||
uint8_t bit_length;
|
||||
int8_t name[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_slave_sync_pdo_entry_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t index;
|
||||
|
||||
// outputs
|
||||
uint32_t data_size;
|
||||
uint32_t logical_base_address;
|
||||
uint16_t working_counter[EC_MAX_NUM_DEVICES];
|
||||
uint16_t expected_working_counter;
|
||||
uint32_t fmmu_count;
|
||||
} ec_ioctl_domain_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t domain_index;
|
||||
uint32_t fmmu_index;
|
||||
|
||||
// outputs
|
||||
uint16_t slave_config_alias;
|
||||
uint16_t slave_config_position;
|
||||
uint8_t sync_index;
|
||||
ec_direction_t dir;
|
||||
uint32_t logical_address;
|
||||
uint32_t data_size;
|
||||
} ec_ioctl_domain_fmmu_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t domain_index;
|
||||
uint32_t data_size;
|
||||
uint8_t *target;
|
||||
} ec_ioctl_domain_data_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint8_t al_state;
|
||||
} ec_ioctl_slave_state_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint16_t sdo_position;
|
||||
|
||||
// outputs
|
||||
uint16_t sdo_index;
|
||||
uint8_t max_subindex;
|
||||
int8_t name[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_slave_sdo_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
int sdo_spec; // positive: index, negative: list position
|
||||
uint8_t sdo_entry_subindex;
|
||||
|
||||
// outputs
|
||||
uint16_t data_type;
|
||||
uint16_t bit_length;
|
||||
uint8_t read_access[EC_SDO_ENTRY_ACCESS_COUNT];
|
||||
uint8_t write_access[EC_SDO_ENTRY_ACCESS_COUNT];
|
||||
int8_t description[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_slave_sdo_entry_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint16_t sdo_index;
|
||||
uint8_t sdo_entry_subindex;
|
||||
size_t target_size;
|
||||
uint8_t *target;
|
||||
|
||||
// outputs
|
||||
size_t data_size;
|
||||
uint32_t abort_code;
|
||||
} ec_ioctl_slave_sdo_upload_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint16_t sdo_index;
|
||||
uint8_t sdo_entry_subindex;
|
||||
uint8_t complete_access;
|
||||
size_t data_size;
|
||||
uint8_t *data;
|
||||
|
||||
// outputs
|
||||
uint32_t abort_code;
|
||||
} ec_ioctl_slave_sdo_download_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint16_t offset;
|
||||
uint32_t nwords;
|
||||
uint16_t *words;
|
||||
} ec_ioctl_slave_sii_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint8_t emergency;
|
||||
uint16_t address;
|
||||
size_t size;
|
||||
uint8_t *data;
|
||||
} ec_ioctl_slave_reg_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint16_t offset;
|
||||
size_t buffer_size;
|
||||
uint8_t *buffer;
|
||||
|
||||
// outputs
|
||||
size_t data_size;
|
||||
uint32_t result;
|
||||
uint32_t error_code;
|
||||
char file_name[32];
|
||||
} ec_ioctl_slave_foe_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint8_t drive_no;
|
||||
uint16_t idn;
|
||||
size_t mem_size;
|
||||
uint8_t *data;
|
||||
|
||||
// outputs
|
||||
size_t data_size;
|
||||
uint16_t error_code;
|
||||
} ec_ioctl_slave_soe_read_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint16_t slave_position;
|
||||
uint8_t drive_no;
|
||||
uint16_t idn;
|
||||
size_t data_size;
|
||||
uint8_t *data;
|
||||
|
||||
// outputs
|
||||
uint16_t error_code;
|
||||
} ec_ioctl_slave_soe_write_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
|
||||
// outputs
|
||||
uint16_t alias;
|
||||
uint16_t position;
|
||||
uint32_t vendor_id;
|
||||
uint32_t product_code;
|
||||
struct {
|
||||
ec_direction_t dir;
|
||||
ec_watchdog_mode_t watchdog_mode;
|
||||
uint32_t pdo_count;
|
||||
uint8_t config_this;
|
||||
} syncs[EC_MAX_SYNC_MANAGERS];
|
||||
uint16_t watchdog_divider;
|
||||
uint16_t watchdog_intervals;
|
||||
uint32_t sdo_count;
|
||||
uint32_t idn_count;
|
||||
uint32_t flag_count;
|
||||
int32_t slave_position;
|
||||
uint16_t dc_assign_activate;
|
||||
ec_sync_signal_t dc_sync[EC_SYNC_SIGNAL_COUNT];
|
||||
} ec_ioctl_config_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint8_t sync_index;
|
||||
uint16_t pdo_pos;
|
||||
|
||||
// outputs
|
||||
uint16_t index;
|
||||
uint8_t entry_count;
|
||||
int8_t name[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_config_pdo_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint8_t sync_index;
|
||||
uint16_t pdo_pos;
|
||||
uint8_t entry_pos;
|
||||
|
||||
// outputs
|
||||
uint16_t index;
|
||||
uint8_t subindex;
|
||||
uint8_t bit_length;
|
||||
int8_t name[EC_IOCTL_STRING_SIZE];
|
||||
} ec_ioctl_config_pdo_entry_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Maximum size for displayed SDO data.
|
||||
* \todo Make this dynamic.
|
||||
*/
|
||||
#define EC_MAX_SDO_DATA_SIZE 1024
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint32_t sdo_pos;
|
||||
|
||||
// outputs
|
||||
uint16_t index;
|
||||
uint8_t subindex;
|
||||
size_t size;
|
||||
uint8_t data[EC_MAX_SDO_DATA_SIZE];
|
||||
uint8_t complete_access;
|
||||
} ec_ioctl_config_sdo_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Maximum size for displayed IDN data.
|
||||
* \todo Make this dynamic.
|
||||
*/
|
||||
#define EC_MAX_IDN_DATA_SIZE 1024
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint32_t idn_pos;
|
||||
|
||||
// outputs
|
||||
uint8_t drive_no;
|
||||
uint16_t idn;
|
||||
ec_al_state_t state;
|
||||
size_t size;
|
||||
uint8_t data[EC_MAX_IDN_DATA_SIZE];
|
||||
} ec_ioctl_config_idn_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** Maximum size for key.
|
||||
*/
|
||||
#define EC_MAX_FLAG_KEY_SIZE 128
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint32_t flag_pos;
|
||||
|
||||
// outputs
|
||||
char key[EC_MAX_FLAG_KEY_SIZE];
|
||||
int32_t value;
|
||||
} ec_ioctl_config_flag_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#ifdef EC_EOE
|
||||
|
||||
typedef struct {
|
||||
// input
|
||||
uint16_t eoe_index;
|
||||
|
||||
// outputs
|
||||
char name[EC_DATAGRAM_NAME_SIZE];
|
||||
uint16_t slave_position;
|
||||
uint8_t open;
|
||||
uint32_t rx_bytes;
|
||||
uint32_t rx_rate;
|
||||
uint32_t tx_bytes;
|
||||
uint32_t tx_rate;
|
||||
uint32_t tx_queued_frames;
|
||||
uint32_t tx_queue_size;
|
||||
} ec_ioctl_eoe_handler_t;
|
||||
|
||||
#endif
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// outputs
|
||||
void *process_data;
|
||||
size_t process_data_size;
|
||||
} ec_ioctl_master_activate_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint16_t pdo_index;
|
||||
uint16_t entry_index;
|
||||
uint8_t entry_subindex;
|
||||
uint8_t entry_bit_length;
|
||||
} ec_ioctl_add_pdo_entry_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint16_t entry_index;
|
||||
uint8_t entry_subindex;
|
||||
uint32_t domain_index;
|
||||
|
||||
// outputs
|
||||
unsigned int bit_position;
|
||||
} ec_ioctl_reg_pdo_entry_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint32_t sync_index;
|
||||
uint32_t pdo_pos;
|
||||
uint32_t entry_pos;
|
||||
uint32_t domain_index;
|
||||
|
||||
// outputs
|
||||
unsigned int bit_position;
|
||||
} ec_ioctl_reg_pdo_pos_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint16_t index;
|
||||
uint8_t subindex;
|
||||
const uint8_t *data;
|
||||
size_t size;
|
||||
uint8_t complete_access;
|
||||
} ec_ioctl_sc_sdo_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
size_t size;
|
||||
uint8_t *target;
|
||||
|
||||
// outputs
|
||||
int32_t overruns;
|
||||
} ec_ioctl_sc_emerg_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
|
||||
// outputs
|
||||
ec_slave_config_state_t *state;
|
||||
} ec_ioctl_sc_state_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
uint8_t drive_no;
|
||||
uint16_t idn;
|
||||
ec_al_state_t al_state;
|
||||
const uint8_t *data;
|
||||
size_t size;
|
||||
} ec_ioctl_sc_idn_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
size_t key_size;
|
||||
char *key;
|
||||
int32_t value;
|
||||
} ec_ioctl_sc_flag_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t domain_index;
|
||||
|
||||
// outputs
|
||||
ec_domain_state_t *state;
|
||||
} ec_ioctl_domain_state_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
|
||||
// inputs/outputs
|
||||
uint32_t request_index;
|
||||
uint16_t sdo_index;
|
||||
uint8_t sdo_subindex;
|
||||
size_t size;
|
||||
uint8_t *data;
|
||||
uint32_t timeout;
|
||||
ec_request_state_t state;
|
||||
} ec_ioctl_sdo_request_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
|
||||
// inputs/outputs
|
||||
uint32_t request_index;
|
||||
uint8_t drive_no;
|
||||
uint16_t idn;
|
||||
size_t size;
|
||||
uint8_t *data;
|
||||
uint32_t timeout;
|
||||
ec_request_state_t state;
|
||||
} ec_ioctl_soe_request_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
size_t mem_size;
|
||||
|
||||
// inputs/outputs
|
||||
uint32_t request_index;
|
||||
uint8_t *data;
|
||||
ec_request_state_t state;
|
||||
uint8_t new_data;
|
||||
uint16_t address;
|
||||
size_t transfer_size;
|
||||
} ec_ioctl_reg_request_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t config_index;
|
||||
|
||||
// inputs/outputs
|
||||
uint32_t voe_index;
|
||||
uint32_t *vendor_id;
|
||||
uint16_t *vendor_type;
|
||||
size_t size;
|
||||
uint8_t *data;
|
||||
ec_request_state_t state;
|
||||
} ec_ioctl_voe_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
typedef struct {
|
||||
// inputs
|
||||
uint32_t dev_idx;
|
||||
|
||||
// outputs
|
||||
ec_master_link_state_t *state;
|
||||
} ec_ioctl_link_state_t;
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#ifdef __KERNEL__
|
||||
|
||||
/** Context data structure for file handles.
|
||||
*/
|
||||
typedef struct {
|
||||
unsigned int writable; /**< Device was opened with write permission. */
|
||||
unsigned int requested; /**< Master was requested via this file handle. */
|
||||
uint8_t *process_data; /**< Total process data area. */
|
||||
size_t process_data_size; /**< Size of the \a process_data. */
|
||||
} ec_ioctl_context_t;
|
||||
|
||||
long ec_ioctl(ec_master_t *, ec_ioctl_context_t *, unsigned int,
|
||||
void __user *);
|
||||
|
||||
#ifdef EC_RTDM
|
||||
|
||||
long ec_ioctl_rtdm(ec_master_t *, ec_ioctl_context_t *, unsigned int,
|
||||
void __user *);
|
||||
int ec_rtdm_mmap(ec_ioctl_context_t *, void **);
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/** \endcond */
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,55 @@
|
||||
/******************************************************************************
|
||||
*
|
||||
* Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH
|
||||
*
|
||||
* This file is part of the IgH EtherCAT master.
|
||||
*
|
||||
* The file is free software; you can redistribute it and/or modify it under
|
||||
* the terms of the GNU Lesser General Public License as published by the
|
||||
* Free Software Foundation; version 2.1 of the License.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but WITHOUT
|
||||
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
||||
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
|
||||
* License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public License
|
||||
* along with this file. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*****************************************************************************/
|
||||
|
||||
/**
|
||||
\file
|
||||
Global definitions and macros.
|
||||
*/
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#ifndef __EC_GLOBALS_H__
|
||||
#define __EC_GLOBALS_H__
|
||||
|
||||
#include "config.h"
|
||||
|
||||
/******************************************************************************
|
||||
* Overall macros
|
||||
*****************************************************************************/
|
||||
|
||||
/** Helper macro for EC_STR(), literates a macro argument.
|
||||
*
|
||||
* \param X argument to literate.
|
||||
*/
|
||||
#define EC_LIT(X) #X
|
||||
|
||||
/** Converts a macro argument to a string.
|
||||
*
|
||||
* \param X argument to stringify.
|
||||
*/
|
||||
#define EC_STR(X) EC_LIT(X)
|
||||
|
||||
/** Master version string
|
||||
*/
|
||||
#define EC_MASTER_VERSION VERSION " " EC_STR(REV)
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
#endif
|
||||
@@ -14,7 +14,7 @@
|
||||
|
||||
#include "ethercat_interface/ec_master.hpp"
|
||||
#include "ethercat_interface/ec_slave.hpp"
|
||||
|
||||
#include "ethercat_interface/ioctl.h"
|
||||
#include <unistd.h>
|
||||
#include <sys/resource.h>
|
||||
#include <pthread.h>
|
||||
@@ -25,7 +25,8 @@
|
||||
#include <string.h>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <fcntl.h>
|
||||
#define EC_NEWTIMEVAL2NANO(TV) \
|
||||
(((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec)
|
||||
|
||||
@@ -90,9 +91,7 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave)
|
||||
printWarning("Add slave. Failed to get slave configuration.");
|
||||
return;
|
||||
}
|
||||
|
||||
// check and setup dc
|
||||
|
||||
if (slave->assign_activate_dc_sync()) {
|
||||
struct timespec t;
|
||||
clock_gettime(CLOCK_MONOTONIC, &t);
|
||||
@@ -138,12 +137,12 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave)
|
||||
domain_info = new DomainInfo(master_);
|
||||
domain_info_[domain_index] = domain_info;
|
||||
}
|
||||
|
||||
registerPDOInDomain(
|
||||
alias, position,
|
||||
iter.second, domain_info,
|
||||
slave);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int EcMaster::configSlaveSdo(
|
||||
@@ -214,7 +213,39 @@ void EcMaster::registerPDOInDomain(
|
||||
ec_pdo_entry_reg_t empty = {0};
|
||||
domain_info->domain_regs.back() = empty;
|
||||
}
|
||||
int EcMaster::set_state(int pos,std::string sta){
|
||||
|
||||
uint8_t state=0;
|
||||
if (sta == "INIT") {
|
||||
state = 0x01;
|
||||
} else if (sta == "PREOP") {
|
||||
state = 0x02;
|
||||
} else if (sta == "BOOT") {
|
||||
state = 0x03;
|
||||
} else if (sta == "SAFEOP") {
|
||||
state = 0x04;
|
||||
} else if (sta == "OP") {
|
||||
state = 0x08;
|
||||
} else {
|
||||
std::cout << "Invalid state "<<std::endl;
|
||||
}
|
||||
|
||||
int fd=-1;
|
||||
if ((fd = ::open("/dev/EtherCAT0",O_RDWR)) == -1) {
|
||||
std::cout<<"open eth err"<<std::endl;
|
||||
}else{
|
||||
ec_ioctl_slave_state_t data;
|
||||
data.slave_position = pos;
|
||||
data.al_state = state;
|
||||
if (ioctl(fd, EC_IOCTL_SLAVE_STATE, &data)) {
|
||||
std::cout << "Failed to request slave state: "<<std::endl;
|
||||
if (errno == EINVAL)
|
||||
std::cout << "Slave " << pos << " does not exist!"<<std::endl;
|
||||
}
|
||||
::close(fd);
|
||||
}
|
||||
|
||||
}
|
||||
bool EcMaster::activate()
|
||||
{
|
||||
// register domain
|
||||
@@ -482,7 +513,23 @@ void EcMaster::checkMasterState()
|
||||
}
|
||||
master_state_ = ms;
|
||||
}
|
||||
|
||||
int EcMaster::get_state(uint16_t* res){
|
||||
int idx=0;
|
||||
int pos=-1;
|
||||
for (SlaveInfo & slave : slave_info_) {
|
||||
ec_slave_config_state_t s;
|
||||
ecrt_slave_config_state(slave.config, &s);
|
||||
///res[idx]=s.al_state;
|
||||
ec_slave_config_t* sc=slave.config;
|
||||
////std::cout<<idx<<" AAAAA "<<s.al_state<<std::endl;
|
||||
if(s.al_state!=EC_SLAVE_STATE_OP){
|
||||
pos=idx;
|
||||
break;
|
||||
}
|
||||
idx+=1;
|
||||
}
|
||||
return pos;
|
||||
}
|
||||
|
||||
void EcMaster::checkSlaveStates()
|
||||
{
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 500 # Hz
|
||||
update_rate: 100 # Hz
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
@@ -27,14 +27,14 @@ gpio_command_controller:
|
||||
- joint_1
|
||||
- joint_2
|
||||
- joint_3
|
||||
# - joint_4
|
||||
# - joint_5
|
||||
# - joint_6
|
||||
# - joint_7
|
||||
# - joint_8
|
||||
# - joint_9
|
||||
# - joint_10
|
||||
# - joint_11
|
||||
- joint_4
|
||||
- joint_5
|
||||
- joint_6
|
||||
- joint_7
|
||||
- joint_8
|
||||
- joint_9
|
||||
- joint_10
|
||||
- joint_11
|
||||
command_interfaces:
|
||||
joint_1:
|
||||
- interfaces:
|
||||
@@ -51,43 +51,43 @@ gpio_command_controller:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
# joint_4:
|
||||
# - interfaces:
|
||||
# # - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_5:
|
||||
# - interfaces:
|
||||
# # - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_6:
|
||||
# - interfaces:
|
||||
# # - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_7:
|
||||
# - interfaces:
|
||||
# # - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_8:
|
||||
# - interfaces:
|
||||
# - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_9:
|
||||
# - interfaces:
|
||||
# - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_10:
|
||||
# - interfaces:
|
||||
# - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
# joint_11:
|
||||
# - interfaces:
|
||||
# - reset_fault
|
||||
# - enable
|
||||
# - position
|
||||
joint_4:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_5:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_6:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_7:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_8:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_9:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_10:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
joint_11:
|
||||
- interfaces:
|
||||
- reset_fault
|
||||
- enable
|
||||
- position
|
||||
|
||||
@@ -14,9 +14,9 @@ rpdo: #PxPDO = receive PDO Mapping
|
||||
- index: 0x1600
|
||||
channels:
|
||||
- {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} #target position
|
||||
- {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0} #target velocity
|
||||
- {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0} #target torque
|
||||
- {index: 0x6072, sub_index: 0, type: uint16, default: 500} # Max torque
|
||||
# - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0} #target velocity
|
||||
# - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0} #target torque
|
||||
# - {index: 0x6072, sub_index: 0, type: uint16, default: 500} # Max torque
|
||||
- {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
|
||||
- {index: 0x6060, sub_index: 0, type: uint8, default: 8} # Control operation
|
||||
- {index: 0xf0ff, sub_index: 0, type: uint8} # Dummy byte
|
||||
@@ -25,10 +25,10 @@ tpdo: #TxPDO = transmit PDO Mapping
|
||||
- index: 0x1a00
|
||||
channels:
|
||||
- {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value
|
||||
- {index: 0x60F4, sub_index: 0, type: int32} #Position Following error actual value
|
||||
# - {index: 0x60F4, sub_index: 0, type: int32} #Position Following error actual value
|
||||
- {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 } # Velocity actual value
|
||||
- {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 } # Torque actual value
|
||||
# - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 } # Torque actual value
|
||||
- {index: 0x6041, sub_index: 0, type: uint16} # State word
|
||||
- {index: 0x603f, sub_index: 0, type: uint16} # Error code
|
||||
# - {index: 0x603f, sub_index: 0, type: uint16} # Error code
|
||||
- {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation} # Mode of operation display
|
||||
- {index: 0xf0ff, sub_index: 0, type: uint8} # dummy byte
|
||||
@@ -3,7 +3,7 @@
|
||||
|
||||
<!-- 公共参数定义（一次修改，全局生效） -->
|
||||
<xacro:property name="master_id" value="0" />
|
||||
<xacro:property name="control_frequency" value="500" />
|
||||
<xacro:property name="control_frequency" value="100" />
|
||||
<xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
|
||||
<xacro:property name="alias" value="0" />
|
||||
<xacro:property name="mode_of_operation" value="8" />
|
||||
@@ -16,7 +16,7 @@
|
||||
<!-- 状态接口（位置/速度/力矩） -->
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
<!-- <state_interface name="effort"/> -->
|
||||
|
||||
<!-- 命令接口（位置控制/故障重置/使能） -->
|
||||
<command_interface name="position"/>
|
||||
@@ -47,14 +47,14 @@
|
||||
<xacro:single_joint_config joint_name="joint_1" ec_position="1" />
|
||||
<xacro:single_joint_config joint_name="joint_2" ec_position="2" />
|
||||
<xacro:single_joint_config joint_name="joint_3" ec_position="3" />
|
||||
<!-- <xacro:single_joint_config joint_name="joint_4" ec_position="4" />
|
||||
<xacro:single_joint_config joint_name="joint_4" ec_position="4" />
|
||||
<xacro:single_joint_config joint_name="joint_5" ec_position="5" />
|
||||
<xacro:single_joint_config joint_name="joint_6" ec_position="6" /> -->
|
||||
<!-- <xacro:single_joint_config joint_name="joint_7" ec_position="7" />
|
||||
<xacro:single_joint_config joint_name="joint_6" ec_position="6" />
|
||||
<xacro:single_joint_config joint_name="joint_7" ec_position="7" />
|
||||
<xacro:single_joint_config joint_name="joint_8" ec_position="8" />
|
||||
<xacro:single_joint_config joint_name="joint_9" ec_position="9" />
|
||||
<xacro:single_joint_config joint_name="joint_10" ec_position="10" />
|
||||
<xacro:single_joint_config joint_name="joint_11" ec_position="11" /> -->
|
||||
<xacro:single_joint_config joint_name="joint_11" ec_position="11" />
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
@@ -10,12 +10,22 @@ namespace robot_control
|
||||
class ArmControl
|
||||
{
|
||||
public:
|
||||
ArmControl(size_t total_joints, double length1, double length2, double maxSpeed, double maxAcc, const std::vector<double>& minLimits, const std::vector<double>& maxLimits, const std::vector<double>& home_positions);
|
||||
ArmControl(
|
||||
size_t total_joints,
|
||||
double length1,
|
||||
double length2,
|
||||
double maxSpeed,
|
||||
double maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
);
|
||||
~ArmControl();
|
||||
|
||||
void MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration);
|
||||
bool MoveToTargetPoint(std::vector<double>& joint_positions, const Eigen::Vector3d& target_pos, double duration);
|
||||
|
||||
void MoveToTargetJoint(const std::vector<double>& target_joint, double duration);
|
||||
bool MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double duration);
|
||||
|
||||
bool GoHome(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
@@ -34,9 +44,14 @@ namespace robot_control
|
||||
double maxSpeed_; // 最大速度
|
||||
double maxAcc_; // 最大加速度
|
||||
|
||||
double stopDurationTime_;
|
||||
double movingDurationTime_;
|
||||
|
||||
TrapezoidalTrajectory* trapezoidalTrajectory_; // 轨迹规划器
|
||||
|
||||
std::vector<double> joint_home_positions_;
|
||||
std::vector<int> joint_directions_;
|
||||
|
||||
std::vector<double> joint_position_; // 机械臂关节位置
|
||||
std::vector<double> joint_velocity_; // 机械臂关节速度
|
||||
std::vector<double> joint_acceleration_; // 机械臂关节加速度
|
||||
|
||||
@@ -7,6 +7,8 @@ namespace robot_control
|
||||
* @brief 机器人状态枚举（全局通用）
|
||||
*/
|
||||
enum class RobotState {
|
||||
INIT,
|
||||
STOPPING, // 停止过程中
|
||||
STOPPED, // 停止
|
||||
MOVING_FORWARD, // 前进
|
||||
MOVING_BACKWARD, // 后退
|
||||
|
||||
@@ -15,7 +15,17 @@ namespace robot_control
|
||||
class LegControl
|
||||
{
|
||||
public:
|
||||
LegControl(size_t totalJoints,double LegLength, double maxSpeed, double maxAcc, const std::vector<double>& minLimits, const std::vector<double>& maxLimits, const std::vector<double>& home_positions);
|
||||
LegControl(
|
||||
size_t totalJoints,
|
||||
double LegLength,
|
||||
double maxSpeed,
|
||||
double maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
);
|
||||
|
||||
~LegControl();
|
||||
|
||||
bool MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt);
|
||||
@@ -35,27 +45,26 @@ namespace robot_control
|
||||
private:
|
||||
|
||||
size_t total_joints_;
|
||||
|
||||
double maxSpeed_;
|
||||
|
||||
double maxAcc_;
|
||||
|
||||
double LegLength_;
|
||||
|
||||
bool is_moving_;
|
||||
|
||||
bool is_stopping_;
|
||||
double stopDurationTime_;
|
||||
double movingDurationTime_;
|
||||
|
||||
TrapezoidalTrajectory* trapezoidalTrajectory_; // 轨迹规划器
|
||||
|
||||
std::vector<double> joint_home_positions_;
|
||||
std::vector<int> joint_directions_;
|
||||
|
||||
std::vector<double> joint_position_; // 机械臂关节位置
|
||||
std::vector<double> joint_velocity_; // 机械臂关节速度
|
||||
std::vector<double> joint_acceleration_; // 机械臂关节加速度
|
||||
std::vector<double> joint_torque_; // 机械臂关节力矩
|
||||
|
||||
std::vector<double> maxLimits_;
|
||||
std::vector<double> minLimits_;
|
||||
std::vector<double> maxLimits_;
|
||||
|
||||
std::vector<double> joint_position_desired_; // 机械臂关节期望位置
|
||||
|
||||
|
||||
@@ -56,12 +56,12 @@ namespace robot_control
|
||||
// 初始化关节索引
|
||||
wheel_joint_indices_ = {0, 1, 2, 3};
|
||||
pitch_body_indices_ = {0};
|
||||
left_leg_joint_indices_ = {1};
|
||||
right_leg_joint_indices_ = {2};
|
||||
left_arm_joint_indices_ = {3, 4, 5, 6};
|
||||
right_arm_joint_indices_ = {7, 8, 9, 10};
|
||||
right_leg_joint_indices_ = {1};
|
||||
left_leg_joint_indices_ = {2};
|
||||
right_arm_joint_indices_ = {3, 4, 5, 6};
|
||||
left_arm_joint_indices_ = {7, 8, 9, 10};
|
||||
|
||||
total_joints_ = 3 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size();
|
||||
total_joints_ = 11 ; //left_arm_joint_indices_.size() + right_arm_joint_indices_.size() + pitch_body_indices_.size() + left_leg_joint_indices_.size() + right_leg_joint_indices_.size();
|
||||
|
||||
// 初始化关节方向
|
||||
wheel_joint_directions_ = {1, -1, 1, -1};
|
||||
@@ -69,7 +69,7 @@ namespace robot_control
|
||||
right_arm_joint_directions_ = {1, 1, 1, 1};
|
||||
pitch_body_directions_ = {1};
|
||||
left_leg_joint_directions_ = {1};
|
||||
right_leg_joint_directions_ = {1};
|
||||
right_leg_joint_directions_ = {-1};
|
||||
leg_wheel_directions_ = {1, -1, -1, 1}; // 腿部 和 腰部的耦合方向
|
||||
|
||||
// 初始化关节状态
|
||||
@@ -95,6 +95,15 @@ namespace robot_control
|
||||
JointLimit(10, 360.0, -360.0, LimitType::POSITION)
|
||||
};
|
||||
|
||||
left_leg_max_limit_.resize(left_leg_joint_indices_.size());
|
||||
left_leg_min_limit_.resize(left_leg_joint_indices_.size());
|
||||
right_leg_max_limit_.resize(right_leg_joint_indices_.size());
|
||||
right_leg_min_limit_.resize(right_leg_joint_indices_.size());
|
||||
left_arm_max_limit_.resize(left_arm_joint_indices_.size());
|
||||
left_arm_min_limit_.resize(left_arm_joint_indices_.size());
|
||||
right_arm_max_limit_.resize(right_arm_joint_indices_.size());
|
||||
right_arm_min_limit_.resize(right_arm_joint_indices_.size());
|
||||
|
||||
for (size_t i = 0; i < left_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
left_leg_max_limit_[i] = joint_limits_[left_leg_joint_indices_[i]].max;
|
||||
@@ -145,6 +154,7 @@ namespace robot_control
|
||||
|
||||
serial_port_ = "/dev/ttyUSB0";
|
||||
baud_rate_ = 115200;
|
||||
|
||||
};
|
||||
|
||||
// 运动参数
|
||||
|
||||
@@ -26,7 +26,7 @@ namespace robot_control
|
||||
void TurnRight();
|
||||
void MoveUp(double dt);
|
||||
void MoveDown(double dt);
|
||||
void Stop(double dt);
|
||||
bool Stop(double dt);
|
||||
bool GoHome(double dt);
|
||||
// void ArmGrab(double dt);
|
||||
|
||||
@@ -41,6 +41,7 @@ namespace robot_control
|
||||
void init();
|
||||
|
||||
MotionParameters motionParams_;
|
||||
bool isMoving_;
|
||||
|
||||
// 控制器
|
||||
ArmControl* leftArmController_;
|
||||
@@ -55,7 +56,7 @@ namespace robot_control
|
||||
std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
|
||||
std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
|
||||
|
||||
bool robotInited_; // 机器人是否已经初始化
|
||||
std::vector<bool> jointInited_; // 机器人是否已经初始化
|
||||
std::vector<bool> robotHomed_; // 关节是否已经复位
|
||||
|
||||
std::vector<double> tempLeftArmCmd;
|
||||
|
||||
@@ -62,6 +62,9 @@ namespace robot_control
|
||||
bool isPaused_;
|
||||
bool isFinished_;
|
||||
bool isRobotEnabled_;
|
||||
bool enableCommandExecuted_;
|
||||
|
||||
int jogDirection_;
|
||||
|
||||
// 状态转换表
|
||||
std::map<std::pair<RobotState, ControlCommand>, RobotState> transitionTable_;
|
||||
|
||||
@@ -106,6 +106,8 @@ private:
|
||||
void calculateTrajectoryParams();
|
||||
|
||||
std::vector<double> start_pos_; // 起始位置
|
||||
std::vector<double> stop_pos_;
|
||||
std::vector<double> stop_vel_;
|
||||
std::vector<double> target_pos_; // 目标位置
|
||||
std::vector<double> current_pos_; // 当前位置
|
||||
std::vector<double> current_velocity_; // 当前速度
|
||||
|
||||
@@ -5,16 +5,28 @@
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
ArmControl::ArmControl(size_t total_joints, double length1, double length2, double maxSpeed, double maxAcc, const std::vector<double>& minLimits, const std::vector<double>& maxLimits, const std::vector<double>& home_positions)
|
||||
: total_joints_(total_joints),
|
||||
length1_(length1),
|
||||
length2_(length2),
|
||||
maxSpeed_(maxSpeed),
|
||||
maxAcc_(maxAcc),
|
||||
joint_home_positions_(home_positions),
|
||||
minLimits_(minLimits),
|
||||
maxLimits_(maxLimits)
|
||||
ArmControl::ArmControl(
|
||||
size_t total_joints,
|
||||
double length1,
|
||||
double length2,
|
||||
double maxSpeed,
|
||||
double maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
):
|
||||
total_joints_(total_joints),
|
||||
length1_(length1),
|
||||
length2_(length2),
|
||||
maxSpeed_(maxSpeed),
|
||||
maxAcc_(maxAcc),
|
||||
joint_home_positions_(home_positions),
|
||||
joint_directions_(joint_directions),
|
||||
minLimits_(minLimits),
|
||||
maxLimits_(maxLimits)
|
||||
{
|
||||
std::cout << "ArmControl Constructor" << std::endl;
|
||||
if (total_joints_ <= 0)
|
||||
throw std::invalid_argument("Total joints must be positive");
|
||||
|
||||
@@ -24,15 +36,14 @@ ArmControl::ArmControl(size_t total_joints, double length1, double length2, doub
|
||||
joint_acceleration_.resize(total_joints_, 0.0);
|
||||
joint_torque_.resize(total_joints_, 0.0);
|
||||
joint_position_desired_.resize(total_joints_, 0.0);
|
||||
joint_home_positions_.resize(total_joints_, 0.0);
|
||||
|
||||
// 设置零点位置
|
||||
joint_home_positions_ = home_positions;
|
||||
|
||||
// 初始化梯形轨迹规划器
|
||||
trapezoidalTrajectory_ = new TrapezoidalTrajectory(10.0, 100);
|
||||
|
||||
is_moving_ = false;
|
||||
is_stopping_ = false;
|
||||
stopDurationTime_ = 0.0;
|
||||
movingDurationTime_ = 0.0;
|
||||
}
|
||||
|
||||
ArmControl::~ArmControl()
|
||||
@@ -41,7 +52,7 @@ ArmControl::~ArmControl()
|
||||
}
|
||||
|
||||
|
||||
void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double duration)
|
||||
bool ArmControl::MoveToTargetPoint(std::vector<double>& joint_positions, const Eigen::Vector3d& target_pos, double dt)
|
||||
{
|
||||
// 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
|
||||
// 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
|
||||
@@ -51,33 +62,63 @@ void ArmControl::MoveToTargetPoint(const Eigen::Vector3d& target_pos, double dur
|
||||
// joint_target = inverse_kinematics(target_pos);
|
||||
|
||||
// 使用梯形轨迹规划移动到目标关节位置
|
||||
MoveToTargetJoint(joint_target, duration);
|
||||
return MoveToTargetJoint(joint_positions, joint_target, dt);
|
||||
}
|
||||
|
||||
void ArmControl::MoveToTargetJoint(const std::vector<double>& target_joint, double duration)
|
||||
bool ArmControl::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
|
||||
{
|
||||
if (target_joint.size() != total_joints_)
|
||||
throw std::invalid_argument("Target joint vector size mismatch");
|
||||
|
||||
// 初始化轨迹规划器
|
||||
trapezoidalTrajectory_->init(joint_position_, target_joint, duration);
|
||||
|
||||
static double elapsed_time = 0.0;
|
||||
if (joint_position_.size() != target_joint.size())
|
||||
{
|
||||
throw std::invalid_argument("Joint position and target joint size do not match.");
|
||||
}
|
||||
|
||||
// 更新轨迹
|
||||
joint_position_ = trapezoidalTrajectory_->update(elapsed_time);
|
||||
if (!is_moving_)
|
||||
{
|
||||
bool all_joints_reached = true;
|
||||
const double position_tolerance = 1e-3;
|
||||
|
||||
for (size_t i = 0; i < target_joint.size(); i++)
|
||||
{
|
||||
double position_error = std::fabs(joint_position_[i] - target_joint[i]);
|
||||
if (position_error > position_tolerance)
|
||||
{
|
||||
all_joints_reached = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (all_joints_reached)
|
||||
{
|
||||
joint_positions = target_joint;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// 从当前关节位置开始规划轨迹
|
||||
trapezoidalTrajectory_->init(joint_position_, target_joint);
|
||||
movingDurationTime_ = 0.0;
|
||||
is_moving_ = true;
|
||||
}
|
||||
|
||||
movingDurationTime_ += dt;
|
||||
|
||||
std::vector<double> desired_pos = trapezoidalTrajectory_->update(movingDurationTime_);
|
||||
|
||||
// 检查是否到达目标位置
|
||||
if (trapezoidalTrajectory_->isFinished(movingDurationTime_))
|
||||
{
|
||||
joint_positions = target_joint;
|
||||
is_moving_ = false;
|
||||
movingDurationTime_ = 0.0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
joint_positions = desired_pos;
|
||||
joint_velocity_ = trapezoidalTrajectory_->getVelocity();
|
||||
joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
|
||||
|
||||
// 更新时间
|
||||
if (trapezoidalTrajectory_->isFinished(elapsed_time))
|
||||
{
|
||||
elapsed_time = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
elapsed_time += 0.01; // 假设控制周期为10ms
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ArmControl::GoHome(std::vector<double>& joint_positions, double dt)
|
||||
@@ -154,35 +195,27 @@ bool ArmControl::Stop(std::vector<double>& joint_positions, double dt)
|
||||
if (is_moving_ && !is_stopping_)
|
||||
{
|
||||
is_stopping_ = true;
|
||||
|
||||
// 从当前关节位置开始规划轨迹
|
||||
std::cout << "arm control stop1" << std::endl;
|
||||
trapezoidalTrajectory_->StopCalculate();
|
||||
std::cout << "arm control stop2" << std::endl;
|
||||
stopDurationTime_ = 0.0;
|
||||
}
|
||||
|
||||
if (is_stopping_)
|
||||
{
|
||||
static double elapsed_time = 0.0;
|
||||
|
||||
std::vector<double> desired_pos = trapezoidalTrajectory_->StopUpdate(dt);
|
||||
|
||||
elapsed_time += dt;
|
||||
|
||||
if (trapezoidalTrajectory_->stopFinished(elapsed_time))
|
||||
stopDurationTime_ += dt;
|
||||
joint_positions = trapezoidalTrajectory_->StopUpdate(stopDurationTime_);
|
||||
if (trapezoidalTrajectory_->stopFinished(stopDurationTime_))
|
||||
{
|
||||
joint_positions = joint_position_;
|
||||
is_moving_ = false;
|
||||
is_stopping_ = false;
|
||||
elapsed_time = 0.0;
|
||||
|
||||
stopDurationTime_ = 0.0;
|
||||
return true;
|
||||
}
|
||||
|
||||
joint_positions = desired_pos;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
return true; // 如果没有移动或停止，直接返回true
|
||||
return true;
|
||||
}
|
||||
|
||||
void ArmControl::UpdateJointStates(
|
||||
|
||||
@@ -12,15 +12,17 @@ LegControl::LegControl(
|
||||
double maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
) :
|
||||
total_joints_(totalJoints),
|
||||
maxSpeed_(maxSpeed),
|
||||
maxAcc_(maxAcc),
|
||||
LegLength_(LegLength),
|
||||
joint_home_positions_(home_positions),
|
||||
maxLimits_(maxLimits),
|
||||
minLimits_(minLimits)
|
||||
joint_directions_(joint_directions),
|
||||
minLimits_(minLimits),
|
||||
maxLimits_(maxLimits)
|
||||
{
|
||||
// 初始化关节状态向量
|
||||
joint_position_.resize(total_joints_, 0.0);
|
||||
@@ -33,11 +35,15 @@ LegControl::LegControl(
|
||||
trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
|
||||
|
||||
is_moving_ = false;
|
||||
|
||||
is_stopping_ = false;
|
||||
stopDurationTime_ = 0.0;
|
||||
movingDurationTime_ = 0.0;
|
||||
}
|
||||
|
||||
LegControl::~LegControl() = default;
|
||||
LegControl::~LegControl()
|
||||
{
|
||||
delete trapezoidalTrajectory_;
|
||||
}
|
||||
|
||||
bool LegControl::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
|
||||
{
|
||||
@@ -45,18 +51,16 @@ bool LegControl::MoveToTargetJoint(std::vector<double>& joint_positions, const s
|
||||
{
|
||||
throw std::invalid_argument("Joint position and target joint size do not match.");
|
||||
}
|
||||
|
||||
static double elapsed_time = 0.0;
|
||||
|
||||
if (!is_moving_)
|
||||
{
|
||||
bool all_joints_reached = true;
|
||||
const double home_tolerance = 1e-3;
|
||||
const double position_tolerance = 1e-3;
|
||||
|
||||
for (size_t i = 0; i < target_joint.size(); i++)
|
||||
{
|
||||
double position_error = std::fabs(joint_position_[i] - target_joint[i]);
|
||||
if (position_error > home_tolerance)
|
||||
if (position_error > position_tolerance)
|
||||
{
|
||||
all_joints_reached = false;
|
||||
break;
|
||||
@@ -72,32 +76,26 @@ bool LegControl::MoveToTargetJoint(std::vector<double>& joint_positions, const s
|
||||
|
||||
// 从当前关节位置开始规划轨迹
|
||||
trapezoidalTrajectory_->init(joint_position_, target_joint);
|
||||
elapsed_time = 0.0;
|
||||
movingDurationTime_ = 0.0;
|
||||
is_moving_ = true;
|
||||
|
||||
std::cout << "Leg Moving to target joint! " << std::endl;
|
||||
}
|
||||
|
||||
std::vector<double> desired_pos = trapezoidalTrajectory_->update(elapsed_time);
|
||||
elapsed_time += dt;
|
||||
movingDurationTime_ += dt;
|
||||
|
||||
std::vector<double> desired_pos = trapezoidalTrajectory_->update(movingDurationTime_);
|
||||
|
||||
// 检查是否到达目标位置
|
||||
if (trapezoidalTrajectory_->isFinished(elapsed_time))
|
||||
if (trapezoidalTrajectory_->isFinished(movingDurationTime_))
|
||||
{
|
||||
joint_positions = target_joint;
|
||||
is_moving_ = false;
|
||||
elapsed_time = 0.0;
|
||||
|
||||
std::cout << "Leg moving finished" << std::endl;
|
||||
movingDurationTime_ = 0.0;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
joint_velocity_ = trapezoidalTrajectory_->getVelocity();
|
||||
joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
|
||||
|
||||
joint_positions = desired_pos;
|
||||
}
|
||||
joint_positions = desired_pos;
|
||||
joint_velocity_ = trapezoidalTrajectory_->getVelocity();
|
||||
joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -108,7 +106,14 @@ bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
|
||||
{
|
||||
for (size_t i = 0; i < joint_position_.size(); i++)
|
||||
{
|
||||
joint_position_desired_[i] = maxLimits_[i];
|
||||
if (joint_directions_[i] == 1)
|
||||
{
|
||||
joint_position_desired_[i] = maxLimits_[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
joint_position_desired_[i] = minLimits_[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -121,7 +126,14 @@ bool LegControl::MoveDown(std::vector<double>& joint_positions, double dt)
|
||||
{
|
||||
for (size_t i = 0; i < joint_position_.size(); i++)
|
||||
{
|
||||
joint_position_desired_[i] = minLimits_[i];
|
||||
if (joint_directions_[i] == 1)
|
||||
{
|
||||
joint_position_desired_[i] = minLimits_[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
joint_position_desired_[i] = maxLimits_[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -133,34 +145,21 @@ bool LegControl::Stop(std::vector<double>& joint_positions, double dt)
|
||||
if (is_moving_ && !is_stopping_)
|
||||
{
|
||||
is_stopping_ = true;
|
||||
|
||||
// 从当前关节位置开始规划轨迹
|
||||
trapezoidalTrajectory_->StopCalculate();
|
||||
|
||||
std::cout << "Leg stopping " << std::endl;
|
||||
stopDurationTime_ = 0.0;
|
||||
}
|
||||
|
||||
if (is_stopping_)
|
||||
{
|
||||
static double elapsed_time = 0.0;
|
||||
|
||||
std::vector<double> desired_pos = trapezoidalTrajectory_->StopUpdate(dt);
|
||||
|
||||
elapsed_time += dt;
|
||||
|
||||
if (trapezoidalTrajectory_->stopFinished(elapsed_time))
|
||||
stopDurationTime_ += dt;
|
||||
joint_positions = trapezoidalTrajectory_->StopUpdate(stopDurationTime_);
|
||||
if (trapezoidalTrajectory_->stopFinished(stopDurationTime_))
|
||||
{
|
||||
joint_positions = joint_position_;
|
||||
is_moving_ = false;
|
||||
is_stopping_ = false;
|
||||
elapsed_time = 0.0;
|
||||
|
||||
std::cout << "Leg stopping finished" << std::endl;
|
||||
stopDurationTime_ = 0.0;
|
||||
return true;
|
||||
}
|
||||
|
||||
joint_positions = desired_pos;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "robot_fsm.hpp"
|
||||
#include <iostream>
|
||||
|
||||
/**
|
||||
* @brief 程序入口函数
|
||||
|
||||
@@ -47,7 +47,7 @@ def plot_joint_trajectories(file_path):
|
||||
|
||||
plt.figure(figsize=(12, 8))
|
||||
for i in range(num_plots):
|
||||
plt.plot(relative_times, joint_data[i], label=f'关节 {i+1}')
|
||||
plt.plot(joint_data[i], label=f'关节 {i+1}')
|
||||
|
||||
# 图形配置
|
||||
plt.xlabel('时间 (秒)')
|
||||
|
||||
@@ -12,10 +12,7 @@ RobotControlManager::RobotControlManager()
|
||||
|
||||
void RobotControlManager::init()
|
||||
{
|
||||
robotInited_ = false;
|
||||
|
||||
// Initialize the robot control manager
|
||||
cout << "Initializing robot control manager..." << std::endl;
|
||||
jointInited_.resize(motionParams_.total_joints_, false);
|
||||
|
||||
// Initialize the robot control manager
|
||||
motionParams_ = MotionParameters();
|
||||
@@ -34,7 +31,8 @@ void RobotControlManager::init()
|
||||
motionParams_.max_joint_acceleration_,
|
||||
motionParams_.left_arm_min_limit_,
|
||||
motionParams_.left_arm_max_limit_,
|
||||
motionParams_.left_arm_home_positions_
|
||||
motionParams_.left_arm_home_positions_,
|
||||
motionParams_.left_arm_joint_indices_
|
||||
);
|
||||
|
||||
rightArmController_ = new ArmControl(
|
||||
@@ -45,7 +43,8 @@ void RobotControlManager::init()
|
||||
motionParams_.max_joint_acceleration_,
|
||||
motionParams_.right_arm_min_limit_,
|
||||
motionParams_.right_arm_max_limit_,
|
||||
motionParams_.right_arm_home_positions_
|
||||
motionParams_.right_arm_home_positions_,
|
||||
motionParams_.right_arm_joint_indices_
|
||||
);
|
||||
|
||||
leftLegController_ = new LegControl(
|
||||
@@ -55,7 +54,8 @@ void RobotControlManager::init()
|
||||
motionParams_.max_joint_acceleration_,
|
||||
motionParams_.left_leg_min_limit_,
|
||||
motionParams_.left_leg_max_limit_,
|
||||
motionParams_.left_leg_home_positions_
|
||||
motionParams_.left_leg_home_positions_,
|
||||
motionParams_.left_leg_joint_directions_
|
||||
);
|
||||
|
||||
rightLegController_ = new LegControl(
|
||||
@@ -65,7 +65,8 @@ void RobotControlManager::init()
|
||||
motionParams_.max_joint_acceleration_,
|
||||
motionParams_.right_leg_min_limit_,
|
||||
motionParams_.right_leg_max_limit_,
|
||||
motionParams_.right_leg_home_positions_
|
||||
motionParams_.right_leg_home_positions_,
|
||||
motionParams_.right_leg_joint_directions_
|
||||
);
|
||||
|
||||
|
||||
@@ -129,9 +130,19 @@ void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues)
|
||||
}
|
||||
}
|
||||
|
||||
bool isAllTrueManual(const std::vector<bool>& vec) {
|
||||
for (bool element : vec) { // 范围 for 循环遍历每个元素
|
||||
if (!element) { // 若存在 false，直接返回 false
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true; // 所有元素都是 true
|
||||
}
|
||||
|
||||
bool isPrint = false;
|
||||
|
||||
void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
std::cout << "Update joint states" << std::endl;
|
||||
jointStates_ = *msg;
|
||||
jointPositions_.resize(motionParams_.total_joints_);
|
||||
jointVelocities_.resize(motionParams_.total_joints_);
|
||||
@@ -140,36 +151,46 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
|
||||
for (size_t i = 0; i < motionParams_.total_joints_; i++)
|
||||
{
|
||||
jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_; // convert to degree
|
||||
jointVelocities_[i] = 0; //jointStates_.velocity[i] * motionParams_.pulse_to_degree_;
|
||||
jointEfforts_[i] = 0; //jointStates_.effort[i] ; // TODO: check the factory
|
||||
jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_;
|
||||
jointEfforts_[i] = 0.0; //jointStates_.effort[i] ; // TODO: check the factory
|
||||
|
||||
}
|
||||
|
||||
std::cout << "Joint positions: ";
|
||||
|
||||
// TODO: This block can be deleted
|
||||
if(!robotInited_)
|
||||
for (size_t i = 0; i < motionParams_.total_joints_; i++)
|
||||
{
|
||||
robotInited_ = true;
|
||||
for (size_t i = 0; i < motionParams_.total_joints_; i++)
|
||||
{
|
||||
if(!jointInited_[i])
|
||||
{
|
||||
jointCommands_.data[i] = jointStates_.position[i]* motionParams_.pulse_to_degree_;
|
||||
std::cout << "Robot inited joint " << i + 1 << jointCommands_.data[i] << std::endl;
|
||||
}
|
||||
return;
|
||||
|
||||
if(jointCommands_.data[i] != 0)
|
||||
{
|
||||
jointInited_[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// // Update the arm controller
|
||||
// size_t armJointSize = motionParams_.left_arm_joint_indices_.size();
|
||||
// std::vector<double> armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
|
||||
// std::vector<double> armVelocities(jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
|
||||
// std::vector<double> armEfforts(jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
|
||||
// leftArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts);
|
||||
if (isAllTrueManual(jointInited_) && !isPrint)
|
||||
{
|
||||
isPrint = true;
|
||||
std::cout << "All joints are initialized" << std::endl;
|
||||
for (size_t i = 0; i < motionParams_.total_joints_; i++)
|
||||
{
|
||||
std::cout << "Joint positions: " << i + 1 << " " << jointCommands_.data[i] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Update the arm controller
|
||||
size_t armJointSize = motionParams_.left_arm_joint_indices_.size();
|
||||
std::vector<double> armPositions(jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
|
||||
std::vector<double> armVelocities(jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
|
||||
std::vector<double> armEfforts(jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.left_arm_joint_indices_[0] + armJointSize);
|
||||
leftArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts);
|
||||
|
||||
// armPositions = std::vector<double>(jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
|
||||
// armVelocities = std::vector<double>(jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
|
||||
// armEfforts = std::vector<double>(jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
|
||||
// rightArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts);
|
||||
armPositions = std::vector<double>(jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0], jointPositions_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
|
||||
armVelocities = std::vector<double>(jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
|
||||
armEfforts = std::vector<double>(jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_arm_joint_indices_[0] + armJointSize);
|
||||
rightArmController_->UpdateJointStates(armPositions, armVelocities, armEfforts);
|
||||
|
||||
// Update the leg controller
|
||||
size_t legJointSize = motionParams_.left_leg_joint_indices_.size();
|
||||
@@ -182,6 +203,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
|
||||
legVelocities = std::vector<double>(jointVelocities_.begin() + motionParams_.right_leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize);
|
||||
legEfforts = std::vector<double>(jointEfforts_.begin() + motionParams_.right_leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.right_leg_joint_indices_[0] + legJointSize);
|
||||
rightLegController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
|
||||
|
||||
}
|
||||
|
||||
MotionParameters RobotControlManager::GetMotionParameters()
|
||||
@@ -213,35 +235,39 @@ void RobotControlManager::TurnRight()
|
||||
wheelController_ -> ExecuteCommand();
|
||||
}
|
||||
|
||||
void RobotControlManager::Stop(double dt)
|
||||
bool RobotControlManager::Stop(double dt)
|
||||
{
|
||||
wheelController_ -> setLinearAngularSpeed(0.0, 0.0);
|
||||
wheelController_ -> ExecuteCommand();
|
||||
|
||||
leftLegController_->Stop(tempLeftLegCmd, dt);
|
||||
bool leftLegStopped = leftLegController_->Stop(tempLeftLegCmd, dt);
|
||||
|
||||
rightLegController_->Stop(tempRightLegCmd, dt);
|
||||
bool rightLegStopped = rightLegController_->Stop(tempRightLegCmd, dt);
|
||||
|
||||
leftArmController_->Stop(tempLeftArmCmd, dt);
|
||||
bool leftArmStopped = leftArmController_->Stop(tempLeftArmCmd, dt);
|
||||
|
||||
rightArmController_->Stop(tempRightArmCmd, dt);
|
||||
bool rightArmStopped = rightArmController_->Stop(tempRightArmCmd, dt);
|
||||
|
||||
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i];
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
|
||||
}
|
||||
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i];
|
||||
}
|
||||
for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = motionParams_.left_arm_joint_directions_[i] * tempLeftArmCmd[i];
|
||||
}
|
||||
for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = motionParams_.right_arm_joint_directions_[i] * tempRightArmCmd[i];
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
|
||||
}
|
||||
|
||||
//TODO: there is no data from arm
|
||||
// for (size_t i = 0; i < motionParams_.left_arm_joint_indices_.size(); i++)
|
||||
// {
|
||||
// jointCommands_.data[motionParams_.left_arm_joint_indices_[i]] = motionParams_.left_arm_joint_directions_[i] * tempLeftArmCmd[i];
|
||||
// }
|
||||
// for (size_t i = 0; i < motionParams_.right_arm_joint_indices_.size(); i++)
|
||||
// {
|
||||
// jointCommands_.data[motionParams_.right_arm_joint_indices_[i]] = motionParams_.right_arm_joint_directions_[i] * tempRightArmCmd[i];
|
||||
// }
|
||||
|
||||
return leftLegStopped && rightLegStopped && leftArmStopped && rightArmStopped;
|
||||
}
|
||||
|
||||
void RobotControlManager::MoveUp(double dt)
|
||||
@@ -254,11 +280,11 @@ void RobotControlManager::MoveUp(double dt)
|
||||
|
||||
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i];
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
|
||||
}
|
||||
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i];
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
|
||||
}
|
||||
}
|
||||
|
||||
@@ -272,11 +298,11 @@ void RobotControlManager::MoveDown(double dt)
|
||||
|
||||
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = motionParams_.left_leg_joint_directions_[i] * tempLeftLegCmd[i];
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
|
||||
}
|
||||
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = motionParams_.right_leg_joint_directions_[i] * tempRightLegCmd[i];
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
|
||||
}
|
||||
}
|
||||
|
||||
@@ -300,23 +326,23 @@ bool RobotControlManager::GoHome(double dt)
|
||||
}
|
||||
}
|
||||
|
||||
// if(!robotHomed_[2])
|
||||
// {
|
||||
// robotHomed_[2] = leftLegController_->GoHome(tempLeftLegCmd, dt);
|
||||
// for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
|
||||
// {
|
||||
// jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
|
||||
// }
|
||||
// }
|
||||
if(!robotHomed_[2])
|
||||
{
|
||||
robotHomed_[2] = leftLegController_->GoHome(tempLeftLegCmd, dt);
|
||||
for (size_t i = 0; i < motionParams_.left_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.left_leg_joint_indices_[i]] = tempLeftLegCmd[i];
|
||||
}
|
||||
}
|
||||
|
||||
// if(!robotHomed_[3])
|
||||
// {
|
||||
// robotHomed_[3] = rightLegController_->GoHome(tempRightLegCmd, dt);
|
||||
// for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
|
||||
// {
|
||||
// jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
|
||||
// }
|
||||
// }
|
||||
if(!robotHomed_[3])
|
||||
{
|
||||
robotHomed_[3] = rightLegController_->GoHome(tempRightLegCmd, dt);
|
||||
for (size_t i = 0; i < motionParams_.right_leg_joint_indices_.size(); i++)
|
||||
{
|
||||
jointCommands_.data[motionParams_.right_leg_joint_indices_[i]] = tempRightLegCmd[i];
|
||||
}
|
||||
}
|
||||
|
||||
return robotHomed_[0] && robotHomed_[1];// && homeFinished[2] && homeFinished[3];
|
||||
}
|
||||
|
||||
@@ -6,13 +6,16 @@ using namespace robot_control;
|
||||
namespace fs = std::filesystem;
|
||||
|
||||
// 机器人控制器构造函数
|
||||
RobotFsm::RobotFsm() : Node("robot_Fsm_node"){
|
||||
|
||||
RobotFsm::RobotFsm() : Node("robot_Fsm_node")
|
||||
{
|
||||
std::cout << "RobotFsm Node is created!" << std::endl;
|
||||
InitTransitionTable();
|
||||
currentState_ = RobotState::STOPPED;
|
||||
previousState_ = RobotState::STOPPED;
|
||||
currentState_ = RobotState::INIT;
|
||||
previousState_ = RobotState::INIT;
|
||||
|
||||
isRobotEnabled_ = false;
|
||||
enableCommandExecuted_ = false;
|
||||
jogDirection_ = 0;
|
||||
|
||||
// 初始化数据文件（设置路径，确保目录存在）
|
||||
data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径
|
||||
@@ -26,9 +29,6 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node"){
|
||||
} else {
|
||||
// 写入表头（仅在文件为空时）
|
||||
data_file_.seekp(0, std::ios::end); // 移动到文件末尾
|
||||
if (data_file_.tellp() == 0) { // 文件为空
|
||||
data_file_ << "timestamp(sec),joint_1_position" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// 添加输入设备
|
||||
@@ -58,6 +58,8 @@ RobotFsm::RobotFsm() : Node("robot_Fsm_node"){
|
||||
|
||||
// 创建定时器，每10ms执行一次控制逻辑（频率100Hz）
|
||||
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(10),std::bind(&RobotFsm::ControlLoop, this)); // 绑定到新的控制函数);
|
||||
|
||||
std::cout << "RobotFsm Node is created finished!" << std::endl;
|
||||
}
|
||||
|
||||
RobotFsm::~RobotFsm()
|
||||
@@ -68,9 +70,10 @@ RobotFsm::~RobotFsm()
|
||||
std::cout << "Robot controller stopped." << std::endl;
|
||||
}
|
||||
|
||||
void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
|
||||
void RobotFsm::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
if (!msg) { // 检查消息是否有效
|
||||
RCLCPP_WARN(this->get_logger(), "收到空的joint_states消息,忽略");
|
||||
std::cout << "get null joint states!" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -134,20 +137,25 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
|
||||
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::RIGHT);
|
||||
}
|
||||
else if (command == "上升") {
|
||||
jogDirection_ = 1;
|
||||
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::UP);
|
||||
}
|
||||
else if (command == "下降") {
|
||||
jogDirection_ = -1;
|
||||
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::DOWN);
|
||||
}
|
||||
else if (command == "停止") {
|
||||
jogDirection_ = 0;
|
||||
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP);
|
||||
}
|
||||
else if (command == "A") {
|
||||
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME);
|
||||
// TODO: need to be checked.
|
||||
// inputDevices_[InputType::JOY]->SetCommand(ControlCommand::GO_HOME);
|
||||
}
|
||||
else if (command == "B") {
|
||||
if (!isRobotEnabled_)
|
||||
if (!enableCommandExecuted_)
|
||||
{
|
||||
enableCommandExecuted_ = true;
|
||||
EnableRobot();
|
||||
}
|
||||
}
|
||||
@@ -158,10 +166,10 @@ void RobotFsm::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
|
||||
|
||||
void RobotFsm::EnableRobot()
|
||||
{
|
||||
control_msgs::msg::DynamicInterfaceGroupValues enableMsg;
|
||||
|
||||
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
|
||||
{
|
||||
control_msgs::msg::DynamicInterfaceGroupValues enableMsg;
|
||||
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
|
||||
enableMsg.interface_groups.push_back(tempInterfaceGroup);
|
||||
|
||||
@@ -169,12 +177,15 @@ void RobotFsm::EnableRobot()
|
||||
tempValue.interface_names = {"enable"};
|
||||
tempValue.values = {1};
|
||||
enableMsg.interface_values.push_back(tempValue);
|
||||
}
|
||||
|
||||
std::cout << "Enable robot" << std::endl;
|
||||
|
||||
// 发布消息
|
||||
gpioPub_->publish(enableMsg);
|
||||
std::cout << "Enable robot " << i + 1 << std::endl;
|
||||
|
||||
gpioPub_->publish(enableMsg);
|
||||
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
sleep(2);
|
||||
|
||||
isRobotEnabled_ = true;
|
||||
}
|
||||
@@ -215,17 +226,15 @@ void RobotFsm::VoiceCommandCallback(const std_msgs::msg::String::SharedPtr msg)
|
||||
|
||||
// 初始化状态转换表
|
||||
void RobotFsm::InitTransitionTable() {
|
||||
// 从任意状态都可以转换到停止状态
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPED;
|
||||
transitionTable_[{RobotState::MOVING_FORWARD, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
transitionTable_[{RobotState::MOVING_BACKWARD, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
transitionTable_[{RobotState::TURNING_LEFT, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
transitionTable_[{RobotState::TURNING_RIGHT, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
transitionTable_[{RobotState::MOVING_UP, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
transitionTable_[{RobotState::MOVING_DOWN, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
transitionTable_[{RobotState::HOME_MOVING, ControlCommand::STOP}] = RobotState::STOPPING;
|
||||
|
||||
// 从停止状态转换到其他状态
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
|
||||
@@ -233,6 +242,14 @@ void RobotFsm::InitTransitionTable() {
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::UP}] = RobotState::MOVING_UP;
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
|
||||
transitionTable_[{RobotState::STOPPED, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING;
|
||||
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::FORWARD}] = RobotState::MOVING_FORWARD;
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::BACKWARD}] = RobotState::MOVING_BACKWARD;
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::LEFT}] = RobotState::TURNING_LEFT;
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::RIGHT}] = RobotState::TURNING_RIGHT;
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::UP}] = RobotState::MOVING_UP;
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::DOWN}] = RobotState::MOVING_DOWN;
|
||||
transitionTable_[{RobotState::INIT, ControlCommand::GO_HOME}] = RobotState::HOME_MOVING;
|
||||
}
|
||||
|
||||
// 添加输入设备
|
||||
@@ -241,32 +258,34 @@ void RobotFsm::AddInputDevice(InputType type, std::unique_ptr<InputDevice> devic
|
||||
std::cout << "Added input device: " << inputDevices_[type]->GetDeviceName() << std::endl;
|
||||
}
|
||||
|
||||
|
||||
// 状态机主循环
|
||||
void RobotFsm::ControlLoop() {
|
||||
|
||||
// 检查所有输入设备
|
||||
ControlCommand cmd = ControlCommand::NONE;
|
||||
for (const auto& [type, device] : inputDevices_) {
|
||||
ControlCommand deviceCmd = device->GetCommand();
|
||||
if (deviceCmd != ControlCommand::NONE) {
|
||||
cmd = deviceCmd;
|
||||
break;
|
||||
}
|
||||
// 检查所有输入设备
|
||||
ControlCommand cmd = ControlCommand::NONE;
|
||||
for (const auto& [type, device] : inputDevices_) {
|
||||
ControlCommand deviceCmd = device->GetCommand();
|
||||
if (deviceCmd != ControlCommand::NONE) {
|
||||
cmd = deviceCmd;
|
||||
break;
|
||||
}
|
||||
|
||||
ProcessCommand(cmd);
|
||||
}
|
||||
|
||||
ProcessCommand(cmd);
|
||||
|
||||
// 计算时间差
|
||||
rclcpp::Time current_time = this->now();
|
||||
rclcpp::Duration dt = current_time - lastTime_;
|
||||
double dt_sec = dt.seconds();
|
||||
lastTime_ = current_time;
|
||||
|
||||
ExecuteStateAction(dt_sec);
|
||||
// 计算时间差
|
||||
rclcpp::Time current_time = this->now();
|
||||
rclcpp::Duration dt = current_time - lastTime_;
|
||||
double dt_sec = dt.seconds();
|
||||
lastTime_ = current_time;
|
||||
|
||||
ExecuteStateAction(dt_sec);
|
||||
|
||||
if (isRobotEnabled_) {
|
||||
Gpio_publish_joint_trajectory();
|
||||
}
|
||||
if (isRobotEnabled_)
|
||||
{
|
||||
Gpio_publish_joint_trajectory();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -304,38 +323,72 @@ void RobotFsm::Publish_joint_trajectory()
|
||||
|
||||
void RobotFsm::Gpio_publish_joint_trajectory()
|
||||
{
|
||||
auto cmd_msg = robotControlManager_.GetJointCommands();
|
||||
static size_t jointIndex = 5;
|
||||
static auto jointValue = robotControlManager_.GetJointCommands().data;
|
||||
static double jogValue = jointValue[jointIndex -1];
|
||||
|
||||
// 1. 获取当前时间戳
|
||||
rclcpp::Time current_time = this->now();
|
||||
double timestamp = current_time.seconds();
|
||||
std::cout << jogValue << std::endl;
|
||||
|
||||
// 2. 保存整个数组数据到txt文件
|
||||
if (data_file_.is_open()) {
|
||||
data_file_ << timestamp;
|
||||
if (true)
|
||||
{
|
||||
control_msgs::msg::DynamicInterfaceGroupValues positionMsg;
|
||||
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
|
||||
{
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i + 1);
|
||||
positionMsg.interface_groups.push_back(tempInterfaceGroup);
|
||||
|
||||
for (const auto& val : cmd_msg.data) {
|
||||
data_file_ << "," << val; // 用逗号分隔每个元素
|
||||
double tempJointValue;
|
||||
if (i + 1 == jointIndex)
|
||||
{
|
||||
jogValue = jogValue + jogDirection_ * 10;
|
||||
tempJointValue = jogValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
tempJointValue = jointValue[i];
|
||||
}
|
||||
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
tempValue.interface_names = {"position"};
|
||||
tempValue.values = {tempJointValue};
|
||||
positionMsg.interface_values.push_back(tempValue);
|
||||
}
|
||||
|
||||
|
||||
// 发布消息
|
||||
gpioPub_->publish(positionMsg);
|
||||
}
|
||||
else
|
||||
{
|
||||
auto cmd_msg = robotControlManager_.GetJointCommands();
|
||||
|
||||
data_file_ << 0;
|
||||
|
||||
// 2. 保存整个数组数据到txt文件
|
||||
if (data_file_.is_open()) {
|
||||
for (const auto& val : cmd_msg.data) {
|
||||
data_file_ << "," << val; // 用逗号分隔每个元素
|
||||
}
|
||||
|
||||
data_file_ << std::endl; // 换行
|
||||
data_file_.flush(); // 强制刷新
|
||||
}
|
||||
|
||||
control_msgs::msg::DynamicInterfaceGroupValues positionMsg;
|
||||
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
|
||||
{
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
|
||||
positionMsg.interface_groups.push_back(tempInterfaceGroup);
|
||||
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
tempValue.interface_names = {"position"};
|
||||
tempValue.values = {cmd_msg.data[i]};
|
||||
positionMsg.interface_values.push_back(tempValue);
|
||||
}
|
||||
|
||||
data_file_ << std::endl; // 换行
|
||||
data_file_.flush(); // 强制刷新
|
||||
// 发布消息
|
||||
//gpioPub_->publish(positionMsg);
|
||||
}
|
||||
|
||||
control_msgs::msg::DynamicInterfaceGroupValues positionMsg;
|
||||
for (size_t i = 0; i < robotControlManager_.GetMotionParameters().total_joints_; i++)
|
||||
{
|
||||
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
|
||||
positionMsg.interface_groups.push_back(tempInterfaceGroup);
|
||||
|
||||
control_msgs::msg::InterfaceValue tempValue;
|
||||
tempValue.interface_names = {"position"};
|
||||
tempValue.values = {cmd_msg.data[i]};
|
||||
positionMsg.interface_values.push_back(tempValue);
|
||||
}
|
||||
|
||||
// 发布消息
|
||||
// gpioPub_->publish(positionMsg);
|
||||
}
|
||||
|
||||
// 处理命令并转换状态
|
||||
@@ -358,8 +411,15 @@ void RobotFsm::ProcessCommand(ControlCommand cmd) {
|
||||
void RobotFsm::ExecuteStateAction(double dt) {
|
||||
|
||||
switch (currentState_) {
|
||||
case RobotState::INIT:
|
||||
break;
|
||||
case RobotState::STOPPING:
|
||||
if(robotControlManager_.Stop(dt))
|
||||
{
|
||||
currentState_ = RobotState::STOPPED;
|
||||
}
|
||||
break;
|
||||
case RobotState::STOPPED:
|
||||
robotControlManager_.Stop(dt);
|
||||
break;
|
||||
case RobotState::MOVING_FORWARD:
|
||||
robotControlManager_.MoveForward();
|
||||
@@ -383,7 +443,7 @@ void RobotFsm::ExecuteStateAction(double dt) {
|
||||
case RobotState::HOME_MOVING:
|
||||
if(robotControlManager_.GoHome(dt))
|
||||
{
|
||||
inputDevices_[InputType::JOY]->SetCommand(ControlCommand::STOP);
|
||||
currentState_ = RobotState::STOPPED;
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -394,6 +454,8 @@ void RobotFsm::ExecuteStateAction(double dt) {
|
||||
// 状态转换为字符串
|
||||
std::string RobotFsm::StateToString(RobotState state) const {
|
||||
switch (state) {
|
||||
case RobotState::INIT: return "INIT";
|
||||
case RobotState::STOPPING: return "STOPPING";
|
||||
case RobotState::STOPPED: return "STOPPED";
|
||||
case RobotState::MOVING_FORWARD: return "MOVING_FORWARD";
|
||||
case RobotState::MOVING_BACKWARD: return "MOVING_BACKWARD";
|
||||
|
||||
@@ -32,8 +32,11 @@ void TrapezoidalTrajectory::init(const std::vector<double>& start_pos, const std
|
||||
start_pos_ = start_pos;
|
||||
target_pos_ = target_pos;
|
||||
current_pos_.resize(start_pos.size());
|
||||
stop_pos_.resize(start_pos.size());
|
||||
stop_vel_.resize(start_pos.size());
|
||||
current_velocity_.resize(start_pos.size());
|
||||
current_acceleration_.resize(start_pos.size());
|
||||
actual_deceleration_.resize(start_pos.size());
|
||||
is_stopping_ = false;
|
||||
|
||||
calculateTrajectoryParams();
|
||||
@@ -259,6 +262,9 @@ void TrapezoidalTrajectory::StopCalculate()
|
||||
{
|
||||
size_t dof = current_pos_.size(); // 自由度数量
|
||||
std::vector<double> decel_times(dof); // 各轴理论减速时间
|
||||
|
||||
stop_pos_ = current_pos_;
|
||||
stop_vel_ = current_velocity_;
|
||||
|
||||
for (size_t i = 0; i < dof; ++i)
|
||||
{
|
||||
@@ -274,10 +280,10 @@ void TrapezoidalTrajectory::StopCalculate()
|
||||
return;
|
||||
}
|
||||
|
||||
// 第二步：计算各轴实际减速度（确保在总时间内减速到0）
|
||||
for (size_t i = 0; i < dof; ++i) {
|
||||
if (std::fabs(current_velocity_[i]) < 1e-9) {
|
||||
actual_deceleration_[i] = 0.0; // 静止轴减速度为0
|
||||
|
||||
} else {
|
||||
// 实际减速度 = 初始速度 / 总时间（方向与速度相反）
|
||||
actual_deceleration_[i] = -std::copysign(max_acceleration_, current_velocity_[i])
|
||||
@@ -288,15 +294,19 @@ void TrapezoidalTrajectory::StopCalculate()
|
||||
|
||||
std::vector<double> TrapezoidalTrajectory::StopUpdate(double dt)
|
||||
{
|
||||
if(dt > stop_time_)
|
||||
{
|
||||
dt = stop_time_;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < current_pos_.size(); ++i) {
|
||||
// 计算当前速度：v(t) = v0 + a*t
|
||||
current_velocity_[i] = current_velocity_[i] + actual_deceleration_[i] * dt;
|
||||
|
||||
// 计算当前位置：s(t) = s0 + v0*t + 0.5*a*t²
|
||||
current_pos_[i] = current_pos_[i]
|
||||
+ current_velocity_[i] * dt
|
||||
|
||||
current_pos_[i] = stop_pos_[i]
|
||||
+ stop_vel_[i] * dt
|
||||
+ 0.5 * actual_deceleration_[i] * dt * dt;
|
||||
|
||||
current_velocity_[i] = stop_vel_[i] + actual_deceleration_[i] * dt;
|
||||
|
||||
// 数值稳定性处理（避免因浮点误差出现微小负速度）
|
||||
if (std::fabs(current_velocity_[i]) < 1e-9) {
|
||||
current_velocity_[i] = 0.0;
|
||||
|
||||
Reference in New Issue
Block a user