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, 6 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.