[38] | 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 |
|
---|
| 25 | using namespace flair::core;
|
---|
| 26 | using namespace flair::gui;
|
---|
| 27 | using std::string;
|
---|
| 28 |
|
---|
| 29 | namespace flair { namespace sensor {
|
---|
| 30 |
|
---|
| 31 | EmulatedController::EmulatedController(const FrameworkManager* parent,string name,uint8_t priority) :
|
---|
| 32 | TargetController(parent,name,priority) {
|
---|
| 33 | }
|
---|
| 34 |
|
---|
| 35 | EmulatedController::~EmulatedController() {
|
---|
| 36 | //TargetController calls TargetEthController methods in its run
|
---|
| 37 | //we must stop the thread now
|
---|
| 38 | SafeStop();
|
---|
| 39 | Join();
|
---|
| 40 | }
|
---|
| 41 |
|
---|
| 42 | bool EmulatedController::IsConnected() const {
|
---|
| 43 | return true;
|
---|
| 44 | }
|
---|
| 45 |
|
---|
| 46 | bool EmulatedController::IsDataFrameReady(){
|
---|
| 47 | return true;
|
---|
| 48 | }
|
---|
| 49 |
|
---|
| 50 | template<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 |
|
---|
| 56 | void 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 |
|
---|
| 62 | void 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 |
|
---|
| 72 | void 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 |
|
---|
| 102 | void 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 |
|
---|
| 134 | void EmulatedController::AcquireAxisData(core::cvmatrix &axis) {
|
---|
| 135 | ComputeControllerData(DataType::axis,axis);
|
---|
| 136 | }
|
---|
| 137 |
|
---|
| 138 | void EmulatedController::AcquireButtonData(core::cvmatrix &button) {
|
---|
| 139 | ComputeControllerData(DataType::button,button);
|
---|
| 140 | }
|
---|
| 141 |
|
---|
| 142 | bool EmulatedController::ProcessMessage(Message *msg) {
|
---|
| 143 | return false;
|
---|
| 144 | }
|
---|
| 145 |
|
---|
| 146 | bool EmulatedController::ControllerInitialization() {
|
---|
| 147 | axisNumber=4;
|
---|
| 148 | buttonNumber=16;
|
---|
| 149 | return true;
|
---|
| 150 | }
|
---|
| 151 |
|
---|
| 152 | } // end namespace sensor
|
---|
| 153 | } // end namespace flair
|
---|