iCub-main
Loading...
Searching...
No Matches
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
32using namespace yarp::dev;
33using namespace yarp::sig;
34using namespace yarp::os;
35using namespace yarp;
36
37using namespace std;
38
41const int localBufferSize=2048;
42bool 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;
121class 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
158public:
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
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
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
374signed 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
385int 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}
void threadRelease()
Definition main.cpp:358
void run()
Definition main.cpp:241
bool threadInit()
Definition main.cpp:181
SnifferThread(int r=SNIFFER_THREAD_RATE)
Definition main.cpp:159
bool done
Definition main.cpp:42
const int SNIFFER_THREAD_RATE
Definition main.cpp:39
const int localBufferSize
Definition main.cpp:41
bool log_start
Definition main.cpp:120
const int CAN_DRIVER_BUFFER_SIZE
Definition main.cpp:40
const int SNIFFER_THREAD_RATE
Definition main.cpp:52
const int localBufferSize
Definition main.cpp:54
const int CAN_DRIVER_BUFFER_SIZE
Definition main.cpp:53
int main()
Definition main.cpp:67
Copyright (C) 2008 RobotCub Consortium.
fprintf(fid,'\n')
fclose(fileID)