rpiservo.cpp

 
#include "rpiservo.h"
#include "plugin.h"
 
#include <string>
#include <algorithm>
#include <iostream>
 
#include <boost/thread.hpp>
 
#ifdef RPI
#include <wiringPi.h>
#include <time.h>
#include <sys/time.h>
#endif
 
long mapData(float x, long in_min, long in_max, long out_min, long out_max)
{
  return (long)((x - (float)in_min) * ((float)out_max - (float)out_min) / ((float)in_max - (float)in_min) + (float)out_min);
}
 
//
//Class RpiServo
//
 
RpiServo::RpiServo()
{
}
 
RpiServo::RpiServo(int pin, float value):
  mPin(pin),
  mValue(1500), // midpoint
  mState(false)
{
  WriteValue(value);
 
#ifdef RPI
  pinMode(mPin, OUTPUT);
  digitalWrite(mPin, LOW);
 
  mState = true;
 
  boost::thread::attributes attrs;
  struct sched_param param;
  param.sched_priority = 50;
  pthread_attr_setschedparam(attrs.native_handle(), &param);
 
  mThread = boost::thread(attrs, boost::bind(&RpiServo::threadLoop, this));
#endif
 
}
 
RpiServo::~RpiServo()
{
#ifdef RPI
  if (mState)
  {
    mState = false;
    mThread.join();
  }
  digitalWrite(mPin, LOW);
#endif
}
 
void RpiServo::threadLoop()
{
#ifdef RPI
  while (mState)
  {
    //wave cycle duration is 20 millisecond, rotation value between 1 and 2 millisecond
    digitalWrite(mPin, HIGH);
    delayMicroseconds(mValue);
    digitalWrite(mPin, LOW);
 
    // Wait until the end of an 8mS time-slot
    boost::this_thread::sleep_for(boost::chrono::microseconds(20000 - mValue));
  }
#endif
}
 
void RpiServo::WriteValue(float value)
{
  if (value < 0.0f)
    value = 0.0f;
  else if (value > 180.0f)
    value = 180.0f;
 
  mValue = mapData(value, 0, 180, -250, 1250) + 1000;
}