source: flair-src/trunk/lib/FlairSensorActuator/src/Servos_impl.cpp@ 340

Last change on this file since 340 was 340, checked in by Sanahuja Guillaume, 2 years ago

add servos

File size: 3.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: 2019/11/28
6// filename: Servos_impl.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Virtual class for servos
14//
15//
16/*********************************************************************/
17#include "Servos_impl.h"
18#include "Servos.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
36Servos_impl::Servos_impl(Servos *self, Layout *layout, string name,
37 uint8_t servos_count) {
38 this->self = self;
39 this->servos_count = servos_count;
40 this->layout = layout;
41 are_enabled = false;
42 tested_servo = -1;
43
44 values = (float *)malloc(servos_count * sizeof(float));
45
46 // station sol
47 GroupBox *groupbox = new GroupBox(layout->NewRow(), "Servos");
48 min_value = new DoubleSpinBox(groupbox->NewRow(), "min value:", -3.15, 3.15, .1, 2);
49 max_value =
50 new DoubleSpinBox(groupbox->LastRowLastCol(), "max value:", -3.15, 3.15, .1, 2);
51 test_value =
52 new DoubleSpinBox(groupbox->LastRowLastCol(), "test value:", -3.15, 3.15, 0.1);
53
54 int index = 0;
55 button_test = (PushButton **)malloc(servos_count * sizeof(PushButton *));
56 for (int i = 0; i < servos_count / 2; i++) {
57 for (int j = 0; j < 2; j++) {
58 ostringstream test_name;
59 test_name << "test servo " << index;
60 button_test[index] =
61 new PushButton(groupbox->At(2 + i, j), test_name.str());
62 index++;
63 }
64 }
65}
66
67Servos_impl::~Servos_impl() {
68 if (values != NULL)
69 free(values);
70 if (button_test != NULL)
71 free(button_test);
72}
73
74void Servos_impl::UseDefaultPlot(TabWidget *tab) {
75
76}
77
78void Servos_impl::UpdateFrom(const io_data *data) {
79 Matrix *input = (Matrix *)data;
80 bool is_motor_running = false;
81
82 if (values == NULL)
83 return; // nothing to do in simulator
84
85 if (input->Rows() != servos_count) {
86 self->Err("nb servos mismatch\n");
87 return;
88 }
89
90 input->GetMutex();
91 for (int i = 0; i < servos_count; i++) {
92 if (are_enabled) {
93 // Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
94 values[i] =Sat(input->ValueNoMutex(i, 0));
95 // Printf("%i %f\n",i,values[i]);
96 } else {
97 values[i] = 0;
98 }
99 if(values[i] != 0) is_motor_running = true;
100 }
101 input->ReleaseMutex();
102
103
104 for (int i = 0; i < servos_count; i++) {
105 if (button_test[i]->Clicked() == true) {
106 if (!are_enabled) {
107 tested_servo = i;
108 test_start_time = GetTime();
109 LockUserInterface();
110 } else {
111 self->Warn("testing servo is not possible when enabled\n");
112 }
113 }
114 }
115 if (tested_servo != -1) {
116 for (int i = 0; i < servos_count; i++) {
117 values[i] = 0;
118 }
119 values[tested_servo] = test_value->Value();
120
121 if (GetTime() > (test_start_time + 2 * 1000000000)) {
122 tested_servo = -1;
123 UnlockUserInterface();
124 }
125 }
126
127 self->SetServos(values);
128}
129
130float Servos_impl::Sat(float value) {
131 float result = value;
132
133 if (result < min_value->Value()) {
134 result = min_value->Value();
135 }
136 if (result > max_value->Value()) {
137 result = max_value->Value();
138 }
139
140 return result;
141}
142
143void Servos_impl::LockUserInterface(void) const { layout->setEnabled(false); }
144
145void Servos_impl::UnlockUserInterface(void) const { layout->setEnabled(true); }
Note: See TracBrowser for help on using the repository browser.