Skip to content

Commit

Permalink
WIP attempt on adding Peak Systems transport
Browse files Browse the repository at this point in the history
  • Loading branch information
@airato
airato committed Jun 8, 2024
1 parent 153c922 commit b1e2975
Show file tree
Hide file tree
Showing 3 changed files with 1,490 additions and 968 deletions.
118 changes: 72 additions & 46 deletions lib/cpp/examples/bandwidth_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,36 +27,43 @@

#include "moteus.h"

namespace {
// A simple way to get the current time accurately as a double.
static double GetNow() {
struct timespec ts = {};
::clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
return static_cast<double>(ts.tv_sec) +
static_cast<double>(ts.tv_nsec) / 1e9;
}
namespace
{
// A simple way to get the current time accurately as a double.
static double GetNow()
{
struct timespec ts = {};
::clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
return static_cast<double>(ts.tv_sec) +
static_cast<double>(ts.tv_nsec) / 1e9;
}
}

int main(int argc, char** argv) {
int main(int argc, char **argv)
{
using namespace mjbots;

const std::vector<std::string> 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<moteus::PeakCan>(options);

// Just for some kind of "--help".
moteus::Controller::DefaultArgProcess(argc, argv);
// 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;
}
const bool minimal_format = [&]()
{
// auto it = std::find(args.begin(), args.end(), "--minimal-format");
// if (it != args.end())
// {
// args.erase(it);
// return true;
// }
return false;
}();

Expand All @@ -66,25 +73,34 @@ int main(int argc, char** argv) {

// Populate our list of controllers with IDs from the command line.
std::vector<std::shared_ptr<moteus::Controller>> controllers;
while (!args.empty()) {
auto id = std::stoul(args.front());
args.erase(args.begin());
controllers.push_back(
std::make_shared<moteus::Controller>(
[&]() {
moteus::Controller::Options options;
options.id = id;
if (minimal_format) {
options.position_format.position = moteus::kInt16;
options.position_format.velocity = moteus::kInt16;
options.query_format.position = moteus::kInt16;
options.query_format.velocity = moteus::kInt16;
options.query_format.torque = moteus::kIgnore;
}
return options;
}()));
responses[id] = false;
}
// while (!args.empty())
// {
auto id = 1;
// args.erase(args.begin());
controllers.push_back(
std::make_shared<moteus::Controller>(
[&]()
{
moteus::Controller::Options options;
options.transport = transport;
options.id = id;
if (minimal_format)
{
std::cout << "is minimal" << std::endl;
options.position_format.position = moteus::kInt16;
options.position_format.velocity = moteus::kInt16;
options.query_format.position = moteus::kInt16;
options.query_format.velocity = moteus::kInt16;
options.query_format.torque = moteus::kIgnore;
options.query_format.voltage = moteus::kIgnore;
options.query_format.mode = moteus::kIgnore;
options.query_format.temperature = moteus::kIgnore;
options.query_format.fault = moteus::kIgnore;
}
return options;
}()));
responses[id] = false;
// }

std::vector<moteus::CanFdFrame> send_frames;
std::vector<moteus::CanFdFrame> receive_frames;
Expand All @@ -97,35 +113,45 @@ int main(int argc, char** argv) {
double status_time = GetNow() + kStatusPeriodS;
int hz_count = 0;

while (true) {
while (true)
{
hz_count++;
send_frames.clear();
receive_frames.clear();

for (auto& c : controllers) {
for (auto &c : controllers)
{
send_frames.push_back(c->MakePosition(cmd));
}
for (auto& pair : responses) {
for (auto &pair : responses)
{
pair.second = false;
}

transport->BlockingCycle(&send_frames[0], send_frames.size(),
&receive_frames);

for (const auto& f : receive_frames) {
for (const auto &f : receive_frames)
{
responses[f.source] = true;
}

const int count = [&]() {
const int count = [&]()
{
int sum = 0;
for (const auto& pair : responses) {
if (pair.second) { sum++; }
for (const auto &pair : responses)
{
if (pair.second)
{
sum++;
}
}
return sum;
}();

const auto now = GetNow();
if (now > status_time) {
if (now > status_time)
{
printf("%6.1fHz rx_count=%2d \r",
hz_count / kStatusPeriodS, count);
fflush(stdout);
Expand Down
46 changes: 46 additions & 0 deletions lib/cpp/examples/stop.cc
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
// Copyright 2023 mjbots Robotic Systems, LLC. [email protected]
//
// 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 <unistd.h>

#include <iostream>

#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();
}
Loading

0 comments on commit b1e2975

Please sign in to comment.