1 #include <iostream>

    2 #include <iomanip>

    3 #include <string>

    4 #include <csignal>

    5 #include <ctime>

    6 #include <boost/program_options.hpp>

    7 #include "falcon/core/FalconDevice.h"

    8 #include "falcon/firmware/FalconFirmwareNovintSDK.h"

    9 #include "falcon/kinematic/FalconKinematicStamper.h"

   10 #include "falcon/util/FalconCLIBase.h"

   11 #include "falcon/util/FalconDeviceBoostThread.h"

   12 

   13 using namespace libnifalcon;

   14 

   15 bool run_app = true;

   16 

   17 class FalconCLIMinimal : public FalconCLIBase

   18 {

   19 public:

   20     FalconCLIMinimal() {}

   21     ~FalconCLIMinimal() {}

   22 

   23     // CLIBase proxy method

   24     void addOptions(int value)

   25     {

   26         FalconCLIBase::addOptions(value);

   27     }

   28 

   29     // CLIBase proxy method

   30     bool parseOptions(int argc, char** argv)

   31     {

   32         if(!FalconCLIBase::parseOptions(argc, argv)) return false;

   33         return true;

   34     }

   35 

   36     // run main loop

   37     void runLoop()

   38     {

   39         // set falcon kinematic

   40         m_falconDevice->setFalconKinematic<FalconKinematicStamper>();

   41 

   42         boost::array<double, 3> pos;

   43         boost::array<double, 3> force;

   44 

   45         while(run_app)

   46         {

   47             if(!m_falconDevice->runIOLoop()) continue;

   48 

   49             // get position vector

   50             pos = m_falconDevice->getPosition();

   51 

   52             // please DO send your force or 0-force everytime

   53             // libnifalcon doesn't set it for you and it may cause damage to your device

   54 

   55             // 0-force

   56             force[0] = 0;

   57             force[1] = 0;

   58             force[2] = 0;

   59 

   60             // set position vector

   61             m_falconDevice->setForce(force);

   62 

   63             // print data

   64             std::cout << std::setprecision(6) << pos[0] << " " << pos[1] << " " <<pos[2] << " : " << force[0] << " " << force[1] << " " << force[2] << std::endl;

   65         }

   66     }

   67 };

   68 

   69 int main(int argc, char** argv)

   70 {

   71     // create an instance of our FalconCLIBase inherited class

   72     FalconCLIMinimal f;

   73 

   74     // add command line options

   75     f.addOptions(FalconCLIMinimal::DEVICE_OPTIONS | FalconCLIMinimal::COMM_OPTIONS | FalconCLIMinimal::FIRMWARE_OPTIONS);

   76 

   77     // parse options

   78     if(!f.parseOptions(argc, argv)) return 0;

   79 

   80     // run the main loop

   81     f.runLoop();

   82 

   83     return 0;

   84 }