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

Last change on this file since 460 was 460, checked in by Sanahuja Guillaume, 6 months ago

modifs plane

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