iCub-main
strainInterface.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2017 iCub Facility - Istituto Italiano di Tecnologia
5  * Author: Marco Accame
6  * email: marco.accame@iit.it
7  * website: www.robotcub.org
8  * Permission is granted to copy, distribute, and/or modify this program
9  * under the terms of the GNU General Public License, version 2 or any
10  * later version published by the Free Software Foundation.
11  *
12  * A copy of the license can be found at
13  * http://www.robotcub.org/icub/license/gpl.txt
14  *
15  * This program is distributed in the hope that it will be useful, but
16  * WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18  * Public License for more details
19 */
20 
21 
22 
23 #include "strainInterface.h"
24 
25 //#include "driver.h"
26 //#include "downloader.h"
27 #include <yarp/os/Time.h>
28 #include <yarp/os/Log.h>
29 #include <stdlib.h> //added for abs
30 #include <string.h>
31 
32 
33 
34 
35 using namespace yarp::dev;
36 using namespace yarp::os;
37 using namespace std;
38 
39 
40 //strainInterface::strainInterface()
41 //{
42 // downloader.set_verbose(false);
43 // config.load_default();
44 //}
45 
46 //strainInterface::~strainInterface()
47 //{
48 
49 //}
50 
51 
52 bool strainInterface::open(const Config &cfg)
53 {
54  if(true == opened)
55  {
56  close();
57  }
58 
59  config = cfg;
60 
61  if(true == downloader.connected)
62  {
64  yError() << "strainInterface::open(): failure because ... Driver Stopped because it was running .... very strange i should have called a strainInterface::close()";
65  return false;
66  }
67 
68 
69  yarp::os::Property params;
70 
71  params.put("device", config.get_networkstring().c_str());
72  if(Network::ETH == config.network)
73  {
74  unsigned int localAddr = (10<<24)|(0<<16)|(1<<8)|104; // 10.0.1.104
75  unsigned int remoteAddr = (10<<24)|(0<<16)|(1<<8)|1; // 10.0.1.1
76 
77  params.put("local", int( localAddr));
78  params.put("remote",int(remoteAddr));
79  params.put("canid", config.get_canbus());
80 
81  }
82  else
83  {
84  int networkId = 0;
85  params.put("canDeviceNum", networkId);
86  params.put("canTxQueue", 64);
87  params.put("canRxQueue", 64);
88  params.put("canTxTimeout", 2000);
89  params.put("canRxTimeout", 2000);
90  }
91 
92  //try to connect to the driver
93  int ret = downloader.initdriver(params);
94 
95  if (0 != ret)
96  {
97  if(-2 == ret)
98  {
99  yDebug("Init ETH driver: The ETH board has just jumped to eUpdater\n Connect again");
100  }
101  else
102  {
103  yError("Init driver failed: Hardware busy or not connected?!");
104  }
105  return false;
106  }
107 
108  yDebug("Driver Connected");
109 
110  opened = true;
111 
112  return true;
113 }
114 
115 
117 {
118  downloader.strain_stop_sampling(config.get_canbus(), config.get_canaddress());
120  config.load_default();
121  opened = false;
122 
123  return true;
124 }
125 
126 
127 bool strainInterface::print(const vector<cDownloader::strain_value_t> &values, FILE *fp)
128 {
129  if(false == opened)
130  {
131  yError() << "strainInterface::print(): you must call strainInterface::open() first";
132  return false;
133  }
134 
135  // if fp is NULL, i dont print to file. the format is the one accepted for calib of strain
136 
137  // printf("Acquired %d samples. Ready! \n", static_cast<int>(values.size()));
138 
139  for(size_t n=0; n<values.size(); n++)
140  {
141  unsigned short unsigned_gaugeData[6] = {0};
142  signed short signed_gaugeData[6] = {0};
143 
144  for(size_t c=0; c<6; c++)
145  {
146  unsigned_gaugeData[c] = values[values.size()-1-n].channel[c];
147  signed_gaugeData[c] = unsigned_gaugeData[c]-0x7fff;
148  }
149 
150  if(NULL != fp)
151  {
152  fprintf(fp,"%d %d %d %d %d %d %d\n",static_cast<int>(n),signed_gaugeData[0],signed_gaugeData[1],signed_gaugeData[2],signed_gaugeData[3],signed_gaugeData[4],signed_gaugeData[5]);
153  }
154  yDebug("samples:%3d [0]:%+6d [1]:%+6d [2]:%+6d [3]:%+6d [4]:%+6d [5]:%+6d\r\n",static_cast<int>(n),signed_gaugeData[0],signed_gaugeData[1],signed_gaugeData[2],signed_gaugeData[3],signed_gaugeData[4],signed_gaugeData[5]);
155 
156  }
157 
158  return true;
159 }
160 
161 
162 bool strainInterface::get(const unsigned int number, vector<cDownloader::strain_value_t> &values)
163 {
164  if(false == opened)
165  {
166  yError() << "strainInterface::get(): you must call strainInterface::open() first";
167  return false;
168  }
169 
170  double t0 = yarp::os::SystemClock::nowSystem();
171 
172  yDebug() << "strainInterface::get(): is acquiring" << number << "from the 6 channels. Please wait ...";
173 
174  const bool calibmode = false;
175  downloader.strain_acquire_start(config.get_canbus(), config.get_canaddress(), config.get_txrate(), calibmode);
176  downloader.strain_acquire_get(config.get_canbus(), config.get_canaddress(), values, number);
177  //yarp::os::SystemClock::delaySystem(0.100); i used it to test the flush operation of strain_acquire_stop()....
178  downloader.strain_acquire_stop(config.get_canbus(), config.get_canaddress());
179 
180  double t1 = yarp::os::SystemClock::nowSystem();
181 
182 
183  yDebug() << "strainInterface::get() has succesfully acquired" << values.size() << "values of the 6 channels in" << (t1-t0) << "seconds";
184  yDebug() << "the values are:";
185  for(int i=0; i<values.size(); i++)
186  {
187  yDebug() << "#" << i+1 << "=" << values[i].channel[0] << values[i].channel[1] << values[i].channel[2] << values[i].channel[3] <<
188  values[i].channel[4] << values[i].channel[5] << values[i].valid;
189  }
190 
191  return true;
192 }
193 
194 
195 // end of file
196 
197 
int strain_acquire_start(int bus, int target_id, uint8_t txratemilli=20, bool calibmode=true, strain_acquisition_mode_t acqmode=strain_acquisition_mode_streaming, string *errorstring=NULL)
int strain_acquire_stop(int bus, int target_id, strain_acquisition_mode_t acqmode=strain_acquisition_mode_streaming, string *errorstring=NULL)
int strain_acquire_get(int bus, int target_id, vector< strain_value_t > &values, const unsigned int howmany=10, void(*updateProgressBar)(void *, float)=NULL, void *arg=NULL, strain_acquisition_mode_t acqmode=strain_acquisition_mode_streaming, const unsigned int maxerrors=1, string *errorstring=NULL)
int strain_stop_sampling(int bus, int target_id, string *errorstring=NULL)
int initdriver(yarp::os::Searchable &config, bool verbose=true)
Definition: downloader.cpp:142
int stopdriver()
Definition: downloader.cpp:122
bool connected
Definition: downloader.h:158
bool get(const unsigned int number, vector< cDownloader::strain_value_t > &values)
bool print(const vector< cDownloader::strain_value_t > &values, FILE *fp=NULL)
bool open(const Config &cfg=Config())
int n
cDownloader downloader
Definition: main.cpp:148
unsigned int remoteAddr
Definition: main.cpp:154
unsigned int localAddr
Definition: main.cpp:153
int networkId
Definition: main.cpp:151
static struct bpf_program fp
fprintf(fid,'\n')