Diff: RobotArm.h

Revision:

4:36a4eceb1b7f

Child:

5:36916b1c5a06

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotArm.h	Wed Dec 23 18:34:06 2015 +0000
@@ -0,0 +1,123 @@
+/* 
+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();
+
+    // 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;
+    int numsteps;
+    int curstep;
+    int delayms;
+    int expDelay;
+    Timer elapseTimer;
+};
+
+#endif