iCub-main
main.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
17 #include <stdio.h>
18 #include <yarp/os/Network.h>
19 #include <yarp/dev/ControlBoardInterfaces.h>
20 #include <yarp/dev/PolyDriver.h>
21 #include <yarp/dev/CanBusInterface.h>
22 #include <yarp/os/Time.h>
23 #include <yarp/sig/Vector.h>
24 #include <yarp/os/PeriodicThread.h>
25 #include "fft.h"
26 #include "hist.h"
27 #include <iostream>
28 #include <iomanip>
29 #include <string>
30 #include <bitset>
31 
32 using namespace yarp::dev;
33 using namespace yarp::sig;
34 using namespace yarp::os;
35 using namespace yarp;
36 
37 using namespace std;
38 
39 const int SNIFFER_THREAD_RATE=50;
40 const int CAN_DRIVER_BUFFER_SIZE=2047;
41 const int localBufferSize=2048;
42 bool done=false;
43 
44 
45 
46 /******************************************************************************/
47 /*void test01 ( void )
48 {
49 #define F_SAMPLE 1000
50  int i;
51  fftw_complex *in;
52  fftw_complex *in2;
53  int n = 1000;
54  fftw_complex *out;
55  fftw_plan plan_forward;
56  double *fft;
57  double *freq;
58 
59  //Create the input array
60  in = (fftw_complex*)fftw_malloc ( sizeof ( fftw_complex ) * n );
61 
62 #define CONV_SIN 6.28/360*0.36
63 
64  for ( i = 0; i < n; i++ )
65  {
66  in[i][0] = 1*sin(5*i*CONV_SIN)+3*sin(22*i*CONV_SIN);
67  in[i][1] = 0;
68  }
69 
70  printf ( "\n" );
71  printf ( " Input Data:\n" );
72  printf ( "\n" );
73 
74  for ( i = 0; i < n; i++ )
75  {
76  printf ( " %3d %12f %12f\n", i, in[i][0], in[i][1] );
77  }
78 
79  //Create the output array.
80  out = (fftw_complex*)fftw_malloc ( sizeof ( fftw_complex ) * n );
81  fft = new double[n];
82 
83  plan_forward = fftw_plan_dft_1d ( n, in, out, FFTW_FORWARD, FFTW_ESTIMATE );
84 
85  fftw_execute ( plan_forward );
86 
87  printf ( "\n" );
88  printf ( " Output FFT Coefficients:\n" );
89  printf ( "\n" );
90 
91  for ( i = 0; i < n; i++ )
92  {
93  fft[i] = 2*norm_abs(out[i][0],out[i][1])/n ;
94  }
95 
96  int NNFT = n; //400
97  freq = new double[NNFT/2];
98  for ( i = 0; i < NNFT/2; i++ )
99  {
100  freq[i] = float(i)/(float(NNFT)/2);
101 
102  freq[i] = F_SAMPLE/2*freq[i];
103  }
104 
105  for ( i = 0; i < 80; i++ )
106  {
107  printf ( " %12f %12f \n", freq[i], fft[i]);
108  }
109 
110 
111  //Free up the allocated memory.
112  fftw_destroy_plan ( plan_forward );
113 
114  fftw_free ( in );
115  fftw_free ( out );
116 
117  return;
118 }
119 */
120  bool log_start=false;
121 class SnifferThread: public PeriodicThread
122 {
123  PolyDriver driver;
124  ICanBus *iCanBus;
125  ICanBufferFactory *iBufferFactory;
126  CanBuffer messageBuffer;
127  unsigned long int cnt;
128  FILE *fp;
129 
130  /* dimension of local buffer, number of recieved messages */
131  unsigned int messages, readMessages;
132 
133  /* variables to be sniffed from the Can Bus */
134  unsigned int position[2];
135  signed short pwm[2];
136  signed short pid[2];
137  signed short kp[2];
138  signed short dutyCycle[2];
139  signed short torque[2];
140  signed short commut[2];
141  unsigned short unsigned_gaugeData[6];
142  signed short signed_gaugeData[6];
143  fft_samples samples;
144  fft_cyclic_sample_buffer csamples;
145  fft_performer fft;
146  hist_performer hist;
147 
148  unsigned char cycleIndex;
149  signed short filterCoefficients[6];
150  signed short dataBuffer[6];
151 
152  signed short filterData(signed short datum);
153 
154 
155 
156 
157 
158 public:
159  SnifferThread(int r=SNIFFER_THREAD_RATE): PeriodicThread((double)r/1000.0)
160  {
161  messages = localBufferSize;
162  cycleIndex = 0;
163  filterCoefficients[0] = 1;
164  filterCoefficients[1] = 1;
165  filterCoefficients[2] = 1;
166  filterCoefficients[3] = 1;
167  filterCoefficients[4] = 1;
168  filterCoefficients[5] = 1;
169  for(int i=0; i<6; i++)
170  dataBuffer[i] = 0;
171 
172  for(int i=0; i<2; i++)
173  {
174  position[i] = 0;
175  pwm[i] = 0;
176  dutyCycle[i] = 0;
177  }
178 
179  }
180 
181  bool threadInit()
182  {
183  /* load configuration parameters into the options property collector */
184  Property prop;
185 
186  /* set driver properties */
187  prop.put("device", "ecan");
188 
189  prop.put("CanTxTimeout", 500);
190  prop.put("CanRxTimeout", 500);
191  prop.put("CanDeviceNum", 2);
192 
193  prop.put("CanTxQueue", CAN_DRIVER_BUFFER_SIZE);
194  prop.put("CanRxQueue", CAN_DRIVER_BUFFER_SIZE);
195 
196  /* open driver */
197  driver.open(prop);
198 
199  if (!driver.isValid())
200  {
201  fprintf(stderr, "Error opening PolyDriver check parameters\n");
202  return false;
203  }
204 
205  driver.view(iCanBus);
206  driver.view(iBufferFactory);
207 
208  /* set the baud rate (0 is defaul for 1Mb/s) */
209  if (!iCanBus->canSetBaudRate(0))
210  fprintf(stderr, "Error setting baud rate\n");
211 
212  /* add the id of can messages to be read */
213  iCanBus->canIdAdd(0x35a);
214  iCanBus->canIdAdd(0x35b);
215 // iCanBus->canIdAdd(0x171);
216 // iCanBus->canIdAdd(0x172);
217  iCanBus->canIdAdd(0x177);
218 
219  messageBuffer=iBufferFactory->createBuffer(localBufferSize);
220 
221  /* turn force-torque sensor on
222  messageBuffer[0].setId(0x205);
223  messageBuffer[0].setLen(2);
224  messageBuffer[0].getData()[0]=7;
225  messageBuffer[0].getData()[1]=0;
226  bool res=iCanBus->canWrite(messageBuffer,1,&readMessages);
227 
228  if (res)
229  return true;
230  else
231  {
232  fprintf(stderr,"Error starting the force/torque sensor\n");
233  return false;
234  }
235  */
236 
237  cnt = 0;
238  fp = fopen("output.dat","w");
239  }
240 
241  void run()
242  {
243  readMessages = 0;
244  /* read from the Can Bus messages with the id that has been
245  specified */
246  bool res=iCanBus->canRead(messageBuffer, messages, &readMessages);
247 
248  /* cycle from zero to the number of read messages */
249  for(int i=0; i<readMessages; i++)
250  {
251  if (messageBuffer[i].getId() == 0x35a)
252  {
253  unsigned_gaugeData[0] = (messageBuffer[i].getData()[1]<<8)|messageBuffer[i].getData()[0];
254  unsigned_gaugeData[1] = (messageBuffer[i].getData()[3]<<8)|messageBuffer[i].getData()[2];
255  unsigned_gaugeData[2] = (messageBuffer[i].getData()[5]<<8)|messageBuffer[i].getData()[4];
256  signed_gaugeData[0] = unsigned_gaugeData[0]-0x7fff;
257  signed_gaugeData[1] = unsigned_gaugeData[1]-0x7fff;
258  signed_gaugeData[2] = unsigned_gaugeData[2]-0x7fff;
259  //if (csamples.add_sample(signed_gaugeData[0])) fft.do_fft(csamples);
260  //if (hist.add_sample(unsigned_gaugeData[0])) hist.do_hist();
261  }
262  if (messageBuffer[i].getId() == 0x35b)
263  {
264  unsigned_gaugeData[3] = (messageBuffer[i].getData()[1]<<8)|messageBuffer[i].getData()[0];
265  unsigned_gaugeData[4] = (messageBuffer[i].getData()[3]<<8)|messageBuffer[i].getData()[2];
266  unsigned_gaugeData[5] = (messageBuffer[i].getData()[5]<<8)|messageBuffer[i].getData()[4];
267  signed_gaugeData[3] = unsigned_gaugeData[3]-0x7fff;
268  signed_gaugeData[4] = unsigned_gaugeData[4]-0x7fff;
269  signed_gaugeData[5] = unsigned_gaugeData[5]-0x7fff;
270  }
271  if (messageBuffer[i].getId() == 0x171)
272  /* recompose scalar value from four bytes */
273  position[0] = (messageBuffer[i].getData()[3]<<24)|(messageBuffer[i].getData()[2]<<16)|(messageBuffer[i].getData()[1]<<8)|messageBuffer[i].getData()[0];
274 
275  if (messageBuffer[i].getId() == 0x177) //COMMUT
276  {
277  //commut[0] = (messageBuffer[i].getData()[5]<<8)|messageBuffer[i].getData()[4];
278  torque[0] = (messageBuffer[i].getData()[1]<<8)|messageBuffer[i].getData()[0];
279  dutyCycle[0] = (messageBuffer[i].getData()[3]<<8)|messageBuffer[i].getData()[2];
280  pid[0] = (messageBuffer[i].getData()[5]<<8)|messageBuffer[i].getData()[4];
281  kp[0] = (messageBuffer[i].getData()[7]<<8)|messageBuffer[i].getData()[6];
282 
283  /* if (kp[0]==1000)
284  {
285  this->stop();
286  fclose(fp);
287  exit(0);
288  };*/
289 
290  //FFT
291  //if (samples.add_sample(torque[0])) fft.do_fft(samples);
292  //if (samples.add_sample(dutyCycle[0])) fft.do_fft(samples);
293  if (csamples.add_sample(torque[0])) fft.do_fft(csamples);
294 
295  if (kp[0]==30 && log_start==false) log_start=true;
296 
297  log_start=true;
298 
299  if(log_start) fprintf(fp,"%d %d %d %d %d\n",cnt,kp[0],pid[0],dutyCycle[0],torque[0]);
300  cnt++;
301  }
302  }
303 
304 
305 /*
306  if (messageBuffer[i].getId() == 0x172)
307  {
308  pwm[0] = (messageBuffer[i].getData()[1]<<8)|messageBuffer[i].getData()[0];
309  dutyCycle[0] = (messageBuffer[i].getData()[5]<<8)|messageBuffer[i].getData()[4];
310  }
311 */
312 
313 // fprintf(fp,"%d %d %d %d %d %d %d\n",cnt,gaugeData[0],gaugeData[1],gaugeData[2],gaugeData[3],gaugeData[4],gaugeData[5]);
314  //fprintf(fp,"%d %d %d %d %d %d\n",cnt,signed_gaugeData[5],torque[0],dutyCycle[0],pid[0],kp[0]);
315 /*
316  if (cnt==50000)
317  {
318  this->stop();
319  done =true;
320  }
321 */
322 
323  /*
324  cout<<setiosflags(ios::fixed)
325  <<setw(10)<<"commut:"
326  <<setw(8)<<setprecision(3)<<commut[0]
327  <<" duty cycle:"
328  <<setw(8)<<setprecision(3)<<dutyCycle[0]
329  <<" gauge 5:"
330  <<setw(8)<<setprecision(3)<<signed_gaugeData[5]
331  <<" dsp torque:"
332  <<setw(8)<<setprecision(3)<<torque[0]
333  <<" pid:"
334  <<setw(8)<<setprecision(3)<<pid[0]
335  <<" kp:"
336  <<setw(8)<<setprecision(3)<<kp[0]
337  <<"\r";
338  */
339  /*
340  cout<<setiosflags(ios::fixed)
341  <<" "
342  <<setw(8)<<setprecision(3)<<cnt
343  <<" "
344  <<setw(8)<<setprecision(3)<<gaugeData[0]
345  <<" "
346  <<setw(8)<<setprecision(3)<<gaugeData[1]
347  <<" "
348  <<setw(8)<<setprecision(3)<<gaugeData[2]
349  <<" "
350  <<setw(8)<<setprecision(3)<<gaugeData[3]
351  <<" "
352  <<setw(8)<<setprecision(3)<<gaugeData[4]
353  <<" "
354  <<setw(8)<<setprecision(3)<<gaugeData[5]<<"\r";
355  */
356  }
357 
358  void threadRelease()
359  {
360  /* turn force-torque sensor off
361  messageBuffer[0].setId(0x205);
362  messageBuffer[0].setLen(2);
363  messageBuffer[0].getData()[0]=7;
364  messageBuffer[0].getData()[1]=1;
365  bool res=iCanBus->canWrite(messageBuffer,1,&readMessages);
366  */
367 
368  iBufferFactory->destroyBuffer(messageBuffer);
369  driver.close();
370  fclose(fp);
371  }
372 };
373 
374 signed short SnifferThread::filterData(signed short datum)
375 {
376  int accumulator = 0;
377  dataBuffer[cycleIndex] = datum;
378  cycleIndex = (cycleIndex==5 ? 0: cycleIndex++);
379  for(int i=0; i<6; i++)
380  accumulator = accumulator + (dataBuffer[i]*filterCoefficients[i]);
381  return accumulator/6;
382 }
383 
384 
385 int main(int argc, char *argv[])
386 {
387  SnifferThread thread;
388 
389  thread.start();
390 
391  std::string input;
392  while(!done)
393  {
394 /* std::cin>>input;
395  if (input=="quit")
396  done=true;*/
397  }
398 
399  thread.stop();
400 }
SnifferThread::threadRelease
void threadRelease()
Definition: main.cpp:358
SnifferThread::run
void run()
Definition: main.cpp:241
SnifferThread::threadInit
bool threadInit()
Definition: main.cpp:181
SnifferThread::SnifferThread
SnifferThread(int r=SNIFFER_THREAD_RATE)
Definition: main.cpp:159
main
int main(int argc, char *argv[])
Definition: main.cpp:31
done
bool done
Definition: main.cpp:42
SNIFFER_THREAD_RATE
const int SNIFFER_THREAD_RATE
Definition: main.cpp:39
localBufferSize
const int localBufferSize
Definition: main.cpp:41
log_start
bool log_start
Definition: main.cpp:120
CAN_DRIVER_BUFFER_SIZE
const int CAN_DRIVER_BUFFER_SIZE
Definition: main.cpp:40
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
fp
static struct bpf_program fp
Definition: pcap_wrapper_linux.cpp:89
fprintf
fprintf(fid,'\n')
fclose
fclose(fileID)
Generated on Fri Apr 26 2024 13:29:10 for iCub-main by doxygen 1.9.1