source: flair-src/trunk/lib/FlairSensorActuator/src/TargetController.cpp @ 15

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

sources reformatted with flair-format-dir script

File size: 4.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:    2015/03/30
6//  filename:   TargetController.cpp
7//
8//  author:     Gildas Bayard
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Base class for target side remote controls
14//
15//
16/*********************************************************************/
17#include "TargetController.h"
18#include "Controller.h"
19#include <TabWidget.h>
20#include <Tab.h>
21#include <FrameworkManager.h>
22#include <cvmatrix.h>
23
24#include <cstring>
25#include <string>
26
27#include <unistd.h> //for usleep
28
29using namespace flair::core;
30using namespace flair::gui;
31using std::string;
32
33namespace flair {
34namespace sensor {
35
36TargetController::TargetController(const FrameworkManager *parent, string name,
37                                   uint8_t priority)
38    : Thread(parent, name, priority), IODevice(parent, name) {
39  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
40  TabWidget *tab = new TabWidget(main_tab->NewRow(), name);
41  setup_tab = new Tab(tab, "Reglages");
42}
43
44TargetController::~TargetController() {
45  SafeStop();
46  Join();
47}
48
49std::string TargetController::GetAxisName(unsigned int axisId) const {
50  return string("axis") + std::to_string(axisId);
51}
52
53std::string TargetController::GetButtonName(unsigned int buttonId) const {
54  return string("button") + std::to_string(buttonId);
55}
56
57bool TargetController::SetLedOn(unsigned int ledId) {
58  if (!IsControllerActionSupported(ControllerAction::SetLedOn))
59    return false;
60  SwitchLedMessage *msgLed = new SwitchLedMessage(true, ledId);
61  changeStateQueue.push(msgLed);
62  return true;
63}
64
65bool TargetController::SetLedOff(unsigned int ledId) {
66  if (!IsControllerActionSupported(ControllerAction::SetLedOn))
67    return false;
68  SwitchLedMessage *msgLed = new SwitchLedMessage(false, ledId);
69  changeStateQueue.push(msgLed);
70  return true;
71}
72
73bool TargetController::Rumble(unsigned int leftForce, unsigned int leftTimeout,
74                              unsigned int rightForce,
75                              unsigned int rightTimeout) {
76  if (!IsControllerActionSupported(ControllerAction::Rumble))
77    return false;
78  RumbleMessage *msgRumble =
79      new RumbleMessage(leftForce, leftTimeout, rightForce, rightTimeout);
80  changeStateQueue.push(msgRumble);
81  return true;
82}
83
84bool TargetController::FlashLed(unsigned int ledId, unsigned int onTimeout,
85                                unsigned int offTimeout) {
86  if (!IsControllerActionSupported(ControllerAction::FlashLed))
87    return false;
88  FlashLedMessage *msgFlashLed =
89      new FlashLedMessage(ledId, onTimeout, offTimeout);
90  changeStateQueue.push(msgFlashLed);
91  return true;
92}
93
94float TargetController::GetAxisValue(unsigned int axisId) const {
95  // TODO we'd better throw an exception here
96  if (axis == NULL)
97    return 0;
98
99  axis->GetMutex();
100  float axisValue = axis->Value(axisId, 0);
101  axis->ReleaseMutex();
102  return axisValue;
103}
104
105bool TargetController::IsButtonPressed(unsigned int buttonId) const {
106  // TODO we'd better throw an exception here
107  if (button == NULL)
108    return false;
109
110  button->GetMutex();
111  bool buttonValue = button->Value(buttonId, 0);
112  button->ReleaseMutex();
113  return buttonValue;
114}
115
116void TargetController::Run() {
117  Message *message;
118
119  if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
120    SafeStop();
121    Thread::Err("An error occured, we don't proceed with the loop.\n");
122  } else {
123
124    axis = new cvmatrix((IODevice *)this, axisNumber, 1, floatType);
125    button =
126        new cvmatrix((IODevice *)this, buttonNumber, 1, SignedIntegerType(8));
127
128    while (!ToBeStopped()) {
129      // Thread::Info("Debug: entering acquisition loop\n");
130      if (getFrameworkManager()->ConnectionLost() == true)
131        SafeStop();
132
133      if (IsDataFrameReady()) {
134        // Thread::Info("Debug: data frame is ready\n");
135
136        AcquireAxisData(*axis);
137        AcquireButtonData(*button);
138
139        // send the data
140        axis->SetDataTime(GetTime());
141        ProcessUpdate(axis);
142
143        // send pending controller state change request(s)
144
145        while (changeStateQueue.size() != 0) {
146          message = changeStateQueue.front();
147          if (ProcessMessage(message)) {
148            changeStateQueue.pop();
149            delete message;
150          }
151        }
152      } else {
153        // Thread::Info("Debug: relax...\n");
154        usleep(20000); // 20ms
155      }
156    }
157  }
158}
159
160Tab *TargetController::GetTab() const { return setup_tab; }
161
162} // end namespace sensor
163} // end namespace flair
Note: See TracBrowser for help on using the repository browser.