Super Accurate 300€ Robot Arm

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

Jun 21, 2020

•

16995 views

•

17 respects

robot arm

hacked servo

inverse kinematic

Components and supplies

MG90S Micro-servo motor

Mean Well LRS-50-5 Switching Power Supply, Single Output, 5V, 10A

Adafruit ItsyBitsy M0 Express

ITR8307

SparkFun Motor Driver - Dual TB6612FNG (1A)

ACS711EX Current Sensor

Turnigy MG959 servo

as5048A

Through Hole Resistor, 8 kohm

Arduino MKR Zero

Through Hole Resistor, 120 ohm

Tools and machines

Soldering iron (generic)

3D Printer (generic)

Project description

Code

ArduinoSketch.ino

arduino

1#undef max
2#undef min
3
4#include "ArduinoC++BugFixes.h"
5#include   "ThreadHandler.h"
6
7#include "config/config.h"
8
9SET_THREAD_HANDLER_TICK(200);
10THREAD_HANDLER_WITH_EXECUTION_ORDER_OPTIMIZED(InterruptTimer::getInstance());
11
12ThreadHandler*   threadHandler = ThreadHandler::getInstance();
13
14std::unique_ptr<Communication>   communication;
15
16void setup()
17{
18    communication = ConfigHolder::getCommunicationHandler();
19     threadHandler->enableThreadExecution();
20}
21
22void loop()
23{
24    communication->run();
25}
26

#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();
}

git repo

repository with all source files, CAD, schematics and documentation

no description/ Read More

Latest commit to the master branch on Invalid date

Download as a zip

DCServo.cpp

arduino

