source: flair-src/trunk/lib/FlairSensorActuator/src/SimulatedServos.cpp

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

add servos

File size: 1.8 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/28
6// filename: SimulatedServos.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#ifdef CORE2_64
18
19#include "SimulatedServos.h"
20#include <FrameworkManager.h>
21#include <SharedMem.h>
22#include <Matrix.h>
23#include <sstream>
24#include <string.h>
25
26using std::string;
27using std::ostringstream;
28using namespace flair::core;
29using namespace flair::gui;
30
31namespace flair {
32namespace actuator {
33
34SimulatedServos::SimulatedServos(const IODevice *parent, Layout *layout, string name,
35 uint8_t servos_count, uint32_t modelId,uint32_t deviceId)
36 : Servos(parent, layout, name, servos_count) {
37 shmem =
38 new SharedMem(this, ShMemName(modelId, deviceId), servos_count * sizeof(float)+sizeof(Time));
39
40
41 buf=(char*)malloc(servos_count * sizeof(float)+sizeof(Time));
42 if(buf==NULL) {
43 Err("error creating buffer\n");
44 return;
45 }
46 SetIsReady(true);
47}
48
49SimulatedServos::~SimulatedServos() {
50 if(buf!=NULL) free(buf);
51}
52
53string SimulatedServos::ShMemName(uint32_t modelId,uint32_t deviceId) {
54 ostringstream dev_name;
55 dev_name << "simu" << modelId << "_servos_" << deviceId;
56 return dev_name.str().c_str();
57}
58
59void SimulatedServos::SetServos(float *position) {
60 float *values=(float*)buf;
61 for (int i = 0; i < ServosCount(); i++) values[i] = position[i];
62 Time time=GetTime();
63 memcpy(buf+ServosCount() * sizeof(float),&time,sizeof(Time));
64
65 shmem->Write(buf, ServosCount() * sizeof(float)+sizeof(Time));
66
67}
68
69} // end namespace sensor
70} // end namespace flair
71
72#endif
Note: See TracBrowser for help on using the repository browser.