Project tutorial
Super Accurate 300€ Robot Arm

Super Accurate 300€ Robot Arm © CC0

6-axis robot arm built with hacked servos demonstrating fast and exact inverse kinematic movements.

  • 12,501 views
  • 2 comments
  • 28 respects

Components and supplies

Necessary tools and machines

3drag
3D Printer (generic)
09507 01
Soldering iron (generic)

About this project


Background

This is a very long project in the making for me. You could say that it started when I ordered six endless servos in middle school, more than 15 years ago, to build a 6-axis robot arm. When I finally got them and powered them up, disappointment washed over me. They could not hold an angle... This is of course because endless servos lack feedback. I had just spent a good portion of my young self's hard earned money on the wrong kind of servos... Instead of accepting defeat and buying new servos, I decided to modify the ones I had. After countless different approaches and attempts I finally got the robot I wanted. The video below show my first 6-axis robot:

What I ended up doing was to put two reflection IR sensors in each servos DC motor, basically creating a 2-bit gray code encoder. I then used logical gates to interpret if each motor was on the right position or if the motor should turn cw or ccw to get to the right position. This worked great, but the control was very jittery since the motors were either full on or off. So I continued my quest for the perfect servo hack. And today I think I have reached something that is good enough.

Theory

The control system of a regular hobby servo looks something like this:

This control scheme is good at holding and moving fast between position, but not so good at following a smooth predefined motion. Which is what you want when you build a robot.

Industrial servo controllers use a cascade based control scheme instead. This is the control scheme used in this project and it looks something like this:

The main benefit of this approach is that it also takes the velocity and torque into account. This allows for a much tighter motion tracking.

Backlash Control

When dealing with cheap gearboxes, backlash is always a problem. The hacked servos in this project uses two encoders to compensate for backlash. One AS5048a magnetic encoder on the output shaft and one custom optical encoder inside the DC motor. The benefit of having the extra encoder inside the motor is getting higher resolution and a main control loop which is backlash free. If one would only use the AS5048a for controlling, the backlash would enter the control loop as a time delay. This limits the control loop performance.

The backlash compensation is done by moving the main control loops reference-position so that the output encoder reaches the correct position.

Optical Encoder Inside DC Motor

Instructions

See README.md in repository root for more info and step-by-step instructions

Comment on the software