1#include "DCServo.h"
2
3DCServo* DCServo::getInstance()
4{
5    static DCServo dcServo;
6    return &dcServo;
7}
8
9DCServo::DCServo()  :
10        controlEnabled(false),
11        onlyUseMainEncoderControl(false),
12        openLoopControlMode(false),
13        pwmOpenLoopMode(false),
14        loopNumber(0),
15        current(0),
16        controlSignal(0),
17        uLimitDiff(0),
18        Ivel(0),
19        mainEncoderHandler(ConfigHolder::createMainEncoderHandler()),
20        outputEncoderHandler(ConfigHolder::createOutputEncoderHandler()),
21        kalmanFilter(std::make_unique<KalmanFilter>())
22{
23    threads.push_back(createThread(2,  1200, 0,
24        [&]()
25        {
26            mainEncoderHandler->triggerSample();
27            if (outputEncoderHandler)
28            {
29                outputEncoderHandler->triggerSample();
30            }
31        }));
32
33    refInterpolator.setGetTimeInterval(1200);
34    refInterpolator.setLoadTimeInterval(12000);
35
36    currentController = ConfigHolder::createCurrentController();
37
38    mainEncoderHandler->init();
39    if (outputEncoderHandler)
40    {
41        outputEncoderHandler->init();
42    }
43
44#ifdef SIMULATE
45    rawMainPos = 2048;
46    rawOutputPos = rawMainPos;
47#else
48    mainEncoderHandler->triggerSample();
49    if (outputEncoderHandler)
50    {
51        outputEncoderHandler->triggerSample();
52    }
53
54    rawMainPos =  mainEncoderHandler->getValue() * ConfigHolder::getMainEncoderGearRation();
55    if  (outputEncoderHandler)
56    {
57        rawOutputPos = outputEncoderHandler->getValue();
58    }
59    else
60    {
61        rawOutputPos = rawMainPos;
62    }
63#endif
64
65    initialOutputPosOffset = rawOutputPos - rawMainPos;
66    outputPosOffset =  initialOutputPosOffset;
67
68    x[0] = rawMainPos;
69    x[1] = 0;
70    x[2]  = 0;
71
72#ifdef SIMULATE
73    xSim = x;
74#endif
75
76    kalmanFilter->reset(x);
77
78    loadNewReference(x[0], 0, 0);
79
80    threads.push_back(createThread(1, 1200,  0,
81        [&]()
82        {
83            controlLoop();
84            loopNumber++;
85        }));
86}
87
88bool DCServo::isEnabled()
89{
90    ThreadInterruptBlocker  blocker;
91    return controlEnabled;
92}
93
94void DCServo::enable(bool b)
95{
96    ThreadInterruptBlocker blocker;
97    if (!isEnabled() && b)
98    {
99        L  = ConfigHolder::ControlParameters::getLVector(controlSpeed, backlashControlSpeed);
100    }
101
102    controlEnabled = b;
103
104    if (!b)
105    {
106        refInterpolator.resetTiming();
107    }
108}
109
110void DCServo::openLoopMode(bool enable, bool pwmMode)
111{
112    ThreadInterruptBlocker blocker;
113    openLoopControlMode = enable;
114    pwmOpenLoopMode  = pwmMode;
115}
116
117void DCServo::onlyUseMainEncoder(bool b)
118{
119    ThreadInterruptBlocker  blocker;
120    onlyUseMainEncoderControl = b;
121}
122
123void DCServo::setControlSpeed(uint8_t  controlSpeed)
124{
125    this->controlSpeed = controlSpeed;
126}
127
128void DCServo::setBacklashControlSpeed(uint8_t  backlashControlSpeed)
129{
130    this->backlashControlSpeed = backlashControlSpeed;
131}
132
133void  DCServo::loadNewReference(float pos, int16_t vel, int16_t feedForwardU)
134{
135    ThreadInterruptBlocker blocker;
136    refInterpolator.loadNew(pos, vel, feedForwardU);
137}
138
139void  DCServo::triggerReferenceTiming()
140{
141    ThreadInterruptBlocker blocker;
142    refInterpolator.updateTiming();
143}
144
145float DCServo::getPosition()
146{
147    ThreadInterruptBlocker blocker;
148    return rawOutputPos;
149}
150
151int16_t  DCServo::getVelocity()
152{
153    ThreadInterruptBlocker blocker;
154    return  x[1];
155}
156
157int16_t DCServo::getControlSignal()
158{
159    ThreadInterruptBlocker  blocker;
160    if (controlEnabled)
161    {
162        return controlSignal;
163    }
164    return 0;
165}
166
167int16_t DCServo::getCurrent()
168{
169    ThreadInterruptBlocker  blocker;
170    return current;
171}
172
173int16_t DCServo::getPwmControlSignal()
174{
175    ThreadInterruptBlocker blocker;
176    return pwmControlSIgnal;
177}
178
179uint16_t  DCServo::getLoopNumber()
180{
181    ThreadInterruptBlocker blocker;
182    return  loopNumber;
183}
184
185float DCServo::getBacklashCompensation()
186{
187    ThreadInterruptBlocker  blocker;
188    return outputPosOffset - initialOutputPosOffset;
189}
190
191float  DCServo::getMainEncoderPosition()
192{
193    ThreadInterruptBlocker blocker;
194    return rawMainPos + initialOutputPosOffset;
195}
196
197template <class T>
198OpticalEncoderHandler::DiagnosticData  getMainEncoderRawDiagnosticDataDispatch(T& encoder)
199{
200    OpticalEncoderHandler::DiagnosticData  out = {0};
201    return out;
202}
203
204template <>
205OpticalEncoderHandler::DiagnosticData  getMainEncoderRawDiagnosticDataDispatch(std::unique_ptr<OpticalEncoderHandler>&  encoder)
206{
207    return encoder->getDiagnosticData();
208}
209
210template <>
211OpticalEncoderHandler::DiagnosticData  DCServo::getMainEncoderDiagnosticData()
212{
213    ThreadInterruptBlocker blocker;
214    return getMainEncoderRawDiagnosticDataDispatch(mainEncoderHandler);
215}
216
217void  DCServo::controlLoop()
218{
219#ifdef SIMULATE
220    xSim = kalmanFilter->getA()  * xSim + kalmanFilter->getB() * controlSignal;
221    rawMainPos =xSim[0];
222    rawOutputPos  = rawMainPos;
223#else
224    rawMainPos = mainEncoderHandler->getValue() * ConfigHolder::getMainEncoderGearRation();
225    if (outputEncoderHandler)
226    {
227        rawOutputPos = outputEncoderHandler->getValue();
228    }
229    else
230    {
231        rawOutputPos = rawMainPos;
232    }
233#endif
234
235    x = kalmanFilter->update(controlSignal, rawMainPos);
236
237    if (controlEnabled)
238    {
239        if (!openLoopControlMode)
240        {
241            uLimitDiff  = 0.99 * uLimitDiff + 0.01 * (controlSignal - currentController->getLimitedRef());
242
243            Ivel += L[3] * uLimitDiff;
244
245            float posRef;
246            float  velRef;
247            float feedForwardU;
248
249            refInterpolator.getNext(posRef,  velRef, feedForwardU);
250
251            if (!onlyUseMainEncoderControl)
252            {
253                outputPosOffset -= L[4] * 0.0012 * (posRef - rawOutputPos);
254            }
255
256            posRef -= outputPosOffset;
257
258            float  posDiff = posRef - x[0];
259
260            float vControlRef = L[0] * posDiff +  velRef;
261
262            float u = L[1] * (vControlRef - x[1]) + Ivel + feedForwardU;
263
264            controlSignal = u;
265
266            currentController->updateVelocity(x[1]);
267            currentController->setReference(static_cast<int16_t>(controlSignal));
268            currentController->applyChanges();
269            current = currentController->getCurrent();
270            pwmControlSIgnal = currentController->getFilteredPwm();
271
272            Ivel  -= L[2] * (vControlRef - x[1]);
273        }
274        else
275        {
276            Ivel  = 0;
277            uLimitDiff = 0;
278            outputPosOffset = rawOutputPos  - rawMainPos;
279
280            float posRef;
281            float velRef;
282            float feedForwardU;
283
284            refInterpolator.getNext(posRef,  velRef, feedForwardU);
285
286            if (pwmOpenLoopMode)
287            {
288                controlSignal = 0;
289                currentController->overidePwmDuty(feedForwardU);
290            }
291            else
292            {
293                controlSignal  = feedForwardU;
294                currentController->setReference(static_cast<int16_t>(controlSignal));
295            }
296            currentController->applyChanges();
297            current  = currentController->getCurrent();
298            pwmControlSIgnal = currentController->getFilteredPwm();
299        }
300    }
301    else
302    {
303        loadNewReference(x[0], 0, 0);
304        Ivel = 0;
305        uLimitDiff = 0;
306        outputPosOffset = rawOutputPos  - rawMainPos;
307        controlSignal = 0;
308        currentController->activateBrake();
309        currentController->applyChanges();
310        current = currentController->getCurrent();
311        pwmControlSIgnal = currentController->getFilteredPwm();
312    }
313
314}
315
316ReferenceInterpolator::ReferenceInterpolator()
317{
318}
319
320void  ReferenceInterpolator::loadNew(float position, float velocity, float feedForward)
321{
322    if (timingInvalid)
323    {
324        midPointTimeOffset = 0;
325
326        pos[2]  = position;
327        vel[2] = velocity;
328        feed[2] = feedForward;
329
330        pos[1] = pos[2];
331        vel[1] = vel[2];
332        feed[1] = feed[2];
333
334        pos[0] = pos[1];
335        vel[0] = vel[1];
336        feed[0] = feed[1];
337    }
338    else
339    {
340        if (midPointTimeOffset > -loadTimeInterval)
341        {
342            midPointTimeOffset -= loadTimeInterval;
343        }
344
345        pos[0] = pos[1];
346        vel[0] = vel[1];
347        feed[0] = feed[1];
348
349        pos[1] = pos[2];
350        vel[1] = vel[2];
351        feed[1] = feed[2];
352
353        pos[2] = position;
354        vel[2] = velocity;
355        feed[2] = feedForward;
356    }
357}
358
359void ReferenceInterpolator::updateTiming()
360{
361    uint16_t  timestamp = micros();
362    if (timingInvalid)
363    {
364        midPointTimeOffset  = getTimeInterval;
365
366        timingInvalid = false;
367    }
368    else
369    {
370        uint16_t updatePeriod = timestamp - lastUpdateTimingTimestamp;
371        uint16_t timeSinceLastGet = timestamp - lastGetTimestamp;
372
373        int16_t  timingError = getTimeInterval - (midPointTimeOffset + timeSinceLastGet);
374        int16_t  periodError = updatePeriod - loadTimeInterval;
375
376        midPointTimeOffset  += timingError / 8;
377        loadTimeInterval += periodError / 16;
378
379        invertedLoadInterval  = 1.0 / loadTimeInterval;
380    }
381
382    lastUpdateTimingTimestamp = timestamp;
383}
384
385void  ReferenceInterpolator::resetTiming()
386{
387    timingInvalid = true;
388}
389
390void  ReferenceInterpolator::getNext(float& position, float& velocity, float& feedForward)
391{
392    lastGetTimestamp = micros();
393
394    if (midPointTimeOffset < 2 * loadTimeInterval)
395    {
396        midPointTimeOffset += getTimeInterval;
397    }
398
399    float  t = midPointTimeOffset * invertedLoadInterval;
400
401    if (t < -1.0)
402    {
403        t = -1.0;
404    }
405    else if (t > 1.2)
406    {
407        t = 1.2;
408    }
409
410    if (t < 0.0)
411    {
412        t += 1.0;
413        position  = pos[0] + t * (pos[1] - pos[0]);
414        velocity = vel[0] + t * (vel[1] - vel[0]);
415        feedForward = feed[0] + t * (feed[1] - feed[0]);
416    }
417    else
418    {
419        position = pos[1] + t * (pos[2] - pos[1]);
420        velocity  = vel[1] + t * (vel[2] - vel[1]);
421        feedForward = feed[1] + t * (feed[2]  - feed[1]);
422    }
423}
424
425void ReferenceInterpolator::setGetTimeInterval(const  uint16_t& interval)
426{
427    timingInvalid = true;
428
429    midPointTimeOffset  = 0;
430    getTimeInterval = interval;
431}
432
433void ReferenceInterpolator::setLoadTimeInterval(const  uint16_t& interval)
434{
435    timingInvalid = true;
436    midPointTimeOffset  = 0;
437
438    loadTimeInterval = interval;
439    invertedLoadInterval = 1.0 /  loadTimeInterval;
440}
441

