demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
RobotArm.h@7:6723f6887d00, 2015-12-29 (annotated)
- Committer:
- henryrawas
- Date:
- Tue Dec 29 23:31:28 2015 +0000
- Revision:
- 7:6723f6887d00
- Parent:
- 5:36916b1c5a06
- Child:
- 8:d98e2dec0f40
motion block alerts, more commands
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
henryrawas | 4:36a4eceb1b7f | 1 | /* |
henryrawas | 4:36a4eceb1b7f | 2 | Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license. |
henryrawas | 4:36a4eceb1b7f | 3 | */ |
henryrawas | 4:36a4eceb1b7f | 4 | |
henryrawas | 4:36a4eceb1b7f | 5 | #ifndef __ROBOT_ARM_H__ |
henryrawas | 4:36a4eceb1b7f | 6 | #define __ROBOT_ARM_H__ |
henryrawas | 4:36a4eceb1b7f | 7 | |
henryrawas | 4:36a4eceb1b7f | 8 | #include "mbed.h" |
henryrawas | 4:36a4eceb1b7f | 9 | #include "rtos.h" |
henryrawas | 4:36a4eceb1b7f | 10 | |
henryrawas | 4:36a4eceb1b7f | 11 | #include "DynamixelBus.h" |
henryrawas | 4:36a4eceb1b7f | 12 | #include "RobotNode.h" |
henryrawas | 4:36a4eceb1b7f | 13 | |
henryrawas | 4:36a4eceb1b7f | 14 | #define MAX_PARTS 8 |
henryrawas | 4:36a4eceb1b7f | 15 | |
henryrawas | 4:36a4eceb1b7f | 16 | |
henryrawas | 4:36a4eceb1b7f | 17 | enum ArmAction |
henryrawas | 4:36a4eceb1b7f | 18 | { |
henryrawas | 4:36a4eceb1b7f | 19 | AA_RunSeq = 0x1, |
henryrawas | 4:36a4eceb1b7f | 20 | AA_Status = 0x2, |
henryrawas | 4:36a4eceb1b7f | 21 | AA_Alert = 0x3 |
henryrawas | 4:36a4eceb1b7f | 22 | }; |
henryrawas | 4:36a4eceb1b7f | 23 | |
henryrawas | 4:36a4eceb1b7f | 24 | enum ArmThreadSignals |
henryrawas | 4:36a4eceb1b7f | 25 | { |
henryrawas | 4:36a4eceb1b7f | 26 | AS_Action = 0x1, |
henryrawas | 4:36a4eceb1b7f | 27 | AS_LocalAlert = 0x2, |
henryrawas | 4:36a4eceb1b7f | 28 | AS_Cancel = 0x4, |
henryrawas | 4:36a4eceb1b7f | 29 | AS_NextStep = 0x8, |
henryrawas | 4:36a4eceb1b7f | 30 | AS_NextSeq = 0x10 |
henryrawas | 4:36a4eceb1b7f | 31 | }; |
henryrawas | 4:36a4eceb1b7f | 32 | |
henryrawas | 4:36a4eceb1b7f | 33 | enum IothubThreadSignals |
henryrawas | 4:36a4eceb1b7f | 34 | { |
henryrawas | 4:36a4eceb1b7f | 35 | IS_Close = 0x1, |
henryrawas | 4:36a4eceb1b7f | 36 | IS_SendStatus = 0x2 |
henryrawas | 4:36a4eceb1b7f | 37 | }; |
henryrawas | 4:36a4eceb1b7f | 38 | |
henryrawas | 7:6723f6887d00 | 39 | |
henryrawas | 4:36a4eceb1b7f | 40 | class RobotArm |
henryrawas | 4:36a4eceb1b7f | 41 | { |
henryrawas | 4:36a4eceb1b7f | 42 | public: |
henryrawas | 4:36a4eceb1b7f | 43 | |
henryrawas | 4:36a4eceb1b7f | 44 | RobotArm(); |
henryrawas | 4:36a4eceb1b7f | 45 | |
henryrawas | 4:36a4eceb1b7f | 46 | // start - move all parts to specified postions in ms time |
henryrawas | 4:36a4eceb1b7f | 47 | bool MoveArmPositionsStart(vector<float> positions, int ms); |
henryrawas | 4:36a4eceb1b7f | 48 | |
henryrawas | 4:36a4eceb1b7f | 49 | // start - move all parts to specified postions - test if done |
henryrawas | 4:36a4eceb1b7f | 50 | bool MoveArmPositionsHasNext(); |
henryrawas | 4:36a4eceb1b7f | 51 | |
henryrawas | 4:36a4eceb1b7f | 52 | // start - move all parts to specified postions - next step |
henryrawas | 7:6723f6887d00 | 53 | bool MoveArmPositionsNext(); |
henryrawas | 7:6723f6887d00 | 54 | |
henryrawas | 7:6723f6887d00 | 55 | // start - move all parts to specified postions - next delay |
henryrawas | 7:6723f6887d00 | 56 | bool MoveArmPositionsDelay(int& nextdelay); |
henryrawas | 4:36a4eceb1b7f | 57 | |
henryrawas | 4:36a4eceb1b7f | 58 | // start - move all parts to specified postions - finish |
henryrawas | 4:36a4eceb1b7f | 59 | bool MoveArmPositionsEnd(); |
henryrawas | 5:36916b1c5a06 | 60 | |
henryrawas | 5:36916b1c5a06 | 61 | // start - test if positions are close to expected |
henryrawas | 7:6723f6887d00 | 62 | bool MoveArmPositionTest(); |
henryrawas | 4:36a4eceb1b7f | 63 | |
henryrawas | 4:36a4eceb1b7f | 64 | // get all parts positions |
henryrawas | 4:36a4eceb1b7f | 65 | bool GetArmPositions(vector<float>& outPos); |
henryrawas | 4:36a4eceb1b7f | 66 | |
henryrawas | 4:36a4eceb1b7f | 67 | // get all parts last positions |
henryrawas | 4:36a4eceb1b7f | 68 | bool GetArmLastPositions(vector<float>& outPos); |
henryrawas | 4:36a4eceb1b7f | 69 | |
henryrawas | 4:36a4eceb1b7f | 70 | // get all parts for a measurement |
henryrawas | 4:36a4eceb1b7f | 71 | bool GetArmMeasure(int measureId, vector<float>& outPos); |
henryrawas | 4:36a4eceb1b7f | 72 | |
henryrawas | 4:36a4eceb1b7f | 73 | // get all parts last measurement |
henryrawas | 4:36a4eceb1b7f | 74 | bool GetArmLastMeasure(int measureId, vector<float>& outPos); |
henryrawas | 4:36a4eceb1b7f | 75 | |
henryrawas | 4:36a4eceb1b7f | 76 | int GetNumParts(); |
henryrawas | 4:36a4eceb1b7f | 77 | |
henryrawas | 4:36a4eceb1b7f | 78 | // set arm speed as ms between steps |
henryrawas | 4:36a4eceb1b7f | 79 | void SetStepMs(int stepms); |
henryrawas | 4:36a4eceb1b7f | 80 | |
henryrawas | 4:36a4eceb1b7f | 81 | // set ThreadId for signals |
henryrawas | 4:36a4eceb1b7f | 82 | void SetThreadId(osThreadId tid); |
henryrawas | 4:36a4eceb1b7f | 83 | |
henryrawas | 4:36a4eceb1b7f | 84 | // get the part object |
henryrawas | 4:36a4eceb1b7f | 85 | RobotNode* GetArmPart(int partIx); |
henryrawas | 4:36a4eceb1b7f | 86 | |
henryrawas | 4:36a4eceb1b7f | 87 | // get last error code from action |
henryrawas | 7:6723f6887d00 | 88 | int GetLastError(); |
henryrawas | 4:36a4eceb1b7f | 89 | |
henryrawas | 4:36a4eceb1b7f | 90 | // get index of part with error |
henryrawas | 4:36a4eceb1b7f | 91 | int GetLastErrorPart(); |
henryrawas | 4:36a4eceb1b7f | 92 | |
henryrawas | 7:6723f6887d00 | 93 | // get size of position diff (valid if error) |
henryrawas | 7:6723f6887d00 | 94 | float GetLastPosDiff(); |
henryrawas | 7:6723f6887d00 | 95 | |
henryrawas | 7:6723f6887d00 | 96 | // set allwable position diff |
henryrawas | 7:6723f6887d00 | 97 | void SetAllowance(float allow); |
henryrawas | 7:6723f6887d00 | 98 | |
henryrawas | 4:36a4eceb1b7f | 99 | private: |
henryrawas | 4:36a4eceb1b7f | 100 | // sensors and actuators |
henryrawas | 4:36a4eceb1b7f | 101 | RobotNode* _armParts[MAX_PARTS]; |
henryrawas | 4:36a4eceb1b7f | 102 | |
henryrawas | 4:36a4eceb1b7f | 103 | int _numParts; |
henryrawas | 4:36a4eceb1b7f | 104 | |
henryrawas | 4:36a4eceb1b7f | 105 | // #ms between steps |
henryrawas | 4:36a4eceb1b7f | 106 | int _stepms; |
henryrawas | 4:36a4eceb1b7f | 107 | |
henryrawas | 4:36a4eceb1b7f | 108 | // thread id |
henryrawas | 4:36a4eceb1b7f | 109 | osThreadId _tid; |
henryrawas | 4:36a4eceb1b7f | 110 | |
henryrawas | 4:36a4eceb1b7f | 111 | // part ix for last error |
henryrawas | 4:36a4eceb1b7f | 112 | int _lastErrorPart; |
henryrawas | 7:6723f6887d00 | 113 | // last HW error |
henryrawas | 7:6723f6887d00 | 114 | int _lastError; |
henryrawas | 7:6723f6887d00 | 115 | // last position error |
henryrawas | 7:6723f6887d00 | 116 | float _lastPosDiff; |
henryrawas | 4:36a4eceb1b7f | 117 | |
henryrawas | 4:36a4eceb1b7f | 118 | // for thread friendly moves |
henryrawas | 4:36a4eceb1b7f | 119 | vector<float> lastpos; |
henryrawas | 4:36a4eceb1b7f | 120 | vector<float> differentials; |
henryrawas | 5:36916b1c5a06 | 121 | vector<float> lastgoals; |
henryrawas | 5:36916b1c5a06 | 122 | |
henryrawas | 5:36916b1c5a06 | 123 | // allowance for difference between expected pos and actual pos |
henryrawas | 7:6723f6887d00 | 124 | float allowance; |
henryrawas | 7:6723f6887d00 | 125 | // keep track of time period when position is off |
henryrawas | 7:6723f6887d00 | 126 | int failms; |
henryrawas | 5:36916b1c5a06 | 127 | |
henryrawas | 4:36a4eceb1b7f | 128 | int numsteps; |
henryrawas | 4:36a4eceb1b7f | 129 | int curstep; |
henryrawas | 4:36a4eceb1b7f | 130 | int delayms; |
henryrawas | 4:36a4eceb1b7f | 131 | int expDelay; |
henryrawas | 4:36a4eceb1b7f | 132 | Timer elapseTimer; |
henryrawas | 4:36a4eceb1b7f | 133 | }; |
henryrawas | 4:36a4eceb1b7f | 134 | |
henryrawas | 4:36a4eceb1b7f | 135 | #endif |