robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

RobotArm.h@12:ac6c9d7f8c40, 2016-01-07 (annotated)

Committer:
henryrawas
Date:
Thu Jan 07 17:31:23 2016 +0000
Revision:
12:ac6c9d7f8c40
Parent:
11:3a2e6eb9fbb8
Child:
13:ffeff9b5e513
refactor robotnode

Who changed what in which revision?

UserRevisionLine numberNew 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 12:ac6c9d7f8c40 12 #include "NodeAX12.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 11:3a2e6eb9fbb8 64 // clear part error state
henryrawas 11:3a2e6eb9fbb8 65 void ClearErrorState();
henryrawas 11:3a2e6eb9fbb8 66
henryrawas 4:36a4eceb1b7f 67 // get all parts positions
henryrawas 4:36a4eceb1b7f 68 bool GetArmPositions(vector<float>& outPos);
henryrawas 4:36a4eceb1b7f 69
henryrawas 4:36a4eceb1b7f 70 // get all parts last positions
henryrawas 4:36a4eceb1b7f 71 bool GetArmLastPositions(vector<float>& outPos);
henryrawas 4:36a4eceb1b7f 72
henryrawas 4:36a4eceb1b7f 73 // get all parts for a measurement
henryrawas 4:36a4eceb1b7f 74 bool GetArmMeasure(int measureId, vector<float>& outPos);
henryrawas 4:36a4eceb1b7f 75
henryrawas 4:36a4eceb1b7f 76 // get all parts last measurement
henryrawas 4:36a4eceb1b7f 77 bool GetArmLastMeasure(int measureId, vector<float>& outPos);
henryrawas 4:36a4eceb1b7f 78
henryrawas 4:36a4eceb1b7f 79 int GetNumParts();
henryrawas 4:36a4eceb1b7f 80
henryrawas 4:36a4eceb1b7f 81 // set arm speed as ms between steps
henryrawas 4:36a4eceb1b7f 82 void SetStepMs(int stepms);
henryrawas 4:36a4eceb1b7f 83
henryrawas 4:36a4eceb1b7f 84 // set ThreadId for signals
henryrawas 4:36a4eceb1b7f 85 void SetThreadId(osThreadId tid);
henryrawas 4:36a4eceb1b7f 86
henryrawas 4:36a4eceb1b7f 87 // get the part object
henryrawas 4:36a4eceb1b7f 88 RobotNode* GetArmPart(int partIx);
henryrawas 4:36a4eceb1b7f 89
henryrawas 4:36a4eceb1b7f 90 // get last error code from action
henryrawas 7:6723f6887d00 91 int GetLastError();
henryrawas 4:36a4eceb1b7f 92
henryrawas 4:36a4eceb1b7f 93 // get index of part with error
henryrawas 4:36a4eceb1b7f 94 int GetLastErrorPart();
henryrawas 4:36a4eceb1b7f 95
henryrawas 7:6723f6887d00 96 // get size of position diff (valid if error)
henryrawas 7:6723f6887d00 97 float GetLastPosDiff();
henryrawas 7:6723f6887d00 98
henryrawas 7:6723f6887d00 99 // set allwable position diff
henryrawas 7:6723f6887d00 100 void SetAllowance(float allow);
henryrawas 7:6723f6887d00 101
henryrawas 4:36a4eceb1b7f 102 private:
henryrawas 4:36a4eceb1b7f 103 // sensors and actuators
henryrawas 4:36a4eceb1b7f 104 RobotNode* _armParts[MAX_PARTS];
henryrawas 4:36a4eceb1b7f 105
henryrawas 4:36a4eceb1b7f 106 int _numParts;
henryrawas 4:36a4eceb1b7f 107
henryrawas 4:36a4eceb1b7f 108 // #ms between steps
henryrawas 4:36a4eceb1b7f 109 int _stepms;
henryrawas 4:36a4eceb1b7f 110
henryrawas 4:36a4eceb1b7f 111 // thread id
henryrawas 4:36a4eceb1b7f 112 osThreadId _tid;
henryrawas 4:36a4eceb1b7f 113
henryrawas 4:36a4eceb1b7f 114 // part ix for last error
henryrawas 4:36a4eceb1b7f 115 int _lastErrorPart;
henryrawas 7:6723f6887d00 116 // last HW error
henryrawas 7:6723f6887d00 117 int _lastError;
henryrawas 7:6723f6887d00 118 // last position error
henryrawas 7:6723f6887d00 119 float _lastPosDiff;
henryrawas 4:36a4eceb1b7f 120
henryrawas 8:d98e2dec0f40 121 // step-wise position moves
henryrawas 8:d98e2dec0f40 122 vector<float> endgoals;
henryrawas 8:d98e2dec0f40 123 vector<float> differentials;
henryrawas 4:36a4eceb1b7f 124 vector<float> lastpos;
henryrawas 5:36916b1c5a06 125 vector<float> lastgoals;
henryrawas 5:36916b1c5a06 126
henryrawas 5:36916b1c5a06 127 // allowance for difference between expected pos and actual pos
henryrawas 7:6723f6887d00 128 float allowance;
henryrawas 7:6723f6887d00 129 // keep track of time period when position is off
henryrawas 7:6723f6887d00 130 int failms;
henryrawas 5:36916b1c5a06 131
henryrawas 4:36a4eceb1b7f 132 int numsteps;
henryrawas 4:36a4eceb1b7f 133 int curstep;
henryrawas 4:36a4eceb1b7f 134 int delayms;
henryrawas 4:36a4eceb1b7f 135 int expDelay;
henryrawas 4:36a4eceb1b7f 136 Timer elapseTimer;
henryrawas 4:36a4eceb1b7f 137 };
henryrawas 4:36a4eceb1b7f 138
henryrawas 4:36a4eceb1b7f 139 #endif