source: flair-src/trunk/lib/FlairSimulator/src/SimuServos.cpp@ 341

Last change on this file since 341 was 341, checked in by Sanahuja Guillaume, 2 years ago

servos

File size: 1.9 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: 2019/11/27
6// filename: SimuServos.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Class for a simulation servos
14//
15//
16/*********************************************************************/
17#include "SimuServos.h"
18#include <SharedMem.h>
19#include <sstream>
20#include <string.h>
21
22using std::string;
23using std::ostringstream;
24using namespace flair::core;
25
26namespace flair {
27namespace actuator {
28
29
30SimuServos::SimuServos(const Object *parent, string name, uint8_t servos_count,
31 uint32_t modelId,uint32_t deviceId)
32 : IODevice(parent, name) {
33 shmem =
34 new SharedMem(this, ShMemName(modelId, deviceId), servos_count * sizeof(float)+sizeof(Time));
35
36 buf=(char*)malloc(servos_count * sizeof(float)+sizeof(Time));
37 if(buf==NULL) {
38 Err("error creating buffer\n");
39 return;
40 }
41
42 // reset values
43 float *values=(float*)buf;
44 for (int i = 0; i < servos_count; i++) values[i] = 0;
45 Time time=GetTime();
46 memcpy(buf+servos_count * sizeof(float),&time,sizeof(Time));
47
48 shmem->Write(buf, servos_count * sizeof(float)+sizeof(Time));
49
50 SetIsReady(true);
51 this->servos_count=servos_count;
52}
53
54SimuServos::~SimuServos() {
55 if(buf!=NULL) free(buf);
56}
57
58string SimuServos::ShMemName(uint32_t modelId,uint32_t deviceId) {
59 ostringstream dev_name;
60 dev_name << "simu" << modelId << "_servos_" << deviceId;
61 return dev_name.str().c_str();
62}
63
64
65void SimuServos::GetPositions(float *value,Time* time) const {
66 float *values=(float*)buf;
67 shmem->Read(buf, servos_count * sizeof(float)+sizeof(Time));
68 memcpy(time,buf+servos_count * sizeof(float),sizeof(Time));
69
70 for (int i = 0; i < servos_count; i++) {
71 value[i] = values[i];
72 }
73}
74
75} // end namespace actuator
76} // end namespace flair
Note: See TracBrowser for help on using the repository browser.