yarp-devices
Jr3Mbed.hpp
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 #ifndef __JR3_MBED_HPP__
4 #define __JR3_MBED_HPP__
5 
6 #include <cstdint>
7 
8 #include <array>
9 #include <atomic>
10 #include <mutex>
11 #include <string>
12 
13 #include <yarp/os/Timer.h>
14 
15 #include <yarp/dev/DeviceDriver.h>
16 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
17 
18 #include "ICanBusSharer.hpp"
19 #include "StateObserver.hpp"
20 
21 namespace roboticslab
22 {
23 
34 class Jr3Mbed : public yarp::dev::DeviceDriver,
35  public yarp::dev::ISixAxisForceTorqueSensors,
36  public ICanBusSharer
37 {
38 public:
39  ~Jr3Mbed() override
40  { close(); }
41 
42  // --------- DeviceDriver Declarations. Implementation in DeviceDriverImpl.cpp ---------
43 
44  bool open(yarp::os::Searchable & config) override;
45  bool close() override;
46 
47  // --------- ICanBusSharer declarations. Implementation in LacqueyFetch.cpp ---------
48 
49  unsigned int getId() override;
50  bool notifyMessage(const can_message & message) override;
51  bool initialize() override;
52  bool finalize() override;
53  bool registerSender(ICanSenderDelegate * sender) override;
54  bool synchronize(double timestamp) override;
55 
56  // --------- ISixAxisForceTorqueSensors Declarations. Implementation in ISixAxisForceTorqueSensorsImpl.cpp ---------
57 
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;
63 
64 private:
65  // keep this in sync with the firmware
66  enum class can_ops : std::uint16_t
67  {
68  ACK = 0x100,
69  START_SYNC = 0x180,
70  START_ASYNC = 0x200,
71  STOP = 0x280,
72  ZERO_OFFS = 0x300,
73  SET_FILTER = 0x380,
74  GET_STATE = 0x400,
75  GET_FS_FORCES = 0x480,
76  GET_FS_MOMENTS = 0x500,
77  RESET = 0x580,
78  FORCES = 0x600,
79  MOMENTS = 0x680,
80  BOOTUP = 0x700,
81  };
82 
83  // keep this in sync with the firmware
84  enum class jr3_state : std::uint8_t
85  { READY = 0x00, NOT_INITIALIZED = 0x01 };
86 
87  enum class jr3_mode
88  { SYNC, ASYNC, INVALID };
89 
90  constexpr unsigned int getCommandId(can_ops op) const
91  { return canId + static_cast<unsigned int>(op); }
92 
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);
97  bool ping();
98  bool queryFullScales();
99 
100  bool monitorWorker(const yarp::os::YarpTimerEvent & event);
101 
102  unsigned int canId {0};
103 
104  double filter {0.0}; // cutoff frequency [Hz]
105  double asyncPeriod {0.0}; // [s]
106 
107  std::string name;
108  std::array<double, 6> scales;
109  bool shouldQueryFullScales {false};
110 
111  jr3_mode mode {jr3_mode::INVALID};
112 
113  ICanSenderDelegate * sender {nullptr};
114  TypedStateObserver<std::uint8_t[]> * ackStateObserver {nullptr};
115 
116  mutable std::mutex mtx;
117 
118  std::array<std::int16_t, 3> buffer {}; // zero-initialize
119  std::array<std::int16_t, 6> raw {}; // zero-initialize
120 
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};
128 
129  yarp::os::Timer * monitorThread {nullptr};
130 
131  static constexpr unsigned int FULL_SCALE = 16384; // 2^14
132 };
133 
134 } // namespace roboticslab
135 
136 #endif // __JR3_MBED_HPP__
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
@ BOOTUP
Initial bootup.
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
Proxy CAN message structure.
Definition: CanMessage.hpp:20