#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;
}

CommunicationHandlers.cpp

arduino

1#include "CommunicationHandlers.h"
2#include "DCServo.h"
3
4static  DCServo* dcServo = nullptr;
5static ThreadHandler* threadHandler = nullptr;
6
7DCServoCommunicationHandler::DCServoCommunicationHandler(unsigned  char nodeNr) :
8    CommunicationNode(nodeNr)
9{
10    dcServo = DCServo::getInstance();
11    threadHandler = ThreadHandler::getInstance();
12    CommunicationNode::intArray[0]  = dcServo->getPosition() * positionUpscaling;
13    CommunicationNode::intArray[1]  = 0;
14    CommunicationNode::intArray[2] = 0;
15    CommunicationNode::charArray[1]  = 0;
16    CommunicationNode::charArray[2] = 0;
17    CommunicationNode::charArray[3]  = 0;
18    CommunicationNode::charArray[4] = 0;
19
20    intArrayIndex0Upscaler.set(CommunicationNode::intArray[0]);
21}
22
23DCServoCommunicationHandler::~DCServoCommunicationHandler()
24{
25}
26
27void  DCServoCommunicationHandler::onReadyToSendEvent()
28{
29}
30
31void DCServoCommunicationHandler::onReceiveCompleteEvent()
32{
33    dcServo->onlyUseMainEncoder(CommunicationNode::charArray[2] == 1);
34
35    if  (CommunicationNode::charArrayChanged[3])
36    {
37        dcServo->setControlSpeed(CommunicationNode::charArray[3]);
38    }
39
40    if (CommunicationNode::charArrayChanged[4])
41    {
42        dcServo->setBacklashControlSpeed(CommunicationNode::charArray[4]);
43    }
44
45    if (CommunicationNode::intArrayChanged[0])
46    {
47        intArrayIndex0Upscaler.update(CommunicationNode::intArray[0]);
48        dcServo->loadNewReference(intArrayIndex0Upscaler.get() * (1.0 / positionUpscaling),  CommunicationNode::intArray[1], CommunicationNode::intArray[2]);
49
50        CommunicationNode::intArrayChanged[0]  = false;
51        dcServo->openLoopMode(false);
52        dcServo->enable(true);
53
54        statusLight.showEnabled();
55    }
56    else if (CommunicationNode::intArrayChanged[2])
57    {
58        intArrayIndex0Upscaler.update(CommunicationNode::intArray[0]);
59        dcServo->loadNewReference(intArrayIndex0Upscaler.get() * (1.0 / positionUpscaling),  CommunicationNode::intArray[1], CommunicationNode::intArray[2]);
60
61        CommunicationNode::intArrayChanged[2]  = false;
62        dcServo->openLoopMode(true, CommunicationNode::charArray[1]  == 1);
63        dcServo->enable(true);
64
65        statusLight.showOpenLoop();
66    }
67    else
68    {
69        dcServo->enable(false);
70
71        statusLight.showDisabled();
72    }
73}
74
75void DCServoCommunicationHandler::onErrorEvent()
76{
77}
78
79void  DCServoCommunicationHandler::onComCycleEvent()
80{
81    {
82        ThreadInterruptBlocker  blocker;
83
84        CommunicationNode::intArray[3] = dcServo->getPosition()  * positionUpscaling;
85        CommunicationNode::intArray[4] = dcServo->getVelocity();
86        CommunicationNode::intArray[5] = dcServo->getControlSignal();
87        CommunicationNode::intArray[6]  = dcServo->getCurrent();
88        CommunicationNode::intArray[7] = dcServo->getPwmControlSignal();
89        CommunicationNode::intArray[8] = threadHandler->getCpuLoad();
90        CommunicationNode::intArray[9]  = dcServo->getLoopNumber();
91        CommunicationNode::intArray[10] = dcServo->getMainEncoderPosition()  * positionUpscaling;
92        CommunicationNode::intArray[11] = dcServo->getBacklashCompensation()  * positionUpscaling;
93
94        auto opticalEncoderChannelData = dcServo->getMainEncoderDiagnosticData<OpticalEncoderHandler::DiagnosticData>();
95        CommunicationNode::intArray[12] = opticalEncoderChannelData.a;
96        CommunicationNode::intArray[13]  = opticalEncoderChannelData.b;
97        CommunicationNode::intArray[14] = opticalEncoderChannelData.minCostIndex;
98        CommunicationNode::intArray[15] = opticalEncoderChannelData.minCost;
99
100        if (dcServo->isEnabled())
101        {
102            dcServo->triggerReferenceTiming();
103        }
104    }
105
106    statusLight.showCommunicationActive();
107}
108
109void  DCServoCommunicationHandler::onComIdleEvent()
110{
111    CommunicationNode::intArrayChanged[0]  = false;
112    CommunicationNode::intArrayChanged[2] = false;
113    dcServo->enable(false);
114
115    statusLight.showDisabled();
116    statusLight.showCommunicationInactive();
117}
118

