This repository has been archived by the owner on Nov 16, 2022. It is now read-only.
forked from reedhedges/AriaCoda
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ArP2Arm.h
297 lines (234 loc) · 9.87 KB
/
ArP2Arm.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
/*
Adept MobileRobots Robotics Interface for Applications (ARIA)
Copyright (C) 2004-2005 ActivMedia Robotics LLC
Copyright (C) 2006-2010 MobileRobots Inc.
Copyright (C) 2011-2015 Adept Technology, Inc.
Copyright (C) 2016-2018 Omron Adept Technologies, Inc.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ARP2ARM_H
#define ARP2ARM_H
#include "ArRobot.h"
#include "ariaTypedefs.h"
#include "ArSerialConnection.h"
#include "ariaOSDef.h"
// P2 Arm classes: ArP2Arm for control and ArP2ArmJoints for P2 Arm joint data
/// P2 Arm joint info
class P2ArmJoint
{
public:
AREXPORT P2ArmJoint();
AREXPORT virtual ~P2ArmJoint();
uint8_t myPos;
uint8_t myVel;
uint8_t myHome;
uint8_t myMin;
uint8_t myCenter;
uint8_t myMax;
uint8_t myTicksPer90;
};
/**
ArP2Arm is the interface to the AROS/P2OS-based Pioneer 2 Arm servers,
by means of which the robot microcontroller firmware can control the
original 5-DOF Pioneer 2 Arm manipulator.
The P2 Arm is attached to the robot's microcontroller via an auxiliary
serial port.
To use ArmP2, you must first set up an ArRobot and have it connect
with the robot. The ArRobot needs to be run so that it reads and writes
packets to and from server. The easiest way is ArRobot::runAsync()
which runs the ArRobot in its own thread.
Then call ArP2Arm::setRobot() with ArRobot, and finally initialized
with ArmP2::init(). Once initialized, use the various ArP2Arm
methods to power the P2 Arm servos, move joints, and so on.
For simple examples on how to use ArP2Arm, look in the Aria/examples
directory for P2ArmSimple.cpp and P2ArmJoydrive.cpp.
For additional information about the original 5-DOF Pioneer 2 Arm,
see the robot operations manual and the arm manual, available at
<a href="http://robots.mobilerobots.com">http://robots.mobilerobots.com</a>.
@ingroup DeviceClasses
@ingroup OptionalClasses
**/
/// Arm Control class
class ArP2Arm
{
public:
/// General error conditions possible from most of the arm related functions
typedef enum {
SUCCESS, ///< Succeded
ALREADY_INITED, ///< The class is already initialized
NOT_INITED, ///< The class is not initialized
ROBOT_NOT_SETUP, ///< The ArRobot class is not setup properly
NO_ARM_FOUND, ///< The arm can not be found
COMM_FAILED, ///< Communications has failed
COULD_NOT_OPEN_PORT, ///< Could not open the communications port
COULD_NOT_SET_UP_PORT, ///< Could not setup the communications port
ALREADY_CONNECTED, ///< Already connected to the arm
NOT_CONNECTED, ///< Not connected with the arm, connect first
INVALID_JOINT, ///< Invalid joint specified
INVALID_POSITION ///< Invalid position specified
} State;
/// Type of arm packet identifiers. Used in ArP2Arm::setPacketCB().
typedef enum {
StatusPacket, ///< The status packet type
InfoPacket ///< The info packet type
} PacketType;
/// Type of status packets to request for. Used in ArP2Arm::requestStatus()
typedef enum
{
StatusOff=0, ///< Stop sending status packets
StatusSingle=1, ///< Send a single status packets
StatusContinuous=2 ///< Send continous packets. Once every 100ms.
} StatusType;
/// Bit for joint 1 in arm status byte
AREXPORT static const int ArmJoint1;
/// Bit for joint 2 in arm status byte
AREXPORT static const int ArmJoint2;
/// Bit for joint 3 in arm status byte
AREXPORT static const int ArmJoint3;
/// Bit for joint 4 in arm status byte
AREXPORT static const int ArmJoint4;
/// Bit for joint 5 in arm status byte
AREXPORT static const int ArmJoint5;
/// Bit for joint 6 in arm status byte
AREXPORT static const int ArmJoint6;
/// Bit for arm good state in arm status byte
AREXPORT static const int ArmGood;
/// Bit for arm initialized in arm status byte
AREXPORT static const int ArmInited;
/// Bit for arm powered on in arm status byte
AREXPORT static const int ArmPower;
/// Bit for arm homing in arm status byte
AREXPORT static const int ArmHoming;
/// Number of joints that the arm has
AREXPORT static int NumJoints;
/// Constructor
AREXPORT ArP2Arm();
/// Destructor
AREXPORT virtual ~ArP2Arm();
/// Set the robot to use to talk to the arm
AREXPORT void setRobot(ArRobot *robot) {myRobot=robot;}
/// Init the arm class
AREXPORT virtual State init();
/// Uninit the arm class
AREXPORT virtual State uninit();
/// Power on the arm
AREXPORT virtual State powerOn(bool doWait=true);
/// Power off the arm
AREXPORT virtual State powerOff();
/// Request the arm info packet
AREXPORT virtual State requestInfo();
/// Request the arm status packet
AREXPORT virtual State requestStatus(StatusType status);
/// Request arm initialization
AREXPORT virtual State requestInit();
/// Check to see if the arm is still connected
AREXPORT virtual State checkArm(bool waitForResponse=true);
/// Home the arm
AREXPORT virtual State home(int joint=-1);
/// Home the arm and power if off
AREXPORT virtual State park();
/// Move a joint to a position in degrees
AREXPORT virtual State moveTo(int joint, float pos, unsigned char vel=0);
/// Move a joint to a position in low level arm controller ticks
AREXPORT virtual State moveToTicks(int joint, unsigned char pos);
/// Move a joint step degrees
AREXPORT virtual State moveStep(int joint, float pos, unsigned char vel=0);
/// Move a joint step ticks
AREXPORT virtual State moveStepTicks(int joint, signed char pos);
/// Set the joint to move at the given velocity
AREXPORT virtual State moveVel(int joint, int vel);
/// Stop the arm
AREXPORT virtual State stop();
/// Set the auto park timer value
AREXPORT virtual State setAutoParkTimer(int waitSecs);
/// Set the gripper park timer value
AREXPORT virtual State setGripperParkTimer(int waitSecs);
/// Set the arm stopped callback
AREXPORT virtual void setStoppedCB(ArFunctor *func) {myStoppedCB=func;}
/// set the arm packet callback
AREXPORT virtual void setPacketCB(ArFunctor1<PacketType> *func)
{myPacketCB=func;}
/// Get the arm version
AREXPORT virtual std::string getArmVersion() {return(myVersion);}
/// Get the joints position in degrees
AREXPORT virtual float getJointPos(int joint);
/// Get the joints position in ticks
AREXPORT virtual unsigned char getJointPosTicks(int joint);
/// Check to see if the arm is moving
AREXPORT virtual bool getMoving(int joint=-1);
/// Check to see if the arm is powered
AREXPORT virtual bool isPowered();
/// Check to see if the arm is communicating
AREXPORT virtual bool isGood();
/// Get the two byts of status info from P2OS
AREXPORT virtual int getStatus() {return(myStatus);}
/// Get when the last arm status packet came in
AREXPORT virtual ArTime getLastStatusTime() {return(myLastStatusTime);}
/// Get the robot that the arm is on
AREXPORT virtual ArRobot * getRobot() {return(myRobot);}
/// Get the joints data structure
AREXPORT virtual P2ArmJoint * getJoint(int joint);
/// Converts degrees to low level arm controller ticks
AREXPORT virtual bool convertDegToTicks(int joint, float pos,
unsigned char *ticks);
/// Converts low level arm controller ticks to degrees
AREXPORT virtual bool convertTicksToDeg(int joint, unsigned char pos,
float *degrees);
protected:
// AROS/P2OS parameters
static const unsigned int ARMpac;
static const unsigned int ARMINFOpac;
static const unsigned char ComArmInfo;
static const unsigned char ComArmStats;
static const unsigned char ComArmInit;
static const unsigned char ComArmCheckArm;
static const unsigned char ComArmPower;
static const unsigned char ComArmHome;
static const unsigned char ComArmPark;
static const unsigned char ComArmPos;
static const unsigned char ComArmSpeed;
static const unsigned char ComArmStop;
static const unsigned char ComArmAutoPark;
static const unsigned char ComArmGripperPark;
bool comArmInfo();
bool comArmStats(StatusType stats=StatusSingle);
bool comArmInit();
bool comArmCheckArm();
bool comArmPower(bool on);
bool comArmHome(unsigned char joint=0xff);
bool comArmPark();
bool comArmPos(unsigned char joint, unsigned char pos);
bool comArmSpeed(unsigned char joint, unsigned char speed);
bool comArmStop(unsigned char joint=0xff);
bool comArmAutoPark(int waitSecs);
bool comArmGripperPark(int waitSecs);
bool armPacketHandler(ArRobotPacket *packet);
bool myInited;
ArRobot *myRobot;
// ArmP2Model myModel;
ArTime myLastStatusTime;
ArTime myLastInfoTime;
std::string myVersion;
StatusType myStatusRequest;
uint16_t myLastStatus;
uint16_t myStatus;
ArSerialConnection myCon;
ArRetFunctorC<State, ArP2Arm> myAriaUninitCB;
ArRetFunctor1C<bool, ArP2Arm, ArRobotPacket*> myArmPacketHandler;
ArFunctor1<PacketType> *myPacketCB;
ArFunctor *myStoppedCB;
// We have 6 joints. Including the gripper. It's here so that we can
// store its position even though its not really a joint.
P2ArmJoint myJoints[6];
};
#endif // _ARP2ARM_H