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

Last change on this file since 15 was 15, checked in by Bayard Gildas, 7 years ago

sources reformatted with flair-format-dir script

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