source: flair-src/trunk/lib/FlairSensorActuator/src/BlCtrlV2_x4_speed.cpp@ 10

Last change on this file since 10 was 3, checked in by Sanahuja Guillaume, 8 years ago

sensoractuator
File size: 13.7 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2013/04/29
6// filename: BlCtrlV2_x4_speed.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: objet integrant les moteurs i2c, controle en vitesse
14//
15//
16/*********************************************************************/
17
18#include "BlCtrlV2_x4_speed.h"
19#include "I2cPort.h"
20#include <TabWidget.h>
21#include <Tab.h>
22#include <GroupBox.h>
23#include <SpinBox.h>
24#include <DoubleSpinBox.h>
25#include <ComboBox.h>
26#include <PushButton.h>
27#include <cvmatrix.h>
28#include <Mutex.h>
29#include <FrameworkManager.h>
30#include <DataPlot1D.h>
31#include <math.h>
32#include <string.h>
33
34#define TAU_US 1000
35
36using std::string;
37using namespace flair::core;
38using namespace flair::gui;
39
40namespace flair
41{
42namespace actuator
43{
44BlCtrlV2_x4_speed::BlCtrlV2_x4_speed(FrameworkManager* parent,string name,I2cPort* i2cport,uint8_t base_address,uint8_t priority) : Thread(parent,name,priority),IODevice(parent,name)
45{
46 this->i2cport=i2cport;
47 slave_address=base_address;
48 tested_motor=-1;
49 enabled=false;
50 int_av_g=0;
51 int_av_d=0;
52 int_ar_g=0;
53 int_ar_d=0;
54
55 //flight time
56 FILE *file;
57 file=fopen("/etc/flight_time","r");
58 if (file == NULL)
59 {
60 Printf("fichier d'info de vol vide\n");
61 time_sec=0;
62 }
63 else
64 {
65 char ligne[32];
66 fgets(ligne, 32, file);
67 time_sec=atoi(ligne);
68 Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
69 fclose(file);
70 }
71
72 //station sol
73 main_tab=new Tab(parent->GetTabWidget(),name);
74 tab=new TabWidget(main_tab->NewRow(),name);
75 Tab *sensor_tab=new Tab(tab,"Reglages");
76 reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
77 poles=new SpinBox(reglages_groupbox->NewRow(),"nb poles",0,255,1);
78 kp=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"kp",0.,255,0.001,4);
79 ki=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"ki",0.,255,0.001,4);
80 min=new SpinBox(reglages_groupbox->NewRow(),"min pwm",0.,2048,1);
81 max=new SpinBox(reglages_groupbox->LastRowLastCol(),"max pwm",0.,2048,1);
82 test=new SpinBox(reglages_groupbox->LastRowLastCol(),"test value",0.,2048,1);
83 start_value=new SpinBox(reglages_groupbox->NewRow(),"valeur demarrage",0,10000,10);
84 trim=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"pas decollage",0,1000,.1);
85
86 av_g=new ComboBox(reglages_groupbox->NewRow(),"avant gauche");
87 av_g->AddItem("1");
88 av_g->AddItem("2");
89 av_g->AddItem("3");
90 av_g->AddItem("4");
91 button_avg=new PushButton(reglages_groupbox->LastRowLastCol(),"test avg");
92
93 av_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"avant droite:");
94 av_d->AddItem("1");
95 av_d->AddItem("2");
96 av_d->AddItem("3");
97 av_d->AddItem("4");
98 button_avd=new PushButton(reglages_groupbox->LastRowLastCol(),"test avd");
99
100 ar_g=new ComboBox(reglages_groupbox->NewRow(),"arriere gauche:");
101 ar_g->AddItem("1");
102 ar_g->AddItem("2");
103 ar_g->AddItem("3");
104 ar_g->AddItem("4");
105 button_arg=new PushButton(reglages_groupbox->LastRowLastCol(),"test arg");
106
107 ar_d=new ComboBox(reglages_groupbox->LastRowLastCol(),"arriere droite:");
108 ar_d->AddItem("1");
109 ar_d->AddItem("2");
110 ar_d->AddItem("3");
111 ar_d->AddItem("4");
112 button_ard=new PushButton(reglages_groupbox->LastRowLastCol(),"test ard");
113
114 pas=new ComboBox(reglages_groupbox->NewRow(),"pas helice avant gauche:");
115 pas->AddItem("normal");
116 pas->AddItem("inverse");
117
118 input=new cvmatrix((IODevice*)this,8,1,floatType);
119
120 cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,2);
121 desc->SetElementName(0,0,"avant gauche");
122 desc->SetElementName(1,0,"arriere droite");
123 desc->SetElementName(2,0,"avant droite");
124 desc->SetElementName(3,0,"arriere gauche");
125
126 desc->SetElementName(0,1,"cons avant gauche");
127 desc->SetElementName(1,1,"cons arriere droite");
128 desc->SetElementName(2,1,"cons avant droite");
129 desc->SetElementName(3,1,"cons arriere gauche");
130 output=new cvmatrix((IODevice*)this,desc,floatType);
131
132
133
134/*
135
136//le 3ieme lu est la tension batteire
137 if(i2c_mutex!=NULL) i2c_mutex->GetMutex();
138 uint16_t pwm_moteur;
139 pwm_moteur=0;
140 ssize_t read;
141 uint8_t rx[8];
142 SetSlave(slave_address);
143
144 for(int j=0;j<10;j++)
145 {
146
147
148 WriteValue(pwm_moteur);
149
150
151 read = rt_dev_read(i2c_fd, rx, sizeof(rx));
152
153 if(read<0)
154 {
155 rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read (%s)\n",IODevice::ObjectName().c_str(),strerror(-read));
156 }
157 else if (read != sizeof(rx))
158 {
159 rt_printf("BlCtrlV2_x4_speed::BlCtrlV2_x4_speed: %s, erreur rt_dev_read %i/2\n",IODevice::ObjectName().c_str(),read);
160
161 }
162 for(int i=0;i<sizeof(rx);i++) printf("%i ",rx[i]);
163
164 printf("\n");
165
166 }
167
168 if(i2c_mutex!=NULL) i2c_mutex->ReleaseMutex();*/
169}
170
171BlCtrlV2_x4_speed::~BlCtrlV2_x4_speed(void)
172{
173 SafeStop();
174 Join();
175 delete main_tab;
176}
177
178void BlCtrlV2_x4_speed::UseDefaultPlot(void)
179{
180 Tab *plot_tab=new Tab(tab,"Mesures");
181 DataPlot1D *av_g_plot=new DataPlot1D(plot_tab->NewRow(),"avg",0,10000);
182 av_g_plot->AddCurve(output->Element(0,0));
183 av_g_plot->AddCurve(output->Element(0,1),DataPlot::Blue);
184 DataPlot1D *av_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"avd",0,10000);
185 av_d_plot->AddCurve(output->Element(2,0));
186 av_d_plot->AddCurve(output->Element(2,1),DataPlot::Blue);
187 DataPlot1D *ar_g_plot=new DataPlot1D(plot_tab->NewRow(),"arg",0,10000);
188 ar_g_plot->AddCurve(output->Element(3,0));
189 ar_g_plot->AddCurve(output->Element(3,1),DataPlot::Blue);
190 DataPlot1D *ar_d_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"ard",0,10000);
191 ar_d_plot->AddCurve(output->Element(1,0));
192 ar_d_plot->AddCurve(output->Element(1,1),DataPlot::Blue);
193}
194
195float BlCtrlV2_x4_speed::TrimValue(void)
196{
197 return (float)trim->Value();
198}
199
200int BlCtrlV2_x4_speed::StartValue(void)
201{
202 return start_value->Value();
203}
204
205void BlCtrlV2_x4_speed::Run(void)
206{
207 WarnUponSwitches(true);
208
209 SetPeriodUS(TAU_US);
210
211 while(!ToBeStopped())
212 {
213 WaitPeriod();
214
215 Update();
216 }
217
218 WarnUponSwitches(false);
219}
220
221void BlCtrlV2_x4_speed::Update(void)
222{
223 float u_roll,u_pitch,u_yaw,u_gaz;
224 float trim_roll,trim_pitch,trim_yaw;
225 float pwm[4];
226 uint16_t pwm_moteur[4];
227
228
229 //on prend une fois pour toute le mutex et on fait des accès directs
230 input->GetMutex();
231
232 u_roll=input->ValueNoMutex(0,0);
233 u_pitch=input->ValueNoMutex(1,0);
234 u_yaw=input->ValueNoMutex(2,0);
235 u_gaz=input->ValueNoMutex(3,0)+input->ValueNoMutex(7,0)*input->ValueNoMutex(7,0);//ugaz+trim*trim
236 trim_roll=input->ValueNoMutex(4,0);
237 trim_pitch=input->ValueNoMutex(5,0);
238 trim_yaw=input->ValueNoMutex(6,0);
239
240 input->ReleaseMutex();
241
242 if(pas->CurrentIndex()==1)
243 {
244 trim_yaw=-trim_yaw;
245 u_yaw=-u_yaw;
246 }
247
248 //rt_printf("%f %f %f %f\n",u_roll,u_pitch,u_yaw,u_gaz);
249 //if(u_gaz!=0) rt_printf("gaz: %f\n",u_gaz);
250
251 //avant gauche
252 if(u_gaz+u_pitch+u_roll+u_yaw>0)
253 {
254 pwm[0]=trim_pitch+trim_roll+trim_yaw+sqrtf(u_gaz+u_pitch+u_roll+u_yaw);
255 }else
256 {
257 pwm[0]=trim_pitch+trim_roll+trim_yaw;
258 }
259
260 //arriere gauche
261 if(u_gaz-u_pitch+u_roll-u_yaw>0)
262 {
263 pwm[3]=-trim_pitch+trim_roll-trim_yaw+sqrtf(u_gaz-u_pitch+u_roll-u_yaw);
264 }else
265 {
266 pwm[3]=-trim_pitch+trim_roll-trim_yaw;
267 }
268
269 //arriere droit
270 if(u_gaz-u_pitch-u_roll+u_yaw>0)
271 {
272 pwm[1]=-trim_pitch-trim_roll+trim_yaw+sqrtf(u_gaz-u_pitch-u_roll+u_yaw);
273 }else
274 {
275 pwm[1]=-trim_pitch-trim_roll+trim_yaw;
276 }
277
278 //avant droit
279 if(u_gaz+u_pitch-u_roll-u_yaw>0)
280 {
281 pwm[2]=trim_pitch-trim_roll-trim_yaw+sqrtf(u_gaz+u_pitch-u_roll-u_yaw);
282 }else
283 {
284 pwm[2]=trim_pitch-trim_roll-trim_yaw;
285 }
286
287
288 int_av_g+=ki->Value()*(pwm[0]-speed_av_g);
289 pwm[0]=kp->Value()*(pwm[0]-speed_av_g)+int_av_g;
290
291 int_ar_g+=ki->Value()*(pwm[3]-speed_ar_g);
292 pwm[3]=kp->Value()*(pwm[3]-speed_ar_g)+int_ar_g;
293
294 int_ar_d+=ki->Value()*(pwm[1]-speed_ar_d);
295 pwm[1]=kp->Value()*(pwm[1]-speed_ar_d)+int_ar_d;
296
297 int_av_d+=ki->Value()*(pwm[2]-speed_av_d);
298 pwm[2]=kp->Value()*(pwm[2]-speed_av_d)+int_av_d;
299
300//rt_printf("%f\n",pwm[0]);
301 for(int i=0;i<4;i++) pwm_moteur[i]=SatPWM(pwm[i],min->Value(),max->Value());
302
303 if(button_avg->Clicked()==true)
304 {
305 tested_motor=0;
306 StartTest();
307 }
308 if(button_avd->Clicked()==true)
309 {
310 tested_motor=2;
311 StartTest();
312 }
313 if(button_arg->Clicked()==true)
314 {
315 tested_motor=3;
316 StartTest();
317 }
318 if(button_ard->Clicked()==true)
319 {
320 tested_motor=1;
321 StartTest();
322 }
323
324 if(tested_motor!=-1)
325 {
326 for(int i=0;i<4;i++)
327 {
328 pwm_moteur[i]=0;
329 }
330 pwm_moteur[tested_motor]=(uint16_t)test->Value();
331
332 if(GetTime()>(start_time+2*1000000000)) StopTest();
333 }
334
335
336 i2cport->GetMutex();
337
338 if(enabled==true)
339 {
340 i2cport->SetSlave(slave_address+av_g->CurrentIndex());
341 WriteValue(pwm_moteur[0]);
342
343 i2cport->SetSlave(slave_address+av_d->CurrentIndex());
344 WriteValue(pwm_moteur[2]);
345
346 i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
347 WriteValue(pwm_moteur[3]);
348
349 i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
350 WriteValue(pwm_moteur[1]);
351
352 }
353 else
354 {
355 for(int i=0;i<4;i++)
356 {
357 i2cport->SetSlave(slave_address+i);
358 WriteValue(0);
359 }
360 int_av_g=0;
361 int_av_d=0;
362 int_ar_g=0;
363 int_ar_d=0;
364 }
365
366 i2cport->SetSlave(slave_address+av_g->CurrentIndex());
367 speed_av_g=GetSpeed();
368
369 i2cport->SetSlave(slave_address+av_d->CurrentIndex());
370 speed_av_d=GetSpeed();
371
372 i2cport->SetSlave(slave_address+ar_g->CurrentIndex());
373 speed_ar_g=GetSpeed();
374
375 i2cport->SetSlave(slave_address+ar_d->CurrentIndex());
376 speed_ar_d=GetSpeed();
377
378 i2cport->ReleaseMutex();
379
380
381 //on prend une fois pour toute le mutex et on fait des accès directs
382 output->GetMutex();
383 output->SetValueNoMutex(0,0,speed_av_g);
384 output->SetValueNoMutex(1,0,speed_ar_d);
385 output->SetValueNoMutex(2,0,speed_av_d);
386 output->SetValueNoMutex(3,0,speed_ar_g);
387 // rt_printf("%i %i %i %i\n",pwm_moteur[0],pwm_moteur[1],pwm_moteur[2],pwm_moteur[3]);
388 output->ReleaseMutex();
389
390 output->SetDataTime(GetTime());
391 ProcessUpdate(output);
392
393}
394
395void BlCtrlV2_x4_speed::StartTest(void)
396{
397 start_time=GetTime();
398 SetEnabled(true);
399}
400
401void BlCtrlV2_x4_speed::StopTest(void)
402{
403 SetEnabled(false);
404 tested_motor=-1;
405}
406
407uint16_t BlCtrlV2_x4_speed::SatPWM(float vel_cons,uint16_t min,uint16_t max)
408{
409 uint16_t sat_value=(uint16_t)vel_cons;
410
411 if(vel_cons>((float)sat_value+0.5)) sat_value++;
412
413 if(vel_cons<(float)min) sat_value=min;
414 if(vel_cons>(float)max) sat_value=max;
415
416 return sat_value;
417}
418
419void BlCtrlV2_x4_speed::LockUserInterface(void)
420{
421 reglages_groupbox->setEnabled(false);
422}
423
424void BlCtrlV2_x4_speed::UnlockUserInterface(void)
425{
426 reglages_groupbox->setEnabled(true);
427}
428
429void BlCtrlV2_x4_speed::SetEnabled(bool status)
430{
431 enabled=status;
432 if(enabled==true)
433 {
434 LockUserInterface();
435
436 flight_start_time = GetTime();
437 }
438 else
439 {
440 UnlockUserInterface();
441
442 Time now= GetTime();
443 int t_sec;
444 FILE *file;
445 char ligne[32];
446
447 t_sec=(now - flight_start_time)/1000000000;
448 time_sec+=t_sec;
449
450 Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
451 Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
452
453 file=fopen("/etc/flight_time","w");
454 if (file == NULL)
455 {
456 Thread::Err("Erreur a l'ouverture du fichier d'info vol\n");
457 }
458 else
459 {
460 sprintf(ligne,"%i",time_sec);
461 fputs(ligne,file);
462 fclose(file);
463 }
464 }
465}
466
467void BlCtrlV2_x4_speed::SetUroll(float value)
468{
469 input->SetValue(0,0,value);
470}
471
472void BlCtrlV2_x4_speed::SetUpitch(float value)
473{
474 input->SetValue(1,0,value);
475}
476
477void BlCtrlV2_x4_speed::SetUyaw(float value)
478{
479 input->SetValue(2,0,value);
480}
481
482void BlCtrlV2_x4_speed::SetUgaz(float value)
483{
484 input->SetValue(3,0,value);
485}
486
487void BlCtrlV2_x4_speed::SetRollTrim(float value)
488{
489 input->SetValue(4,0,value);
490}
491
492void BlCtrlV2_x4_speed::SetPitchTrim(float value)
493{
494 input->SetValue(5,0,value);
495}
496
497void BlCtrlV2_x4_speed::SetYawTrim(float value)
498{
499 input->SetValue(6,0,value);
500}
501
502void BlCtrlV2_x4_speed::SetGazTrim(float value)
503{
504 input->SetValue(7,0,value);
505}
506
507void BlCtrlV2_x4_speed::WriteValue(uint16_t value)
508{
509 unsigned char tx[2];
510 ssize_t written;
511
512 tx[0]=(unsigned char)(value>>3);//msb
513 tx[1]=16+8+(value&0x07);//16+8 pour recuperer la vitesse
514 written = i2cport->Write(tx, 2);
515 if(written<0)
516 {
517 Thread::Err("erreur rt_dev_write (%s)\n",strerror(-written));
518 }
519 else if (written != 2)
520 {
521 Thread::Err("erreur rt_dev_write %i/2\n",written);
522 }
523}
524
525float BlCtrlV2_x4_speed::GetSpeed(void)
526{
527 ssize_t read;
528 uint8_t value;
529 read = i2cport->Read(&value, 1);
530
531 if(read<0)
532 {
533 Thread::Err("erreur rt_dev_read (%s)\n",strerror(-read));
534 }
535 else if (read != 1)
536 {
537 Thread::Err("erreur rt_dev_read %i/2\n",read);
538
539 }
540
541 return value*780./poles->Value();
542}
543
544} // end namespace actuator
545} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.