source: flair-src/trunk/lib/FlairSensorActuator/src/EmulatedController.cpp@ 38

Last change on this file since 38 was 38, checked in by Bayard Gildas, 8 years ago

Modif. pour ajour manette émulée (EmulatedController)

File size: 5.9 KB
Line 
1// created: 2015/03/30
2// filename: EmulatedController.cpp
3//
4// author: Gildas Bayard
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: class that emulate a remote control.
10// Typical use case: run a demo without any reliable communication channel
11//
12/*********************************************************************/
13#include "EmulatedController.h"
14#include <Controller.h>
15#include <FrameworkManager.h>
16#include <TcpSocket.h>
17#include <Socket.h>
18#include <cstring>
19#include <string>
20#include <cvmatrix.h>
21#include <stdexcept>
22
23#include <unistd.h> // for sleep (debug)
24
25using namespace flair::core;
26using namespace flair::gui;
27using std::string;
28
29namespace flair { namespace sensor {
30
31EmulatedController::EmulatedController(const FrameworkManager* parent,string name,uint8_t priority) :
32 TargetController(parent,name,priority) {
33}
34
35EmulatedController::~EmulatedController() {
36 //TargetController calls TargetEthController methods in its run
37 //we must stop the thread now
38 SafeStop();
39 Join();
40}
41
42bool EmulatedController::IsConnected() const {
43 return true;
44}
45
46bool EmulatedController::IsDataFrameReady(){
47 return true;
48}
49
50template<typename T> void EmulatedController::fillVectorNoMutex(cvmatrix &vector,T data[],unsigned int size) {
51 for (unsigned int i=0; i<size; i++) {
52 vector.SetValueNoMutex(i,0,data[i]);
53 }
54}
55
56void EmulatedController::fillVectorNoMutex(cvmatrix &destination,cvmatrix &source,unsigned int size) {
57 for (unsigned int i=0; i<size; i++) {
58 destination.SetValueNoMutex(i,0,source.Value(i,0));
59 }
60}
61
62void EmulatedController::StepData::Print() {
63 Printf("<StepData %s, duration=%d, axisData={%f,%f,%f,%f}, buttonData={%c,%c,%c,%c,%c,%c,%c,%c,%c,%c,%c,%c,%c,%c,%c,%c} />\n",description.c_str(),durationMs,
64 axisData->Value(0,0),axisData->Value(1,0),axisData->Value(2,0),axisData->Value(3,0),
65 buttonData->Value(0,0)?'X':'-',buttonData->Value(1,0)?'X':'-',buttonData->Value(2,0)?'X':'-',buttonData->Value(3,0)?'X':'-',
66 buttonData->Value(4,0)?'X':'-',buttonData->Value(5,0)?'X':'-',buttonData->Value(6,0)?'X':'-',buttonData->Value(7,0)?'X':'-',
67 buttonData->Value(8,0)?'X':'-',buttonData->Value(9,0)?'X':'-',buttonData->Value(10,0)?'X':'-',buttonData->Value(11,0)?'X':'-',
68 buttonData->Value(12,0)?'X':'-',buttonData->Value(13,0)?'X':'-',buttonData->Value(14,0)?'X':'-',buttonData->Value(15,0)?'X':'-'
69 );
70}
71
72void EmulatedController::AddStep(unsigned int durationMs,string description,uint16_t buttonPressed, float leftAxisX, float leftAxisY, float rightAxisX, float rightAxisY) {
73 cvmatrix *axisMatrix=new cvmatrix((IODevice *)this,4,1,floatType);
74 axisMatrix->SetValueNoMutex(0,0,leftAxisX);
75 axisMatrix->SetValueNoMutex(1,0,leftAxisY);
76 axisMatrix->SetValueNoMutex(2,0,rightAxisX);
77 axisMatrix->SetValueNoMutex(3,0,rightAxisY);
78
79 cvmatrix *buttonMatrix=new cvmatrix((IODevice *)this,16,1,SignedIntegerType(8));
80 if (buttonPressed&(uint16_t)ButtonType::start) buttonMatrix->SetValueNoMutex(0,0,1);
81 if (buttonPressed&(uint16_t)ButtonType::select) buttonMatrix->SetValueNoMutex(1,0,1);
82 if (buttonPressed&(uint16_t)ButtonType::square) buttonMatrix->SetValueNoMutex(2,0,1);
83 if (buttonPressed&(uint16_t)ButtonType::triangle) buttonMatrix->SetValueNoMutex(3,0,1);
84 if (buttonPressed&(uint16_t)ButtonType::circle) buttonMatrix->SetValueNoMutex(4,0,1);
85 if (buttonPressed&(uint16_t)ButtonType::cross) buttonMatrix->SetValueNoMutex(5,0,1);
86 if (buttonPressed&(uint16_t)ButtonType::left1) buttonMatrix->SetValueNoMutex(6,0,1);
87 if (buttonPressed&(uint16_t)ButtonType::left2) buttonMatrix->SetValueNoMutex(7,0,1);
88 if (buttonPressed&(uint16_t)ButtonType::left3) buttonMatrix->SetValueNoMutex(8,0,1);
89 if (buttonPressed&(uint16_t)ButtonType::right1) buttonMatrix->SetValueNoMutex(9,0,1);
90 if (buttonPressed&(uint16_t)ButtonType::right2) buttonMatrix->SetValueNoMutex(10,0,1);
91 if (buttonPressed&(uint16_t)ButtonType::right3) buttonMatrix->SetValueNoMutex(11,0,1);
92 if (buttonPressed&(uint16_t)ButtonType::up) buttonMatrix->SetValueNoMutex(12,0,1);
93 if (buttonPressed&(uint16_t)ButtonType::down) buttonMatrix->SetValueNoMutex(13,0,1);
94 if (buttonPressed&(uint16_t)ButtonType::left) buttonMatrix->SetValueNoMutex(14,0,1);
95 if (buttonPressed&(uint16_t)ButtonType::right) buttonMatrix->SetValueNoMutex(15,0,1);
96
97 StepData step={durationMs,axisMatrix,buttonMatrix,description};
98 //step.Print();
99 steps.push_back(step);
100}
101
102void EmulatedController::ComputeControllerData(DataType dataType, cvmatrix &data) {
103 static Time startStepTime=GetTime();
104
105 Time now=GetTime();
106
107 axis->GetMutex();
108 button->GetMutex();
109
110 if (steps.empty()) {
111 if (dataType==DataType::axis) {
112 float values[]={0.,0.,0.,0.};
113 fillVectorNoMutex(data,values,4);
114 }
115 if (dataType==DataType::button) {
116 int values[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
117 fillVectorNoMutex(data,values,16);
118 }
119 } else {
120 StepData currentStep=steps.front();
121 if (dataType==DataType::axis) fillVectorNoMutex(data,*(currentStep.axisData),4);
122 if (dataType==DataType::button) fillVectorNoMutex(data,*(currentStep.buttonData),16);
123 if (now-startStepTime>(Time)currentStep.durationMs*1000*1000) {
124 Thread::Info("Step '%s' done\n",currentStep.description.c_str());
125 startStepTime=now;
126 steps.pop_front();
127 }
128 }
129
130 button->ReleaseMutex();
131 axis->ReleaseMutex();
132}
133
134void EmulatedController::AcquireAxisData(core::cvmatrix &axis) {
135 ComputeControllerData(DataType::axis,axis);
136}
137
138void EmulatedController::AcquireButtonData(core::cvmatrix &button) {
139 ComputeControllerData(DataType::button,button);
140}
141
142bool EmulatedController::ProcessMessage(Message *msg) {
143 return false;
144}
145
146bool EmulatedController::ControllerInitialization() {
147 axisNumber=4;
148 buttonNumber=16;
149 return true;
150}
151
152} // end namespace sensor
153} // end namespace flair
Note: See TracBrowser for help on using the repository browser.