150 lines
3.0 KiB
C++
150 lines
3.0 KiB
C++
#include "src/drivers/servo.h"
|
|
|
|
#include "mbed-os/rtos/Thread.h"
|
|
|
|
namespace drivers {
|
|
|
|
namespace {
|
|
|
|
Thread* pServoThread;
|
|
|
|
double posToPulseWidthInUs(double position)
|
|
{
|
|
return (position/2+1.5)*1000;
|
|
}
|
|
|
|
typedef struct {
|
|
double from;
|
|
double to;
|
|
double intervalInSec;
|
|
PwmOut* pServo;
|
|
} SweepData;
|
|
|
|
typedef struct {
|
|
double latestPosition;
|
|
PwmOut* pServo;
|
|
} NodData;
|
|
|
|
void sweepWorker(SweepData* pSweepData)
|
|
{
|
|
SweepData sweepData = *pSweepData;
|
|
|
|
double resolution(24);
|
|
double range(sweepData.to - sweepData.from);
|
|
double value(sweepData.to);
|
|
double increment(range/resolution);
|
|
|
|
while (true)
|
|
{
|
|
value += increment;
|
|
|
|
if (value >= sweepData.to)
|
|
{
|
|
increment = -abs(increment);
|
|
}
|
|
else if (value <= sweepData.from)
|
|
{
|
|
increment = abs(increment);
|
|
}
|
|
sweepData.pServo->pulsewidth_us(posToPulseWidthInUs(value));
|
|
|
|
// E.g resolution 10 will give a 100ms delay between steps
|
|
Thread::wait(1000/resolution);
|
|
}
|
|
}
|
|
|
|
void nodWorker(NodData* pNodData)
|
|
{
|
|
pNodData->pServo->pulsewidth_us(posToPulseWidthInUs(pNodData->latestPosition-0.4));
|
|
Thread::wait(500);
|
|
pNodData->pServo->pulsewidth_us(posToPulseWidthInUs(pNodData->latestPosition));
|
|
}
|
|
}
|
|
|
|
Servo::Servo(PinName servoArm)
|
|
: m_servo(servoArm)
|
|
, m_dir(1)
|
|
, m_inMovement(false)
|
|
, m_latestPosition(0.0)
|
|
{
|
|
center();
|
|
}
|
|
|
|
void Servo::center()
|
|
{
|
|
setPosition(0.0);
|
|
}
|
|
|
|
void Servo::resetTimers()
|
|
{
|
|
m_servo.period_ms(20);
|
|
}
|
|
|
|
void Servo::setPosition(double position)
|
|
{
|
|
stop();
|
|
|
|
double increment = position*500.0;
|
|
|
|
if (increment < -500)
|
|
{
|
|
increment = -500;
|
|
}
|
|
else if (increment > 500)
|
|
{
|
|
increment = 500;
|
|
}
|
|
|
|
m_servo.pulsewidth_us(1500 + increment*(double)m_dir);
|
|
|
|
m_latestPosition = position;
|
|
}
|
|
|
|
void Servo::invert()
|
|
{
|
|
m_dir = -1;
|
|
}
|
|
|
|
void Servo::sweep(double from, double to, double intervalInSec)
|
|
{
|
|
stop();
|
|
|
|
pServoThread = new Thread;
|
|
|
|
SweepData sweepData;
|
|
sweepData.from = from;
|
|
sweepData.to = to;
|
|
sweepData.intervalInSec = intervalInSec;
|
|
sweepData.pServo = &m_servo;
|
|
|
|
pServoThread->start(callback(sweepWorker, &sweepData));
|
|
wait(5); // Task will run for at least this long (can get destroyed after)
|
|
}
|
|
|
|
void Servo::nod()
|
|
{
|
|
stop();
|
|
|
|
NodData nodData;
|
|
nodData.pServo = &m_servo;
|
|
nodData.latestPosition = m_latestPosition;
|
|
|
|
pServoThread = new Thread;
|
|
|
|
pServoThread->start(callback(nodWorker, &nodData));
|
|
wait(1);
|
|
pServoThread->join(); // Wait for the nod to end
|
|
}
|
|
|
|
void Servo::stop()
|
|
{
|
|
if (pServoThread != NULL && pServoThread->get_state() != Thread::Deleted)
|
|
{
|
|
pServoThread->terminate();
|
|
delete pServoThread;
|
|
pServoThread = NULL;
|
|
}
|
|
}
|
|
|
|
} // namespace drivers
|