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