File content as of revision 5:36916b1c5a06:

/* 
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 "RobotNode.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();
    
    // move all parts to specified postions in ms time
    bool MoveArmPositions(vector<float> positions, int ms, int steps);

    // 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(int& nextdelay);

    // start - move all parts to specified postions - finish
    bool MoveArmPositionsEnd();
    
    // start - test if positions are close to expected
    bool TestArmPosition(int& partix, float& diffpos);
    
    // move one part to specified postion in ms time
    bool MovePartPosition(int partIx, float position, int ms, int steps);
    
    // get all parts positions
    bool GetArmPositions(vector<float>& outPos);
    
    // get all parts last positions
    bool GetArmLastPositions(vector<float>& outPos);
    
    // get one part position
    float GetPartPosition(int partIx);
    
    // 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);
    
    // get one part measurement
    float GetPartMeasure(int measureId, int partIx);
   
    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
    unsigned char GetLastError();
    
    // get index of part with error
    int GetLastErrorPart();
    
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;
    
    // for thread friendly moves
    vector<float> lastpos;
    vector<float> differentials;
    vector<float> lastgoals;
    
    // allowance for difference between expected pos and actual pos
    static const float allowance = 1.0f;
    
    int numsteps;
    int curstep;
    int delayms;
    int expDelay;
    Timer elapseTimer;
};

#endif