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++)
172 for(
int i=0; i<2; i++)
187 prop.put(
"device",
"ecan");
189 prop.put(
"CanTxTimeout", 500);
190 prop.put(
"CanRxTimeout", 500);
191 prop.put(
"CanDeviceNum", 2);
199 if (!driver.isValid())
201 fprintf(stderr,
"Error opening PolyDriver check parameters\n");
205 driver.view(iCanBus);
206 driver.view(iBufferFactory);
209 if (!iCanBus->canSetBaudRate(0))
210 fprintf(stderr,
"Error setting baud rate\n");
213 iCanBus->canIdAdd(0x35a);
214 iCanBus->canIdAdd(0x35b);
217 iCanBus->canIdAdd(0x177);
238 fp = fopen(
"output.dat",
"w");
246 bool res=iCanBus->canRead(messageBuffer, messages, &readMessages);
249 for(
int i=0; i<readMessages; i++)
251 if (messageBuffer[i].getId() == 0x35a)
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;
262 if (messageBuffer[i].getId() == 0x35b)
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;
271 if (messageBuffer[i].getId() == 0x171)
273 position[0] = (messageBuffer[i].getData()[3]<<24)|(messageBuffer[i].getData()[2]<<16)|(messageBuffer[i].getData()[1]<<8)|messageBuffer[i].getData()[0];
275 if (messageBuffer[i].getId() == 0x177)
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];
293 if (csamples.add_sample(torque[0])) fft.do_fft(csamples);
299 if(
log_start)
fprintf(fp,
"%d %d %d %d %d\n",cnt,kp[0],pid[0],dutyCycle[0],torque[0]);