File content as of revision 12:ac6c9d7f8c40:

/* 
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 MAX_PARTS   8


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();
    
    // start - move all parts to specified postions in ms time
    bool MoveArmPositionsStart(vector<float> positions, int ms);

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

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

    // start - move all parts to specified postions - finish
    bool MoveArmPositionsEnd();
    
    // start - test if positions are close to expected
    bool MoveArmPositionTest();
    
    // clear part error state
    void ClearErrorState();
    
    // get all parts positions
    bool GetArmPositions(vector<float>& outPos);
    
    // get all parts last positions
    bool GetArmLastPositions(vector<float>& outPos);
    
    // get all parts for a measurement
    bool GetArmMeasure(int measureId, vector<float>& outPos);
    
    // get all parts last measurement
    bool GetArmLastMeasure(int measureId, vector<float>& outPos);
   
    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();
    
    // set allwable position diff
    void SetAllowance(float allow);
    
private:
    // sensors and actuators
    RobotNode* _armParts[MAX_PARTS];
    
    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
    vector<float> endgoals;
    vector<float> differentials;
    vector<float> lastpos;
    vector<float> lastgoals;
    
    // allowance for difference between expected pos and actual pos
    float allowance;
    // keep track of time period when position is off
    int failms;
    
    int numsteps;
    int curstep;
    int delayms;
    int expDelay;
    Timer elapseTimer;
};

#endif