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

Last change on this file since 137 was 137, checked in by Sanahuja Guillaume, 5 years ago

singleton manager

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