[10] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[10] | 4 | // %flair:license}
|
---|
[7] | 5 | // created: 2011/05/01
|
---|
| 6 | // filename: TrajectoryGenerator1D_impl.cpp
|
---|
| 7 | //
|
---|
| 8 | // author: Guillaume Sanahuja
|
---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 10 | //
|
---|
| 11 | // version: $Id: $
|
---|
| 12 | //
|
---|
| 13 | // purpose: Class generating a trajectory in 1D
|
---|
| 14 | //
|
---|
| 15 | //
|
---|
| 16 | /*********************************************************************/
|
---|
| 17 |
|
---|
| 18 | #include "TrajectoryGenerator1D.h"
|
---|
| 19 | #include "TrajectoryGenerator1D_impl.h"
|
---|
| 20 | #include <cvmatrix.h>
|
---|
| 21 | #include <Layout.h>
|
---|
| 22 | #include <GroupBox.h>
|
---|
| 23 | #include <DoubleSpinBox.h>
|
---|
| 24 | #include <cmath>
|
---|
| 25 |
|
---|
| 26 | using std::string;
|
---|
| 27 | using namespace flair::core;
|
---|
| 28 | using namespace flair::gui;
|
---|
| 29 | using namespace flair::filter;
|
---|
| 30 |
|
---|
[15] | 31 | TrajectoryGenerator1D_impl::TrajectoryGenerator1D_impl(
|
---|
| 32 | TrajectoryGenerator1D *self, const LayoutPosition *position, string name,
|
---|
| 33 | string unit) {
|
---|
| 34 | first_update = true;
|
---|
| 35 | is_started = false;
|
---|
[7] | 36 |
|
---|
[15] | 37 | // init UI
|
---|
| 38 | GroupBox *reglages_groupbox = new GroupBox(position, name);
|
---|
| 39 | T = new DoubleSpinBox(reglages_groupbox->NewRow(), "period, 0 for auto:",
|
---|
| 40 | " s", 0, 1, 0.01);
|
---|
| 41 | if (unit == "") {
|
---|
| 42 | max_veloctity =
|
---|
| 43 | new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
|
---|
| 44 | "velocity max (absolute):", 0., 200000, 1);
|
---|
| 45 | } else {
|
---|
| 46 | max_veloctity = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
|
---|
| 47 | "velocity max (absolute):",
|
---|
| 48 | " " + unit + "/s", 0., 200000, 1);
|
---|
| 49 | }
|
---|
| 50 | if (unit == "") {
|
---|
| 51 | acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
|
---|
| 52 | "acceleration (absolute):", 0., 10, 1, 3);
|
---|
| 53 | } else {
|
---|
| 54 | acceleration = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
|
---|
| 55 | "acceleration (absolute):",
|
---|
| 56 | " " + unit + "/s²", 0., 200000, 1);
|
---|
| 57 | }
|
---|
[7] | 58 |
|
---|
[15] | 59 | Reset();
|
---|
[7] | 60 |
|
---|
[15] | 61 | // init matrix
|
---|
| 62 | cvmatrix_descriptor *desc = new cvmatrix_descriptor(2, 1);
|
---|
| 63 | desc->SetElementName(0, 0, "pos");
|
---|
| 64 | desc->SetElementName(1, 0, "vel");
|
---|
| 65 | output = new cvmatrix(self, desc, floatType, name);
|
---|
[7] | 66 |
|
---|
[15] | 67 | output->SetValue(0, 0, pos);
|
---|
| 68 | output->SetValue(1, 0, v);
|
---|
[7] | 69 | }
|
---|
| 70 |
|
---|
[15] | 71 | TrajectoryGenerator1D_impl::~TrajectoryGenerator1D_impl() {}
|
---|
[7] | 72 |
|
---|
| 73 | void TrajectoryGenerator1D_impl::Reset(void) {
|
---|
[15] | 74 | pos = 0;
|
---|
| 75 | v = 0;
|
---|
| 76 | pos_off = 0;
|
---|
| 77 | vel_off = 0;
|
---|
[7] | 78 | }
|
---|
| 79 |
|
---|
[15] | 80 | void TrajectoryGenerator1D_impl::StartTraj(float start_pos, float end_pos) {
|
---|
| 81 | is_started = true;
|
---|
| 82 | is_finished = false;
|
---|
| 83 | first_update = true;
|
---|
[7] | 84 |
|
---|
[15] | 85 | // configure trajectory
|
---|
| 86 | end_position = end_pos;
|
---|
| 87 | pos = start_pos;
|
---|
| 88 | acc = acceleration->Value();
|
---|
| 89 | v = 0;
|
---|
| 90 | if (end_position < start_pos) {
|
---|
| 91 | acc = -acc;
|
---|
| 92 | // max_veloctity=-max_veloctity;
|
---|
| 93 | }
|
---|
[7] | 94 | }
|
---|
| 95 |
|
---|
[15] | 96 | // revoir l'interet du stop?
|
---|
[7] | 97 | void TrajectoryGenerator1D_impl::StopTraj(void) {
|
---|
[15] | 98 | is_started = false;
|
---|
| 99 | v = 0;
|
---|
| 100 | // output->SetValue(1,0,v);
|
---|
[7] | 101 | }
|
---|
| 102 |
|
---|
| 103 | void TrajectoryGenerator1D_impl::Update(Time time) {
|
---|
[15] | 104 | float delta_t;
|
---|
[7] | 105 |
|
---|
[15] | 106 | if (T->Value() == 0) {
|
---|
| 107 | if (first_update == true) {
|
---|
| 108 | first_update = false;
|
---|
| 109 | previous_time = time;
|
---|
| 110 | output->GetMutex();
|
---|
| 111 | output->SetValueNoMutex(0, 0, pos + pos_off);
|
---|
| 112 | output->SetValueNoMutex(1, 0, v + vel_off);
|
---|
| 113 | output->ReleaseMutex();
|
---|
[7] | 114 |
|
---|
[15] | 115 | output->SetDataTime(time);
|
---|
| 116 | return;
|
---|
[7] | 117 | } else {
|
---|
[15] | 118 | delta_t = (float)(time - previous_time) / 1000000000.;
|
---|
[7] | 119 | }
|
---|
[15] | 120 | } else {
|
---|
| 121 | delta_t = T->Value();
|
---|
| 122 | }
|
---|
| 123 | previous_time = time;
|
---|
[7] | 124 |
|
---|
[15] | 125 | if (is_started == true) {
|
---|
| 126 | if (is_finished == false) {
|
---|
| 127 | v += acc * delta_t;
|
---|
| 128 | if (fabs(v) > fabs(max_veloctity->Value())) {
|
---|
| 129 | if (v > 0)
|
---|
| 130 | v = max_veloctity->Value();
|
---|
| 131 | else
|
---|
| 132 | v = -max_veloctity->Value();
|
---|
| 133 | }
|
---|
| 134 | pos += v * delta_t;
|
---|
| 135 | if (end_position - v * v / (2 * acc) <= pos && v >= 0)
|
---|
| 136 | acc = -acc;
|
---|
| 137 | if (end_position - v * v / (2 * acc) >= pos && v < 0)
|
---|
| 138 | acc = -acc;
|
---|
| 139 | if (pos >= end_position && v >= 0)
|
---|
| 140 | is_finished = true;
|
---|
| 141 | if (pos <= end_position && v < 0)
|
---|
| 142 | is_finished = true;
|
---|
[7] | 143 | }
|
---|
[15] | 144 | // else
|
---|
| 145 | if (is_finished == true) {
|
---|
| 146 | v = 0;
|
---|
| 147 | pos = end_position;
|
---|
| 148 | }
|
---|
| 149 | }
|
---|
[7] | 150 |
|
---|
[15] | 151 | // on prend une fois pour toute les mutex et on fait des accès directs
|
---|
| 152 | output->GetMutex();
|
---|
| 153 | output->SetValueNoMutex(0, 0, pos + pos_off);
|
---|
| 154 | output->SetValueNoMutex(1, 0, v + vel_off);
|
---|
| 155 | output->ReleaseMutex();
|
---|
[7] | 156 |
|
---|
[15] | 157 | output->SetDataTime(time);
|
---|
[7] | 158 | }
|
---|