3 #ifndef __JR3_MBED_HPP__
4 #define __JR3_MBED_HPP__
13 #include <yarp/os/Timer.h>
15 #include <yarp/dev/DeviceDriver.h>
16 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
18 #include "ICanBusSharer.hpp"
19 #include "StateObserver.hpp"
34 class Jr3Mbed :
public yarp::dev::DeviceDriver,
35 public yarp::dev::ISixAxisForceTorqueSensors,
44 bool open(yarp::os::Searchable & config)
override;
45 bool close()
override;
49 unsigned int getId()
override;
58 std::size_t getNrOfSixAxisForceTorqueSensors()
const override;
59 yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(std::size_t sens_index)
const override;
60 bool getSixAxisForceTorqueSensorName(std::size_t sens_index, std::string & name)
const override;
61 bool getSixAxisForceTorqueSensorFrameName(std::size_t sens_index, std::string & name)
const override;
62 bool getSixAxisForceTorqueSensorMeasure(std::size_t sens_index, yarp::sig::Vector & out,
double & timestamp)
const override;
66 enum class can_ops : std::uint16_t
75 GET_FS_FORCES = 0x480,
76 GET_FS_MOMENTS = 0x500,
84 enum class jr3_state : std::uint8_t
85 { READY = 0x00, NOT_INITIALIZED = 0x01 };
88 { SYNC, ASYNC, INVALID };
90 constexpr
unsigned int getCommandId(can_ops op)
const
91 {
return canId +
static_cast<unsigned int>(op); }
93 bool performRequest(
const std::string & cmd,
const can_message & msg, std::uint8_t * response,
bool quiet =
false);
94 bool sendStartSyncCommand(
double filter);
95 bool sendStartAsyncCommand(
double filter,
double delay);
96 bool sendCommand(
const std::string & cmd, can_ops op);
98 bool queryFullScales();
100 bool monitorWorker(
const yarp::os::YarpTimerEvent & event);
102 unsigned int canId {0};
105 double asyncPeriod {0.0};
108 std::array<double, 6> scales;
109 bool shouldQueryFullScales {
false};
111 jr3_mode mode {jr3_mode::INVALID};
116 mutable std::mutex mtx;
118 std::array<std::int16_t, 3> buffer {};
119 std::array<std::int16_t, 6> raw {};
121 std::atomic<yarp::dev::MAS_status> status {yarp::dev::MAS_UNKNOWN};
122 std::atomic_bool isBooting {
false};
123 double timestamp {0.0};
124 std::uint16_t frameCounter {0};
125 double diagnosticsPeriod {0.0};
126 double lastDiagnosticsTimestamp {0.0};
127 unsigned int lastFrameCounter {0};
129 yarp::os::Timer * monitorThread {
nullptr};
131 static constexpr
unsigned int FULL_SCALE = 16384;
Abstract base for a CAN bus sharer.
Definition: ICanBusSharer.hpp:25
Implementation-agnostic consumer for TX CAN transfers.
Definition: ICanSenderDelegate.hpp:22
Implementation of a CAN node on an Mbed board that publishes data from a JR3 sensor.
Definition: Jr3Mbed.hpp:37
unsigned int getId() override
Retrieve CAN node ID.
Definition: ICanBusSharerImpl.cpp:37
bool registerSender(ICanSenderDelegate *sender) override
Pass a handle to a CAN sender delegate instance.
Definition: ICanBusSharerImpl.cpp:130
bool finalize() override
Finalize CAN node communications.
Definition: ICanBusSharerImpl.cpp:76
bool initialize() override
Perform CAN node initialization.
Definition: ICanBusSharerImpl.cpp:44
bool notifyMessage(const can_message &message) override
Notify observers that a new CAN message has arrived.
Definition: ICanBusSharerImpl.cpp:88
bool synchronize(double timestamp) override
Perform synchronized action on CAN master's request.
Definition: ICanBusSharerImpl.cpp:138
Type state observer for unsigned char arrays.
Definition: StateObserver.hpp:142
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
Proxy CAN message structure.
Definition: CanMessage.hpp:20