This project uses my ThreadHandler Arduino library and I have found i very useful. You can read more about it here (https://create.arduino.cc/projecthub/adamb314/interrupting-thread-handler).

Code

ArduinoSketch.inoArduino
#undef max
#undef min

#include "ArduinoC++BugFixes.h"
#include "ThreadHandler.h"

#include "config/config.h"

SET_THREAD_HANDLER_TICK(200);
THREAD_HANDLER_WITH_EXECUTION_ORDER_OPTIMIZED(InterruptTimer::getInstance());

ThreadHandler* threadHandler = ThreadHandler::getInstance();

std::unique_ptr<Communication> communication;

void setup()
{
    communication = ConfigHolder::getCommunicationHandler();
    threadHandler->enableThreadExecution();
}

void loop()
{
    communication->run();
}
CommunicationHandlers.cppArduino
#include "CommunicationHandlers.h"
#include "DCServo.h"

static DCServo* dcServo = nullptr;
static ThreadHandler* threadHandler = nullptr;

DCServoCommunicationHandler::DCServoCommunicationHandler(unsigned char nodeNr) :
    CommunicationNode(nodeNr)
{
    dcServo = DCServo::getInstance();
    threadHandler = ThreadHandler::getInstance();
    CommunicationNode::intArray[0] = dcServo->getPosition() * positionUpscaling;
    CommunicationNode::intArray[1] = 0;
    CommunicationNode::intArray[2] = 0;
    CommunicationNode::charArray[1] = 0;
    CommunicationNode::charArray[2] = 0;
    CommunicationNode::charArray[3] = 0;
    CommunicationNode::charArray[4] = 0;

    intArrayIndex0Upscaler.set(CommunicationNode::intArray[0]);
}

DCServoCommunicationHandler::~DCServoCommunicationHandler()
{
}

void DCServoCommunicationHandler::onReadyToSendEvent()
{
}

void DCServoCommunicationHandler::onReceiveCompleteEvent()
{
    dcServo->onlyUseMainEncoder(CommunicationNode::charArray[2] == 1);

    if (CommunicationNode::charArrayChanged[3])
    {
        dcServo->setControlSpeed(CommunicationNode::charArray[3]);
    }

    if (CommunicationNode::charArrayChanged[4])
    {
        dcServo->setBacklashControlSpeed(CommunicationNode::charArray[4]);
    }

    if (CommunicationNode::intArrayChanged[0])
    {
        intArrayIndex0Upscaler.update(CommunicationNode::intArray[0]);
        dcServo->loadNewReference(intArrayIndex0Upscaler.get() * (1.0 / positionUpscaling), CommunicationNode::intArray[1], CommunicationNode::intArray[2]);

        CommunicationNode::intArrayChanged[0] = false;
        dcServo->openLoopMode(false);
        dcServo->enable(true);

        statusLight.showEnabled();
    }
    else if (CommunicationNode::intArrayChanged[2])
    {
        intArrayIndex0Upscaler.update(CommunicationNode::intArray[0]);
        dcServo->loadNewReference(intArrayIndex0Upscaler.get() * (1.0 / positionUpscaling), CommunicationNode::intArray[1], CommunicationNode::intArray[2]);

        CommunicationNode::intArrayChanged[2] = false;
        dcServo->openLoopMode(true, CommunicationNode::charArray[1] == 1);
        dcServo->enable(true);

        statusLight.showOpenLoop();
    }
    else
    {
        dcServo->enable(false);

        statusLight.showDisabled();
    }
}

void DCServoCommunicationHandler::onErrorEvent()
{
}

void DCServoCommunicationHandler::onComCycleEvent()
{
    {
        ThreadInterruptBlocker blocker;

        CommunicationNode::intArray[3] = dcServo->getPosition() * positionUpscaling;
        CommunicationNode::intArray[4] = dcServo->getVelocity();
        CommunicationNode::intArray[5] = dcServo->getControlSignal();
        CommunicationNode::intArray[6] = dcServo->getCurrent();
        CommunicationNode::intArray[7] = dcServo->getPwmControlSignal();
        CommunicationNode::intArray[8] = threadHandler->getCpuLoad();
        CommunicationNode::intArray[9] = dcServo->getLoopNumber();
        CommunicationNode::intArray[10] = dcServo->getMainEncoderPosition() * positionUpscaling;
        CommunicationNode::intArray[11] = dcServo->getBacklashCompensation() * positionUpscaling;

        auto opticalEncoderChannelData = dcServo->getMainEncoderDiagnosticData<OpticalEncoderHandler::DiagnosticData>();
        CommunicationNode::intArray[12] = opticalEncoderChannelData.a;
        CommunicationNode::intArray[13] = opticalEncoderChannelData.b;
        CommunicationNode::intArray[14] = opticalEncoderChannelData.minCostIndex;
        CommunicationNode::intArray[15] = opticalEncoderChannelData.minCost;

        if (dcServo->isEnabled())
        {
            dcServo->triggerReferenceTiming();
        }
    }

    statusLight.showCommunicationActive();
}

void DCServoCommunicationHandler::onComIdleEvent()
{
    CommunicationNode::intArrayChanged[0] = false;
    CommunicationNode::intArrayChanged[2] = false;
    dcServo->enable(false);

    statusLight.showDisabled();
    statusLight.showCommunicationInactive();
}
DCServo.cppArduino
#include "DCServo.h"

DCServo* DCServo::getInstance()
{
    static DCServo dcServo;
    return &dcServo;
}

DCServo::DCServo() :
        controlEnabled(false),
        onlyUseMainEncoderControl(false),
        openLoopControlMode(false),
        pwmOpenLoopMode(false),
        loopNumber(0),
        current(0),
        controlSignal(0),
        uLimitDiff(0),
        Ivel(0),
        mainEncoderHandler(ConfigHolder::createMainEncoderHandler()),
        outputEncoderHandler(ConfigHolder::createOutputEncoderHandler()),
        kalmanFilter(std::make_unique<KalmanFilter>())
{
    threads.push_back(createThread(2, 1200, 0,
        [&]()
        {
            mainEncoderHandler->triggerSample();
            if (outputEncoderHandler)
            {
                outputEncoderHandler->triggerSample();
            }
        }));

    refInterpolator.setGetTimeInterval(1200);
    refInterpolator.setLoadTimeInterval(12000);

    currentController = ConfigHolder::createCurrentController();

    mainEncoderHandler->init();
    if (outputEncoderHandler)
    {
        outputEncoderHandler->init();
    }

#ifdef SIMULATE
    rawMainPos = 2048;
    rawOutputPos = rawMainPos;
#else
    mainEncoderHandler->triggerSample();
    if (outputEncoderHandler)
    {
        outputEncoderHandler->triggerSample();
    }

    rawMainPos = mainEncoderHandler->getValue() * ConfigHolder::getMainEncoderGearRation();
    if (outputEncoderHandler)
    {
        rawOutputPos = outputEncoderHandler->getValue();
    }
    else
    {
        rawOutputPos = rawMainPos;
    }
#endif

    initialOutputPosOffset = rawOutputPos - rawMainPos;
    outputPosOffset = initialOutputPosOffset;

    x[0] = rawMainPos;
    x[1] = 0;
    x[2] = 0;

#ifdef SIMULATE
    xSim = x;
#endif

    kalmanFilter->reset(x);

    loadNewReference(x[0], 0, 0);

    threads.push_back(createThread(1, 1200, 0,
        [&]()
        {
            controlLoop();
            loopNumber++;
        }));
}

bool DCServo::isEnabled()
{
    ThreadInterruptBlocker blocker;
    return controlEnabled;
}

void DCServo::enable(bool b)
{
    ThreadInterruptBlocker blocker;
    if (!isEnabled() && b)
    {
        L = ConfigHolder::ControlParameters::getLVector(controlSpeed, backlashControlSpeed);
    }

    controlEnabled = b;

    if (!b)
    {
        refInterpolator.resetTiming();
    }
}

void DCServo::openLoopMode(bool enable, bool pwmMode)
{
    ThreadInterruptBlocker blocker;
    openLoopControlMode = enable;
    pwmOpenLoopMode = pwmMode;
}

void DCServo::onlyUseMainEncoder(bool b)
{
    ThreadInterruptBlocker blocker;
    onlyUseMainEncoderControl = b;
}

void DCServo::setControlSpeed(uint8_t controlSpeed)
{
    this->controlSpeed = controlSpeed;
}

void DCServo::setBacklashControlSpeed(uint8_t backlashControlSpeed)
{
    this->backlashControlSpeed = backlashControlSpeed;
}

void DCServo::loadNewReference(float pos, int16_t vel, int16_t feedForwardU)
{
    ThreadInterruptBlocker blocker;
    refInterpolator.loadNew(pos, vel, feedForwardU);
}

void DCServo::triggerReferenceTiming()
{
    ThreadInterruptBlocker blocker;
    refInterpolator.updateTiming();
}

float DCServo::getPosition()
{
    ThreadInterruptBlocker blocker;
    return rawOutputPos;
}

int16_t DCServo::getVelocity()
{
    ThreadInterruptBlocker blocker;
    return x[1];
}

int16_t DCServo::getControlSignal()
{
    ThreadInterruptBlocker blocker;
    if (controlEnabled)
    {
        return controlSignal;
    }
    return 0;
}

int16_t DCServo::getCurrent()
{
    ThreadInterruptBlocker blocker;
    return current;
}

int16_t DCServo::getPwmControlSignal()
{
    ThreadInterruptBlocker blocker;
    return pwmControlSIgnal;
}

uint16_t DCServo::getLoopNumber()
{
    ThreadInterruptBlocker blocker;
    return loopNumber;
}

float DCServo::getBacklashCompensation()
{
    ThreadInterruptBlocker blocker;
    return outputPosOffset - initialOutputPosOffset;
}

float DCServo::getMainEncoderPosition()
{
    ThreadInterruptBlocker blocker;
    return rawMainPos + initialOutputPosOffset;
}

template <class T>
OpticalEncoderHandler::DiagnosticData getMainEncoderRawDiagnosticDataDispatch(T& encoder)
{
    OpticalEncoderHandler::DiagnosticData out = {0};
    return out;
}

template <>
OpticalEncoderHandler::DiagnosticData getMainEncoderRawDiagnosticDataDispatch(std::unique_ptr<OpticalEncoderHandler>& encoder)
{
    return encoder->getDiagnosticData();
}

template <>
OpticalEncoderHandler::DiagnosticData DCServo::getMainEncoderDiagnosticData()
{
    ThreadInterruptBlocker blocker;
    return getMainEncoderRawDiagnosticDataDispatch(mainEncoderHandler);
}

void DCServo::controlLoop()
{
#ifdef SIMULATE
    xSim = kalmanFilter->getA() * xSim + kalmanFilter->getB() * controlSignal;
    rawMainPos =xSim[0];
    rawOutputPos = rawMainPos;
#else
    rawMainPos = mainEncoderHandler->getValue() * ConfigHolder::getMainEncoderGearRation();
    if (outputEncoderHandler)
    {
        rawOutputPos = outputEncoderHandler->getValue();
    }
    else
    {
        rawOutputPos = rawMainPos;
    }
#endif

    x = kalmanFilter->update(controlSignal, rawMainPos);

    if (controlEnabled)
    {
        if (!openLoopControlMode)
        {
            uLimitDiff = 0.99 * uLimitDiff + 0.01 * (controlSignal - currentController->getLimitedRef());

            Ivel += L[3] * uLimitDiff;

            float posRef;
            float velRef;
            float feedForwardU;

            refInterpolator.getNext(posRef, velRef, feedForwardU);

            if (!onlyUseMainEncoderControl)
            {
                outputPosOffset -= L[4] * 0.0012 * (posRef - rawOutputPos);
            }

            posRef -= outputPosOffset;

            float posDiff = posRef - x[0];

            float vControlRef = L[0] * posDiff + velRef;

            float u = L[1] * (vControlRef - x[1]) + Ivel + feedForwardU;

            controlSignal = u;

            currentController->updateVelocity(x[1]);
            currentController->setReference(static_cast<int16_t>(controlSignal));
            currentController->applyChanges();
            current = currentController->getCurrent();
            pwmControlSIgnal = currentController->getFilteredPwm();

            Ivel -= L[2] * (vControlRef - x[1]);
        }
        else
        {
            Ivel = 0;
            uLimitDiff = 0;
            outputPosOffset = rawOutputPos - rawMainPos;

            float posRef;
            float velRef;
            float feedForwardU;

            refInterpolator.getNext(posRef, velRef, feedForwardU);

            if (pwmOpenLoopMode)
            {
                controlSignal = 0;
                currentController->overidePwmDuty(feedForwardU);
            }
            else
            {
                controlSignal = feedForwardU;
                currentController->setReference(static_cast<int16_t>(controlSignal));
            }
            currentController->applyChanges();
            current = currentController->getCurrent();
            pwmControlSIgnal = currentController->getFilteredPwm();
        }
    }
    else
    {
        loadNewReference(x[0], 0, 0);
        Ivel = 0;
        uLimitDiff = 0;
        outputPosOffset = rawOutputPos - rawMainPos;
        controlSignal = 0;
        currentController->activateBrake();
        currentController->applyChanges();
        current = currentController->getCurrent();
        pwmControlSIgnal = currentController->getFilteredPwm();
    }

}

ReferenceInterpolator::ReferenceInterpolator()
{
}

void ReferenceInterpolator::loadNew(float position, float velocity, float feedForward)
{
    if (timingInvalid)
    {
        midPointTimeOffset = 0;

        pos[2] = position;
        vel[2] = velocity;
        feed[2] = feedForward;

        pos[1] = pos[2];
        vel[1] = vel[2];
        feed[1] = feed[2];

        pos[0] = pos[1];
        vel[0] = vel[1];
        feed[0] = feed[1];
    }
    else
    {
        if (midPointTimeOffset > -loadTimeInterval)
        {
            midPointTimeOffset -= loadTimeInterval;
        }

        pos[0] = pos[1];
        vel[0] = vel[1];
        feed[0] = feed[1];

        pos[1] = pos[2];
        vel[1] = vel[2];
        feed[1] = feed[2];

        pos[2] = position;
        vel[2] = velocity;
        feed[2] = feedForward;
    }
}

void ReferenceInterpolator::updateTiming()
{
    uint16_t timestamp = micros();
    if (timingInvalid)
    {
        midPointTimeOffset = getTimeInterval;

        timingInvalid = false;
    }
    else
    {
        uint16_t updatePeriod = timestamp - lastUpdateTimingTimestamp;
        uint16_t timeSinceLastGet = timestamp - lastGetTimestamp;

        int16_t timingError = getTimeInterval - (midPointTimeOffset + timeSinceLastGet);
        int16_t periodError = updatePeriod - loadTimeInterval;

        midPointTimeOffset += timingError / 8;
        loadTimeInterval += periodError / 16;

        invertedLoadInterval = 1.0 / loadTimeInterval;
    }

    lastUpdateTimingTimestamp = timestamp;
}

void ReferenceInterpolator::resetTiming()
{
    timingInvalid = true;
}

void ReferenceInterpolator::getNext(float& position, float& velocity, float& feedForward)
{
    lastGetTimestamp = micros();

    if (midPointTimeOffset < 2 * loadTimeInterval)
    {
        midPointTimeOffset += getTimeInterval;
    }

    float t = midPointTimeOffset * invertedLoadInterval;

    if (t < -1.0)
    {
        t = -1.0;
    }
    else if (t > 1.2)
    {
        t = 1.2;
    }

    if (t < 0.0)
    {
        t += 1.0;
        position = pos[0] + t * (pos[1] - pos[0]);
        velocity = vel[0] + t * (vel[1] - vel[0]);
        feedForward = feed[0] + t * (feed[1] - feed[0]);
    }
    else
    {
        position = pos[1] + t * (pos[2] - pos[1]);
        velocity = vel[1] + t * (vel[2] - vel[1]);
        feedForward = feed[1] + t * (feed[2] - feed[1]);
    }
}

void ReferenceInterpolator::setGetTimeInterval(const uint16_t& interval)
{
    timingInvalid = true;

    midPointTimeOffset = 0;
    getTimeInterval = interval;
}

void ReferenceInterpolator::setLoadTimeInterval(const uint16_t& interval)
{
    timingInvalid = true;
    midPointTimeOffset = 0;

    loadTimeInterval = interval;
    invertedLoadInterval = 1.0 / loadTimeInterval;
}
git repo
repository with all source files, CAD, schematics and documentation

Schematics

MainPCB schematic
Screenshot from 2020 06 30 14 29 54 vpodqcxjuy
AS5048a board schematic
Screenshot from 2020 06 30 14 41 56 ivcsc9rbvf
Optical Encoder schematic
Screenshot from 2020 07 04 12 46 29 stqssa84a0

Comments

Similar projects you might like

Amazing 6WD Off-Road Robot | Arduino RC Robot

Project tutorial by Jithin Sanal

  • 21,272 views
  • 1 comment
  • 82 respects

DIY Arduino Robot Arm – Controlled by Hand Gestures

Project tutorial by Eben Kouao

  • 12,657 views
  • 9 comments
  • 45 respects

A DIY Tangible Coding Robot

Project tutorial by Tart Robotics

  • 2,519 views
  • 1 comment
  • 10 respects

Scriba Robot - A Printing Robot

Project in progress by RobinB

  • 7,293 views
  • 1 comment
  • 18 respects

MeArm Robot Arm - Your Robot - V1.0

Project tutorial by Benjamin Gray

  • 41,724 views
  • 4 comments
  • 57 respects

MK2 Plus Robot Arm Controller

Project tutorial by Samira Peiris

  • 23,224 views
  • 7 comments
  • 37 respects
Add projectSign up / Login