source: flair-src/trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp @ 4

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

sensoractuator

File size: 5.6 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/11/14
6//  filename:   Bldc_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Virtual class for brushless drivers
14//
15//
16/*********************************************************************/
17#include "Bldc_impl.h"
18#include "Bldc.h"
19#include <GridLayout.h>
20#include <GroupBox.h>
21#include <DoubleSpinBox.h>
22#include <cvmatrix.h>
23#include <Label.h>
24#include <PushButton.h>
25#include <DataPlot1D.h>
26#include <TabWidget.h>
27#include <Tab.h>
28#include <FrameworkManager.h>
29#include <sstream>
30
31using std::string;
32using std::ostringstream;
33using namespace flair::core;
34using namespace flair::gui;
35using namespace flair::actuator;
36
37Bldc_impl::Bldc_impl(Bldc* self,Layout* layout,string name,uint8_t motors_count) {
38    this->self=self;
39    this->motors_count=motors_count;
40    this->layout=layout;
41        are_enabled=false;
42        is_running=false;
43        tested_motor=-1;
44
45    values=(float*)malloc(motors_count*sizeof(float));
46    power=(float*)malloc(motors_count*sizeof(float));
47    for(int i=0;i<motors_count;i++) power[i]=1;
48
49    //station sol
50        GroupBox* groupbox=new GroupBox(layout->NewRow(),"bldc");
51        flight_time=new Label(groupbox->NewRow(),"flight time");
52        min_value=new DoubleSpinBox(groupbox->NewRow(),"min value:",0,1,.1,2);
53        max_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"max value:",0,1,.1,2);
54        test_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"test value:",0,1,0.1);
55
56    int index=0;
57    button_test=(PushButton**)malloc(motors_count*sizeof(PushButton*));
58    for(int i=0;i<motors_count/2;i++) {
59        for(int j=0;j<2;j++) {
60            ostringstream test_name;
61            test_name << "test motor " << index;
62            button_test[index]=new PushButton(groupbox->At(2+i,j),test_name.str());
63            index++;
64        }
65    }
66
67        //flight time
68        FILE *file;
69    file=fopen("/etc/flight_time","r");
70    if (file == NULL) {
71        time_sec=0;
72    } else {
73        char ligne[32];
74        fgets(ligne, 32, file);
75        time_sec=atoi(ligne);
76        fclose(file);
77    }
78    flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
79}
80
81Bldc_impl::Bldc_impl(Bldc* self,uint8_t motors_count) {
82    this->self=self;
83    this->motors_count=motors_count;
84    values=NULL;
85    button_test=NULL;
86    power=NULL;
87}
88
89Bldc_impl::~Bldc_impl() {
90    if(values!=NULL) free(values);
91    if(button_test!=NULL) free(button_test);
92    if(power!=NULL) free(power);
93}
94
95void Bldc_impl::UseDefaultPlot(TabWidget* tab) {
96    Tab* plot_tab=new Tab(tab,"speeds");
97    plots=new DataPlot1D(plot_tab->NewRow(),"values",0,1);
98    for(int i=0;i<motors_count;i++) {
99        plots->AddCurve(self->output->Element(i));
100    }
101}
102
103void Bldc_impl::UpdateFrom(const io_data *data) {
104    cvmatrix* input=(cvmatrix*)data;
105    bool is_motor_running=false;
106
107    if(values==NULL) return;//nothing to do in simulator
108
109    if(input->Rows()!=motors_count) {
110        self->Err("nb motors mismatch\n");
111        return;
112    }
113
114    input->GetMutex();
115    for(int i=0;i<motors_count;i++) {
116        if(input->ValueNoMutex(i,0)!=0) is_motor_running=true;
117        if(are_enabled) {
118            //Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
119            values[i]=power[i]*Sat(input->ValueNoMutex(i,0));
120            //Printf("%i %f\n",i,values[i]);
121        } else {
122            values[i]=0;
123        }
124    }
125    input->ReleaseMutex();
126
127    if(are_enabled && is_motor_running && !is_running) {
128        flight_start_time = GetTime();
129        is_running=true;
130    }
131    if((!are_enabled || !is_motor_running) && is_running) {
132        Time now= GetTime();
133        int t_sec;
134        FILE *file;
135        char ligne[32];
136
137        t_sec=(now - flight_start_time)/1000000000;
138        time_sec+=t_sec;
139
140        Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
141        //Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
142        flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
143
144        file=fopen("/etc/flight_time","w");
145        if (file == NULL) {
146            Printf("Erreur a l'ouverture du fichier d'info vol\n");
147        } else {
148            sprintf(ligne,"%i",time_sec);
149            fputs(ligne,file);
150            fclose(file);
151        }
152        is_running=false;
153    }
154
155    for(int i=0;i<motors_count;i++) {
156        if(button_test[i]->Clicked()==true) {
157            if(!are_enabled) {
158                tested_motor=i;
159                test_start_time=GetTime();
160                LockUserInterface();
161            } else {
162                self->Warn("testing motor is not possible when enabled\n");
163            }
164        }
165    }
166    if(tested_motor!=-1) {
167        for(int i=0;i<motors_count;i++) {
168            values[i]=0;
169        }
170        values[tested_motor]=test_value->Value();
171
172        if(GetTime()>(test_start_time+2*1000000000)) {
173            tested_motor=-1;
174            UnlockUserInterface();
175        }
176    }
177
178    self->SetMotors(values);
179    self->output->SetDataTime(data->DataTime());
180    self->ProcessUpdate(self->output);
181}
182
183float Bldc_impl::Sat(float value) {
184    float result=value;
185
186    if(result<min_value->Value()) {
187        result=min_value->Value();
188    }
189    if(result>max_value->Value()) {
190        result=max_value->Value();
191    }
192
193    return result;
194}
195
196void Bldc_impl::LockUserInterface(void) const {
197    layout->setEnabled(false);
198}
199
200void Bldc_impl::UnlockUserInterface(void) const {
201    layout->setEnabled(true);
202}
Note: See TracBrowser for help on using the repository browser.