diff --git a/lib/cpp/examples/bandwidth_test.cc b/lib/cpp/examples/bandwidth_test.cc index 307bd414..dd37323a 100644 --- a/lib/cpp/examples/bandwidth_test.cc +++ b/lib/cpp/examples/bandwidth_test.cc @@ -41,22 +41,24 @@ int main(int argc, char** argv) { using namespace mjbots; const std::vector args_in(argv, argv + argc); - auto args = moteus::Controller::ProcessTransportArgs(args_in); - auto transport = moteus::Controller::MakeSingletonTransport({}); + // auto args = moteus::Controller::ProcessTransportArgs(args_in); + // auto transport = moteus::Controller::MakeSingletonTransport({}); + moteus::PeakCan::Options options; + auto transport = std::make_shared(options); // Just for some kind of "--help". moteus::Controller::DefaultArgProcess(argc, argv); - args.erase(args.begin()); // our name + // args.erase(args.begin()); // our name // Should use use int16 position/velocity command and query and // disable torque query? const bool minimal_format = [&]() { - auto it = std::find(args.begin(), args.end(), "--minimal-format"); - if (it != args.end()) { - args.erase(it); - return true; - } + // auto it = std::find(args.begin(), args.end(), "--minimal-format"); + // if (it != args.end()) { + // args.erase(it); + // return true; + // } return false; }(); @@ -66,9 +68,9 @@ int main(int argc, char** argv) { // Populate our list of controllers with IDs from the command line. std::vector> controllers; - while (!args.empty()) { - auto id = std::stoul(args.front()); - args.erase(args.begin()); + // while (!args.empty()) { + auto id = 1; // std::stoul(args.front()); + // args.erase(args.begin()); controllers.push_back( std::make_shared( [&]() { @@ -84,7 +86,7 @@ int main(int argc, char** argv) { return options; }())); responses[id] = false; - } + // } std::vector send_frames; std::vector receive_frames; @@ -136,4 +138,4 @@ int main(int argc, char** argv) { ::usleep(10); } -} +} \ No newline at end of file diff --git a/lib/cpp/examples/stop.cc b/lib/cpp/examples/stop.cc new file mode 100644 index 00000000..96ed7360 --- /dev/null +++ b/lib/cpp/examples/stop.cc @@ -0,0 +1,46 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file +/// +/// Show how to send Position mode commands at a regular interval and +/// intepret telemetry data from the servo. + +#include + +#include + +#include "moteus.h" + +int main(int argc, char **argv) +{ + using namespace mjbots; + + // The following DefaultArgProcess is an optional call. If made, + // then command line arguments will be handled which allow setting + // and configuring the default 'transport' to be used if none is + // specified in Controller::Options::transport. + moteus::Controller::DefaultArgProcess(argc, argv); + + // There are many possible options to set for each controller + // instance. Here we re-set the ID to the default (1), just to show + // how it is done. + moteus::Controller::Options options; + options.id = 1; + + moteus::Controller controller(options); + + // Command a stop to the controller. + controller.SetStop(); +} diff --git a/lib/cpp/mjbots/moteus/moteus_transport.h b/lib/cpp/mjbots/moteus/moteus_transport.h index 822c3c9d..f122eee6 100644 --- a/lib/cpp/mjbots/moteus/moteus_transport.h +++ b/lib/cpp/mjbots/moteus/moteus_transport.h @@ -44,6 +44,7 @@ #include "moteus_protocol.h" #include "moteus_tokenizer.h" +#include "PCANBasic.h" #ifdef CANFD_FDF #define MJBOTS_MOTEUS_ENABLE_SOCKETCAN 1 @@ -888,6 +889,170 @@ class Socketcan : public details::TimeoutTransport { }; #endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN +class PeakCan : public details::TimeoutTransport +{ + public: + struct Options : details::TimeoutTransport::Options + { + TPCANHandle ifname = PCAN_PCIBUS3; + + /// + /// Sets the bitrate for CAN FD devices. + /// Example - Bitrate Nom: 1Mbit/s Data: 2Mbit/s: + /// "f_clock_mhz=20, nom_brp=5, nom_tseg1=2, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=3, data_tseg2=1, data_sjw=1" + /// + // TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=20, nom_brp=5, nom_tseg1=2, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=3, data_tseg2=1, data_sjw=1"); + // TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=40, nom_brp=25, nom_tseg1=12, nom_tseg2=1, nom_sjw=1, data_brp=2, data_tseg1=2, data_tseg2=1, data_sjw=1"); + TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=80, nom_brp=10, nom_tseg1=12, nom_tseg2=1, nom_sjw=1, data_brp=4, data_tseg1=2, data_tseg2=1, data_sjw=1"); + // TPCANBitrateFD BitrateFD = const_cast("f_clock_mhz=80, nom_brp=10, nom_tseg1=51, nom_tseg2=28, nom_sjw=10, data_brp=1, data_tseg1=9, data_tseg2=6, data_sjw=5"); + + Options() {} + }; + + PeakCan(const Options &options) + : details::TimeoutTransport(options), + options_(options) + { + // socket_ = Open(options_.ifname); + TPCANStatus stsResult = CAN_InitializeFD(options_.ifname, options_.BitrateFD); + if (stsResult != PCAN_ERROR_OK) + { + char errorMsg[256]; + CAN_GetErrorText(stsResult, 0, errorMsg); + FailIf(true, errorMsg); // Replace with your error handling + } + } + + virtual ~PeakCan() + { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + CAN_Uninitialize(options_.ifname); // Uninitialize PCAN handle + } + + private: + static void SetNonblock(int fd) + { + int flags = ::fcntl(fd, F_GETFL, 0); + FailIf(flags < 0, "error getting flags"); + flags |= O_NONBLOCK; + FailIf(::fcntl(fd, F_SETFL, flags), "error setting flags"); + } + + static int Open(const std::string &ifname) + { + const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + FailIf(fd < 0, "error opening CAN socket"); + + SetNonblock(fd); + + struct ifreq ifr = {}; + std::strncpy(&ifr.ifr_name[0], ifname.c_str(), + sizeof(ifr.ifr_name) - 1); + FailIf(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0, + "could not find CAN: " + ifname); + + const int enable_canfd = 1; + FailIf(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) != 0, + "could not set CAN-FD mode"); + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + FailIf(::bind(fd, + reinterpret_cast(&addr), + sizeof(addr)) < 0, + "could not bind to CAN if"); + + return fd; + } + + virtual int CHILD_GetReadFd() const override + { + return -1; // Not applicable for PCAN, return an invalid descriptor + } + + virtual void CHILD_SendCanFdFrame(const CanFdFrame &frame) override + { + TPCANMsgFD msgCanMessageFD = {}; + msgCanMessageFD.ID = frame.arbitration_id; + if (msgCanMessageFD.ID >= 0x7ff) + { + // Set the frame format flag if we need an extended ID. + msgCanMessageFD.ID |= PCAN_MESSAGE_EXTENDED; + } + msgCanMessageFD.DLC = frame.size; // You might need to adjust DLC calculation + std::memcpy(msgCanMessageFD.DATA, frame.data, frame.size); + + using F = CanFdFrame; + + msgCanMessageFD.MSGTYPE = + ((frame.fdcan_frame == F::kDefault || frame.fdcan_frame == F::kForceOn) + ? PCAN_MESSAGE_FD + : 0) | + (((frame.brs == F::kDefault && !options_.disable_brs) || frame.brs == F::kForceOn) + ? PCAN_MESSAGE_BRS + : 0); + + TPCANStatus status = CAN_WriteFD(options_.ifname, &msgCanMessageFD); + if (status != PCAN_ERROR_OK) + { + // Handle the error + char errorMsg[256]; + CAN_GetErrorText(status, 0, errorMsg); + FailIf(true, errorMsg); // Replace with your error handling + } + } + + virtual ConsumeCount CHILD_ConsumeData( + std::vector *replies, + int /* expected_ok_count */, + std::vector *expected_reply_count) override + { + // Reading data from PCAN + TPCANMsgFD recv_frame = {}; + TPCANTimestampFD CANTimeStamp; + TPCANStatus status = CAN_ReadFD(options_.ifname, &recv_frame, &CANTimeStamp); + FailIf(status != PCAN_ERROR_OK, "error reading CAN frame"); + + CanFdFrame this_frame; + this_frame.arbitration_id = recv_frame.ID & 0x1fffffff; + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.brs = (recv_frame.MSGTYPE & PCAN_MESSAGE_BRS) ? CanFdFrame::kForceOn : CanFdFrame::kForceOff; + this_frame.fdcan_frame = (recv_frame.MSGTYPE & PCAN_MESSAGE_FD) ? CanFdFrame::kForceOn : CanFdFrame::kForceOff; + + std::memcpy(this_frame.data, recv_frame.DATA, recv_frame.DLC); + this_frame.size = recv_frame.DLC; + + if (expected_reply_count) + { + if (this_frame.source < + static_cast(expected_reply_count->size())) + { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } + + if (replies) + { + replies->emplace_back(std::move(this_frame)); + } + + ConsumeCount result; + result.ok = 1; + result.rcv = 1; + return result; + } + + virtual void CHILD_FlushTransmit() override {} + + const Options options_; +}; + /// A factory which can create transports given an optional set of /// commandline arguments. class TransportFactory { @@ -1125,4 +1290,4 @@ class TransportRegistry { }; } -} +} \ No newline at end of file