File content as of revision 13:ffeff9b5e513:

/* 
Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
*/

#ifndef __ROBOT_ARM_H__
#define __ROBOT_ARM_H__

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

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

// define number of joints for this arm
#define NUMJOINTS   5


enum ArmAction
{
    AA_RunSeq           = 0x1,
    AA_Status           = 0x2,
    AA_Alert            = 0x3
};

enum ArmThreadSignals
{
    AS_Action           = 0x1,
    AS_LocalAlert       = 0x2,
    AS_Cancel           = 0x4,
    AS_NextStep         = 0x8,
    AS_NextSeq          = 0x10
};

enum IothubThreadSignals
{
    IS_Close            = 0x1,
    IS_SendStatus       = 0x2
};


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[]);
    
    // 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];
    
    // allowance for difference between expected pos and actual pos
    float allowance;
    // keep track of time period when position is off
    int failms[NUMJOINTS];
    
    int numsteps;
    int curstep;
    int delayms;
    int expDelay;
    Timer elapseTimer;
};

#endif