#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();
}

git repo

repository with all source files, CAD, schematics and documentation

no description/ Read More

Latest commit to the master branch on Invalid date

Download as a zip

CommunicationHandlers.cpp

arduino

1#include "CommunicationHandlers.h"
2#include "DCServo.h"
3
4static
5  DCServo* dcServo = nullptr;
6static ThreadHandler* threadHandler = nullptr;
7
8DCServoCommunicationHandler::DCServoCommunicationHandler(unsigned
9  char nodeNr) :
10    CommunicationNode(nodeNr)
11{
12    dcServo = DCServo::getInstance();
13
14    threadHandler = ThreadHandler::getInstance();
15    CommunicationNode::intArray[0]
16  = dcServo->getPosition() * positionUpscaling;
17    CommunicationNode::intArray[1]
18  = 0;
19    CommunicationNode::intArray[2] = 0;
20    CommunicationNode::charArray[1]
21  = 0;
22    CommunicationNode::charArray[2] = 0;
23    CommunicationNode::charArray[3]
24  = 0;
25    CommunicationNode::charArray[4] = 0;
26
27    intArrayIndex0Upscaler.set(CommunicationNode::intArray[0]);
28}
29
30DCServoCommunicationHandler::~DCServoCommunicationHandler()
31{
32}
33
34void
35  DCServoCommunicationHandler::onReadyToSendEvent()
36{
37}
38
39void DCServoCommunicationHandler::onReceiveCompleteEvent()
40{
41
42    dcServo->onlyUseMainEncoder(CommunicationNode::charArray[2] == 1);
43
44    if
45  (CommunicationNode::charArrayChanged[3])
46    {
47        dcServo->setControlSpeed(CommunicationNode::charArray[3]);
48
49    }
50
51    if (CommunicationNode::charArrayChanged[4])
52    {
53        dcServo->setBacklashControlSpeed(CommunicationNode::charArray[4]);
54
55    }
56
57    if (CommunicationNode::intArrayChanged[0])
58    {
59        intArrayIndex0Upscaler.update(CommunicationNode::intArray[0]);
60
61        dcServo->loadNewReference(intArrayIndex0Upscaler.get() * (1.0 / positionUpscaling),
62  CommunicationNode::intArray[1], CommunicationNode::intArray[2]);
63
64        CommunicationNode::intArrayChanged[0]
65  = false;
66        dcServo->openLoopMode(false);
67        dcServo->enable(true);
68
69
70        statusLight.showEnabled();
71    }
72    else if (CommunicationNode::intArrayChanged[2])
73
74    {
75        intArrayIndex0Upscaler.update(CommunicationNode::intArray[0]);
76
77        dcServo->loadNewReference(intArrayIndex0Upscaler.get() * (1.0 / positionUpscaling),
78  CommunicationNode::intArray[1], CommunicationNode::intArray[2]);
79
80        CommunicationNode::intArrayChanged[2]
81  = false;
82        dcServo->openLoopMode(true, CommunicationNode::charArray[1]
83  == 1);
84        dcServo->enable(true);
85
86        statusLight.showOpenLoop();
87
88    }
89    else
90    {
91        dcServo->enable(false);
92
93        statusLight.showDisabled();
94
95    }
96}
97
98void DCServoCommunicationHandler::onErrorEvent()
99{
100}
101
102void
103  DCServoCommunicationHandler::onComCycleEvent()
104{
105    {
106        ThreadInterruptBlocker
107  blocker;
108
109        CommunicationNode::intArray[3] = dcServo->getPosition()
110  * positionUpscaling;
111        CommunicationNode::intArray[4] = dcServo->getVelocity();
112
113        CommunicationNode::intArray[5] = dcServo->getControlSignal();
114        CommunicationNode::intArray[6]
115  = dcServo->getCurrent();
116        CommunicationNode::intArray[7] = dcServo->getPwmControlSignal();
117
118        CommunicationNode::intArray[8] = threadHandler->getCpuLoad();
119        CommunicationNode::intArray[9]
120  = dcServo->getLoopNumber();
121        CommunicationNode::intArray[10] = dcServo->getMainEncoderPosition()
122  * positionUpscaling;
123        CommunicationNode::intArray[11] = dcServo->getBacklashCompensation()
124  * positionUpscaling;
125
126        auto opticalEncoderChannelData = dcServo->getMainEncoderDiagnosticData<OpticalEncoderHandler::DiagnosticData>();
127
128        CommunicationNode::intArray[12] = opticalEncoderChannelData.a;
129        CommunicationNode::intArray[13]
130  = opticalEncoderChannelData.b;
131        CommunicationNode::intArray[14] = opticalEncoderChannelData.minCostIndex;
132
133        CommunicationNode::intArray[15] = opticalEncoderChannelData.minCost;
134
135
136        if (dcServo->isEnabled())
137        {
138            dcServo->triggerReferenceTiming();
139
140        }
141    }
142
143    statusLight.showCommunicationActive();
144}
145
146void
147  DCServoCommunicationHandler::onComIdleEvent()
148{
149    CommunicationNode::intArrayChanged[0]
150  = false;
151    CommunicationNode::intArrayChanged[2] = false;
152    dcServo->enable(false);
153
154
155    statusLight.showDisabled();
156    statusLight.showCommunicationInactive();
157}
158

#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();
}

ArduinoSketch.ino

arduino

1#undef max
2#undef min
3
4#include "ArduinoC++BugFixes.h"
5#include  "ThreadHandler.h"
6
7#include "config/config.h"
8
9SET_THREAD_HANDLER_TICK(200);
10THREAD_HANDLER_WITH_EXECUTION_ORDER_OPTIMIZED(InterruptTimer::getInstance());
11
12ThreadHandler*  threadHandler = ThreadHandler::getInstance();
13
14std::unique_ptr<Communication>  communication;
15
16void setup()
17{
18    communication = ConfigHolder::getCommunicationHandler();
19    threadHandler->enableThreadExecution();
20}
21
22void loop()
23{
24    communication->run();
25}
26

#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();
}

Downloadable files

Optical Encoder schematic

AS5048a board schematic

MainPCB schematic

Optical Encoder schematic

MainPCB schematic

AS5048a board schematic

Comments

Only logged in users can leave comments

adamb314

0 Followers

•

0 Projects

Intro