Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
tree: 9f5cc665f2
Fetching contributors…

Cannot retrieve contributors at this time

file 75 lines (68 sloc) 2.765 kb
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75
/**************************************************************************
* Copyright 2012 KISS Institute for Practical Robotics *
* *
* This file is part of libkovan. *
* *
* libkovan is free software: you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation, either version 2 of the License, or *
* (at your option) any later version. *
* *
* libkovan is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with libkovan. Check the LICENSE file in the project root. *
* If not, see <http://www.gnu.org/licenses/>. *
**************************************************************************/

#include "servo_p.hpp"
#include "shm_p.hpp"
#include "private.hpp"
#include <cstdio>

namespace Private
{
class ServoPublishListener : public PublishListener
{
public:
virtual void published(Private::SharedMemoryClient *client)
{
static unsigned char i = 0;
if(i++ == 0) printf("Cleared servo dirty bit");
client->servoDirty = 0;
}
};
}

Private::Servo::Servo()
: m_listener(new ServoPublishListener())
{
SharedMemoryImpl::instance()->addPublishListener(m_listener);
}

Private::Servo::~Servo()
{
delete m_listener;
}

bool Private::Servo::setPosition(const port_t& port, const unsigned short& position)
{
Private::SharedMemoryClient *shm = SharedMemoryImpl::instance()->sharedMemoryClient();
if(!shm) return false;
if(port < 1 || port > 4) return false;
shm->servoPositions[port - 1] = position & 0x3FF;
shm->servoDirty |= 1 << (4 - port);
SharedMemoryImpl::instance()->doAutoPublish();
return true;
}

unsigned short Private::Servo::position(const port_t& port) const
{
Private::SharedMemoryServer *shm = SharedMemoryImpl::instance()->sharedMemoryServer();
if(!shm) return false;
if(port < 1 || port > 4) return false;
unsigned short pos = shm->servoPositions[port - 1] & 0x3FF;
return pos;
}

Private::Servo *Private::Servo::instance()
{
static Servo instance;
return &instance;
}
Something went wrong with that request. Please try again.