[340] | 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 |
|
---|
| 30 | using std::string;
|
---|
| 31 | using std::ostringstream;
|
---|
| 32 | using namespace flair::core;
|
---|
| 33 | using namespace flair::gui;
|
---|
| 34 | using namespace flair::actuator;
|
---|
| 35 |
|
---|
| 36 | Servos_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 |
|
---|
| 67 | Servos_impl::~Servos_impl() {
|
---|
| 68 | if (values != NULL)
|
---|
| 69 | free(values);
|
---|
| 70 | if (button_test != NULL)
|
---|
| 71 | free(button_test);
|
---|
| 72 | }
|
---|
| 73 |
|
---|
| 74 | void Servos_impl::UseDefaultPlot(TabWidget *tab) {
|
---|
| 75 |
|
---|
| 76 | }
|
---|
| 77 |
|
---|
| 78 | void 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 |
|
---|
| 130 | float 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 |
|
---|
| 143 | void Servos_impl::LockUserInterface(void) const { layout->setEnabled(false); }
|
---|
| 144 |
|
---|
| 145 | void Servos_impl::UnlockUserInterface(void) const { layout->setEnabled(true); }
|
---|