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@13:ffeff9b5e513, 2016-01-15 (annotated)

Committer:
henryrawas
Date:
Fri Jan 15 22:02:46 2016 +0000
Revision:
13:ffeff9b5e513
Parent:
12:ac6c9d7f8c40
Child:
15:4bd10f531cdc
Always test status and send data

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 13:ffeff9b5e513 14 // define number of joints for this arm
henryrawas 13:ffeff9b5e513 15 #define NUMJOINTS 5
henryrawas 4:36a4eceb1b7f 16
henryrawas 4:36a4eceb1b7f 17
henryrawas 4:36a4eceb1b7f 18 enum ArmAction
henryrawas 4:36a4eceb1b7f 19 {
henryrawas 4:36a4eceb1b7f 20 AA_RunSeq = 0x1,
henryrawas 4:36a4eceb1b7f 21 AA_Status = 0x2,
henryrawas 4:36a4eceb1b7f 22 AA_Alert = 0x3
henryrawas 4:36a4eceb1b7f 23 };
henryrawas 4:36a4eceb1b7f 24
henryrawas 4:36a4eceb1b7f 25 enum ArmThreadSignals
henryrawas 4:36a4eceb1b7f 26 {
henryrawas 4:36a4eceb1b7f 27 AS_Action = 0x1,
henryrawas 4:36a4eceb1b7f 28 AS_LocalAlert = 0x2,
henryrawas 4:36a4eceb1b7f 29 AS_Cancel = 0x4,
henryrawas 4:36a4eceb1b7f 30 AS_NextStep = 0x8,
henryrawas 4:36a4eceb1b7f 31 AS_NextSeq = 0x10
henryrawas 4:36a4eceb1b7f 32 };
henryrawas 4:36a4eceb1b7f 33
henryrawas 4:36a4eceb1b7f 34 enum IothubThreadSignals
henryrawas 4:36a4eceb1b7f 35 {
henryrawas 4:36a4eceb1b7f 36 IS_Close = 0x1,
henryrawas 4:36a4eceb1b7f 37 IS_SendStatus = 0x2
henryrawas 4:36a4eceb1b7f 38 };
henryrawas 4:36a4eceb1b7f 39
henryrawas 7:6723f6887d00 40
henryrawas 4:36a4eceb1b7f 41 class RobotArm
henryrawas 4:36a4eceb1b7f 42 {
henryrawas 4:36a4eceb1b7f 43 public:
henryrawas 4:36a4eceb1b7f 44
henryrawas 4:36a4eceb1b7f 45 RobotArm();
henryrawas 4:36a4eceb1b7f 46
henryrawas 13:ffeff9b5e513 47 // move all parts to specified postions in ms time - start
henryrawas 13:ffeff9b5e513 48 bool MoveArmPositionsStart(float positions[], int ms);
henryrawas 13:ffeff9b5e513 49
henryrawas 13:ffeff9b5e513 50 // move all parts to specified postions - resume after pause
henryrawas 13:ffeff9b5e513 51 bool MoveArmPositionsResume();
henryrawas 4:36a4eceb1b7f 52
henryrawas 13:ffeff9b5e513 53 // move all parts to specified postions - test if done
henryrawas 4:36a4eceb1b7f 54 bool MoveArmPositionsHasNext();
henryrawas 4:36a4eceb1b7f 55
henryrawas 13:ffeff9b5e513 56 // move all parts to specified postions - next step
henryrawas 7:6723f6887d00 57 bool MoveArmPositionsNext();
henryrawas 7:6723f6887d00 58
henryrawas 13:ffeff9b5e513 59 // move all parts to specified postions - next step delay
henryrawas 7:6723f6887d00 60 bool MoveArmPositionsDelay(int& nextdelay);
henryrawas 4:36a4eceb1b7f 61
henryrawas 13:ffeff9b5e513 62 // move all parts to specified postions - finish
henryrawas 4:36a4eceb1b7f 63 bool MoveArmPositionsEnd();
henryrawas 5:36916b1c5a06 64
henryrawas 13:ffeff9b5e513 65 // reset goal to the current position ie. stop trying to move
henryrawas 13:ffeff9b5e513 66 void MoveArmPositionsStop();
henryrawas 4:36a4eceb1b7f 67
henryrawas 11:3a2e6eb9fbb8 68 // clear part error state
henryrawas 11:3a2e6eb9fbb8 69 void ClearErrorState();
henryrawas 11:3a2e6eb9fbb8 70
henryrawas 4:36a4eceb1b7f 71 // get all parts positions
henryrawas 13:ffeff9b5e513 72 bool GetArmPositions(float outVals[]);
henryrawas 4:36a4eceb1b7f 73
henryrawas 4:36a4eceb1b7f 74 // get all parts last positions
henryrawas 13:ffeff9b5e513 75 bool GetArmLastPositions(float outVals[]);
henryrawas 13:ffeff9b5e513 76
henryrawas 13:ffeff9b5e513 77 // test if measurements are in expected range
henryrawas 13:ffeff9b5e513 78 int ArmMeasuresTest(int measureId);
henryrawas 4:36a4eceb1b7f 79
henryrawas 4:36a4eceb1b7f 80 // get all parts for a measurement
henryrawas 13:ffeff9b5e513 81 bool GetArmMeasure(int measureId, float outVals[]);
henryrawas 4:36a4eceb1b7f 82
henryrawas 4:36a4eceb1b7f 83 // get all parts last measurement
henryrawas 13:ffeff9b5e513 84 bool GetArmLastMeasure(int measureId, float outVals[]);
henryrawas 4:36a4eceb1b7f 85
henryrawas 4:36a4eceb1b7f 86 int GetNumParts();
henryrawas 4:36a4eceb1b7f 87
henryrawas 4:36a4eceb1b7f 88 // set arm speed as ms between steps
henryrawas 4:36a4eceb1b7f 89 void SetStepMs(int stepms);
henryrawas 4:36a4eceb1b7f 90
henryrawas 4:36a4eceb1b7f 91 // set ThreadId for signals
henryrawas 4:36a4eceb1b7f 92 void SetThreadId(osThreadId tid);
henryrawas 4:36a4eceb1b7f 93
henryrawas 4:36a4eceb1b7f 94 // get the part object
henryrawas 4:36a4eceb1b7f 95 RobotNode* GetArmPart(int partIx);
henryrawas 4:36a4eceb1b7f 96
henryrawas 4:36a4eceb1b7f 97 // get last error code from action
henryrawas 7:6723f6887d00 98 int GetLastError();
henryrawas 4:36a4eceb1b7f 99
henryrawas 4:36a4eceb1b7f 100 // get index of part with error
henryrawas 4:36a4eceb1b7f 101 int GetLastErrorPart();
henryrawas 4:36a4eceb1b7f 102
henryrawas 7:6723f6887d00 103 // get size of position diff (valid if error)
henryrawas 7:6723f6887d00 104 float GetLastPosDiff();
henryrawas 7:6723f6887d00 105
henryrawas 4:36a4eceb1b7f 106 private:
henryrawas 4:36a4eceb1b7f 107 // sensors and actuators
henryrawas 13:ffeff9b5e513 108 RobotNode* _armParts[NUMJOINTS];
henryrawas 4:36a4eceb1b7f 109
henryrawas 4:36a4eceb1b7f 110 int _numParts;
henryrawas 4:36a4eceb1b7f 111
henryrawas 4:36a4eceb1b7f 112 // #ms between steps
henryrawas 4:36a4eceb1b7f 113 int _stepms;
henryrawas 4:36a4eceb1b7f 114
henryrawas 4:36a4eceb1b7f 115 // thread id
henryrawas 4:36a4eceb1b7f 116 osThreadId _tid;
henryrawas 4:36a4eceb1b7f 117
henryrawas 4:36a4eceb1b7f 118 // part ix for last error
henryrawas 4:36a4eceb1b7f 119 int _lastErrorPart;
henryrawas 7:6723f6887d00 120 // last HW error
henryrawas 7:6723f6887d00 121 int _lastError;
henryrawas 7:6723f6887d00 122 // last position error
henryrawas 7:6723f6887d00 123 float _lastPosDiff;
henryrawas 4:36a4eceb1b7f 124
henryrawas 8:d98e2dec0f40 125 // step-wise position moves
henryrawas 13:ffeff9b5e513 126 float endgoals[NUMJOINTS];
henryrawas 13:ffeff9b5e513 127 float differentials[NUMJOINTS];
henryrawas 13:ffeff9b5e513 128 float lastpos[NUMJOINTS];
henryrawas 13:ffeff9b5e513 129 float lastgoals[NUMJOINTS];
henryrawas 5:36916b1c5a06 130
henryrawas 5:36916b1c5a06 131 // allowance for difference between expected pos and actual pos
henryrawas 7:6723f6887d00 132 float allowance;
henryrawas 7:6723f6887d00 133 // keep track of time period when position is off
henryrawas 13:ffeff9b5e513 134 int failms[NUMJOINTS];
henryrawas 5:36916b1c5a06 135
henryrawas 4:36a4eceb1b7f 136 int numsteps;
henryrawas 4:36a4eceb1b7f 137 int curstep;
henryrawas 4:36a4eceb1b7f 138 int delayms;
henryrawas 4:36a4eceb1b7f 139 int expDelay;
henryrawas 4:36a4eceb1b7f 140 Timer elapseTimer;
henryrawas 4:36a4eceb1b7f 141 };
henryrawas 4:36a4eceb1b7f 142
henryrawas 4:36a4eceb1b7f 143 #endif