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

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

sources reformatted with flair-format-dir script

File size: 5.5 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]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
[15]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;
[3]45
[15]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;
[3]50
[15]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);
[3]59
[15]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++;
[3]69    }
[15]70  }
[3]71
[15]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);
[3]85}
86
[15]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;
[3]93}
94
95Bldc_impl::~Bldc_impl() {
[15]96  if (values != NULL)
97    free(values);
98  if (button_test != NULL)
99    free(button_test);
100  if (power != NULL)
101    free(power);
[3]102}
103
[15]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  }
[3]110}
111
112void Bldc_impl::UpdateFrom(const io_data *data) {
[15]113  cvmatrix *input = (cvmatrix *)data;
114  bool is_motor_running = false;
[3]115
[15]116  if (values == NULL)
117    return; // nothing to do in simulator
[3]118
[15]119  if (input->Rows() != motors_count) {
120    self->Err("nb motors mismatch\n");
121    return;
122  }
[3]123
[15]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;
[3]134    }
[15]135  }
136  input->ReleaseMutex();
[3]137
[15]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];
[3]147
[15]148    t_sec = (now - flight_start_time) / 1000000000;
149    time_sec += t_sec;
[3]150
[15]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);
[3]156
[15]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);
[3]164    }
[15]165    is_running = false;
166  }
[3]167
[15]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      }
[3]177    }
[15]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();
[3]184
[15]185    if (GetTime() > (test_start_time + 2 * 1000000000)) {
186      tested_motor = -1;
187      UnlockUserInterface();
[3]188    }
[15]189  }
[3]190
[15]191  self->SetMotors(values);
192  self->output->SetDataTime(data->DataTime());
193  self->ProcessUpdate(self->output);
[3]194}
195
196float Bldc_impl::Sat(float value) {
[15]197  float result = value;
[3]198
[15]199  if (result < min_value->Value()) {
200    result = min_value->Value();
201  }
202  if (result > max_value->Value()) {
203    result = max_value->Value();
204  }
[3]205
[15]206  return result;
[3]207}
208
[15]209void Bldc_impl::LockUserInterface(void) const { layout->setEnabled(false); }
[3]210
[15]211void Bldc_impl::UnlockUserInterface(void) const { layout->setEnabled(true); }
Note: See TracBrowser for help on using the repository browser.