Example usage

// Copyright (c) 2025 Touchlab Limited. All Rights Reserved
// Unauthorized copying or modifications of this file, via any medium is strictly prohibited.

#include <iostream>
#include <iomanip>
#include <numeric>
#include <chrono>
#include <cmath>

#include "schunk_svh_api/schunk_svh_api.hpp"

using namespace std::chrono_literals;  // NOLINT
using schunk_svh::toc;
using schunk_svh::tic;

int main(int argc, char **argv)
{
        if (argc == 2)
    {
        schunk_svh::SchunkSVH svh;
        svh.connect(argv[1], "012345678");

        svh.set_position_settings(schunk_svh::SVH_INDEX_FINGER_DISTAL,
            {-1.0e6, 1.0e6,  200.0e3, 1.00, 1e-3, -1000.0, 1000.0, 0.6, 0.36, 1800.0});

        std::cout << "Schunk SVH driver started\n";
        auto start = tic();
        auto start_command = tic();
        long count = 0;
        double hz = 0.0;
        auto finger = schunk_svh::SVH_INDEX_FINGER_DISTAL;
        double range = (svh.get_position_max(finger) - svh.get_position_min(finger));
        double mid_point = (svh.get_position_max(finger) + svh.get_position_min(finger)) * 0.5;
        std::cout << "Min: " << svh.get_position_min(finger) << "\n";
        std::cout << "Max: " << svh.get_position_max(finger) << "\n";

        // Reserve sensor data buffer
        std::vector<double> position(schunk_svh::SVH_DIMENSION, 0.0);
        std::vector<double> velocity(schunk_svh::SVH_DIMENSION, 0.0);
        std::vector<double> effort(schunk_svh::SVH_DIMENSION, 0.0);
        std::vector<double> command(schunk_svh::SVH_DIMENSION, 0.0);
        std::vector<double> initial_command(schunk_svh::SVH_DIMENSION, 0.0);

        bool has_initial_command = false;

        std::cout << "Range: " << range << "\n";
        std::cout << "Midpoint: " << mid_point << "\n";

        while(true)
        {
            // Read data
            svh.read(position, velocity, effort);

            if (!has_initial_command)
            {
                svh.get_last_command(initial_command);
                has_initial_command = true;

                std::cout << "Initial command:";
                std::cout << std::fixed << std::setprecision(5);
                for (size_t i = 0; i < schunk_svh::SVH_DIMENSION; ++i)
                {
                    std::cout << " " << initial_command[i];
                }
                std::cout << "\n";
            } else {
                double t = toc(start_command);
                command = initial_command;
                command[finger] = mid_point + sin(t * 3.1415 / 5.0) * range / 4.0;
                svh.write(command);
            }

            // Update sensor rate
            if (toc(start) > 0.5)
            {
                hz = static_cast<double>(count) / toc(start);

                // Print update rate and data
                std::cout << "Rate: " << std::fixed << std::setprecision(1) << hz <<
                    "Hz\nPosition: ";
                std::cout << std::fixed << std::setprecision(5);
                for(size_t i = 0; i < schunk_svh::SVH_DIMENSION; i++) std::cout << position[i] << " ";
                std::cout << "\n";
                std::cout << "Velocity: ";
                for(size_t i = 0; i < schunk_svh::SVH_DIMENSION; i++) std::cout << velocity[i] << " ";
                std::cout << "\n";
                std::cout << "Current: ";
                for(size_t i = 0; i < schunk_svh::SVH_DIMENSION; i++) std::cout << effort[i] << " ";
                std::cout << "\n";

                start = tic();
                count = 0;
            }
            ++count;
        }
    } else {
        std::cout << "Usage:\nschunk_svh_example <serial_port>\n";
        exit(1);
    }
    return 0;
}