File content as of revision 33:8b9dcbf6d8ec:

// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

#ifndef __ROBOT_ARM_H__
#define __ROBOT_ARM_H__

#include "mbed.h"
#include "rtos.h"

#include "DynamixelBus.h"
#include "NodeAX12.h"
#include "RobotArmCfg.h"


class RobotArm
{
public:
    
    RobotArm();
    
    // move all parts to specified postions in ms time - start
    bool MoveArmPositionsStart(float positions[], int ms);
    
    // move all parts to specified postions - resume after pause
    bool MoveArmPositionsResume();

    // move all parts to specified postions - test if done
    bool MoveArmPositionsHasNext();
    
    // move all parts to specified postions - next step
    bool MoveArmPositionsNext();

    // move all parts to specified postions - next step delay
    bool MoveArmPositionsDelay(int& nextdelay);

    // move all parts to specified postions - finish
    bool MoveArmPositionsEnd();
    
    // reset goal to the current position ie.  stop trying to move
    void MoveArmPositionsStop();
    
    // clear part error state
    void ClearErrorState();
    
    // get all parts positions
    bool GetArmPositions(float outVals[]);
    
    // get all parts last positions
    bool GetArmLastPositions(float outVals[]);
    
    // prepare to test arm state
    void ArmMeasuresTestStart();
    
    // test if measurements are in expected range
    int ArmMeasuresTest(int measureId);
    
    // get all parts for a measurement
    bool GetArmMeasure(int measureId, float outVals[]);
    
    // get all parts last measurement
    bool GetArmLastMeasure(int measureId, float outVals[]);
   
    int GetNumParts();
    
    // set arm speed as ms between steps
    void SetStepMs(int stepms);
    
    // set ThreadId for signals
    void SetThreadId(osThreadId tid);
    
    // get the part object
    RobotNode* GetArmPart(int partIx);
    
    // get last error code from action
    int GetLastError();
    
    // get index of part with error
    int GetLastErrorPart();
    
    // get size of position diff (valid if error)
    float GetLastPosDiff();
    
private:
    // sensors and actuators
    RobotNode* _armParts[NUMJOINTS];
    
    int _numParts;
    
    // #ms between steps
    int _stepms;
    
    // thread id
    osThreadId _tid;
    
    // part ix for last error
    int _lastErrorPart;
    // last HW error
    int _lastError;
    // last position error
    float _lastPosDiff;
    
    // step-wise position moves
    float _endgoals[NUMJOINTS];
    float _differentials[NUMJOINTS];
    float _lastpos[NUMJOINTS];
    float _lastgoals[NUMJOINTS];
    
    // keep track of time period when position is off
    int _failms[NUMJOINTS];
    
    int _numsteps;
    int _curstep;
    int _delayms;
    int _expDelay;
    Timer _elapseTimer;
};

#endif