RobotArm.h

00001 // Copyright (c) Microsoft. All rights reserved.
00002 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
00003 
00004 #ifndef __ROBOT_ARM_H__
00005 #define __ROBOT_ARM_H__
00006 
00007 #include "mbed.h"
00008 #include "rtos.h"
00009 
00010 #include "DynamixelBus.h"
00011 #include "NodeAX12.h"
00012 #include "RobotArmCfg.h"
00013 
00014 
00015 class RobotArm
00016 {
00017 public:
00018     
00019     RobotArm();
00020     
00021     // move all parts to specified postions in ms time - start
00022     bool MoveArmPositionsStart(float positions[], int ms);
00023     
00024     // move all parts to specified postions - resume after pause
00025     bool MoveArmPositionsResume();
00026 
00027     // move all parts to specified postions - test if done
00028     bool MoveArmPositionsHasNext();
00029     
00030     // move all parts to specified postions - next step
00031     bool MoveArmPositionsNext();
00032 
00033     // move all parts to specified postions - next step delay
00034     bool MoveArmPositionsDelay(int& nextdelay);
00035 
00036     // move all parts to specified postions - finish
00037     bool MoveArmPositionsEnd();
00038     
00039     // reset goal to the current position ie.  stop trying to move
00040     void MoveArmPositionsStop();
00041     
00042     // clear part error state
00043     void ClearErrorState();
00044     
00045     // get all parts positions
00046     bool GetArmPositions(float outVals[]);
00047     
00048     // get all parts last positions
00049     bool GetArmLastPositions(float outVals[]);
00050     
00051     // prepare to test arm state
00052     void ArmMeasuresTestStart();
00053     
00054     // test if measurements are in expected range
00055     int ArmMeasuresTest(int measureId);
00056     
00057     // get all parts for a measurement
00058     bool GetArmMeasure(int measureId, float outVals[]);
00059     
00060     // get all parts last measurement
00061     bool GetArmLastMeasure(int measureId, float outVals[]);
00062    
00063     int GetNumParts();
00064     
00065     // set arm speed as ms between steps
00066     void SetStepMs(int stepms);
00067     
00068     // set ThreadId for signals
00069     void SetThreadId(osThreadId tid);
00070     
00071     // get the part object
00072     RobotNode* GetArmPart(int partIx);
00073     
00074     // get last error code from action
00075     int GetLastError();
00076     
00077     // get index of part with error
00078     int GetLastErrorPart();
00079     
00080     // get size of position diff (valid if error)
00081     float GetLastPosDiff();
00082     
00083 private:
00084     // sensors and actuators
00085     RobotNode* _armParts[NUMJOINTS];
00086     
00087     int _numParts;
00088     
00089     // #ms between steps
00090     int _stepms;
00091     
00092     // thread id
00093     osThreadId _tid;
00094     
00095     // part ix for last error
00096     int _lastErrorPart;
00097     // last HW error
00098     int _lastError;
00099     // last position error
00100     float _lastPosDiff;
00101     
00102     // step-wise position moves
00103     float _endgoals[NUMJOINTS];
00104     float _differentials[NUMJOINTS];
00105     float _lastpos[NUMJOINTS];
00106     float _lastgoals[NUMJOINTS];
00107     
00108     // keep track of time period when position is off
00109     int _failms[NUMJOINTS];
00110     
00111     int _numsteps;
00112     int _curstep;
00113     int _delayms;
00114     int _expDelay;
00115     Timer _elapseTimer;
00116 };
00117 
00118 #endif