-
Notifications
You must be signed in to change notification settings - Fork 34
/
Copy pathHex_Cfg.h
342 lines (279 loc) · 13.1 KB
/
Hex_Cfg.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
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
//====================================================================
//Project Lynxmotion Phoenix
//Description:
// This is the hardware configuration file for the
// Version 2 PhantomX robot.
// Will first define to use their commander unit.
//
//Date: June 19, 2013
//Programmer: Kurt (aka KurtE)
//
//NEW IN V1.0
// - First Release - Changes from Trossen member KingPin
//
//====================================================================
#ifndef HEX_CFG_H
#define HEX_CFG_H
//==================================================================================================================================
// Define which input classes we will use. If we wish to use more than one we need to define USEMULTI - This will define a forwarder
// type implementation, that the Inputcontroller will need to call. There will be some negotion for which one is in contol.
//
// If this is not defined, The included Controller should simply implement the InputController Class...
//==================================================================================================================================
#define USECOMMANDER
// Global defines to control which configuration we are using. Note: Only define one of these...
//
// Which type of control(s) do you want to compile in
#if defined(KINETISK)
#define DBGSerial Serial
#else
#if defined(UBRR2H)
#define DBGSerial Serial
#endif
#endif
// Define other optional compnents to be included or not...
//#define PHANTOMX_V2 // Some code may depend on it being a V2 PhantomX
#define cFemurHornOffset1 -35
#define cTibiaHornOffset1 463
#define cRRTibiaInv 0
#define cRMTibiaInv 0
#define cRFTibiaInv 0
#define cLRTibiaInv 1
#define cLMTibiaInv 1
#define cLFTibiaInv 1
//===================================================================
// Debug Options
#ifdef DBGSerial
#define OPT_TERMINAL_MONITOR
#define OPT_TERMINAL_MONITOR_IC // Allow the input controller to define stuff as well
//#define OPT_FIND_SERVO_OFFSETS // Only useful if terminal monitor is enabled
//#define OPT_PYPOSE
#endif
//#define DEBUG_IOPINS
#ifdef DEBUG_IOPINS
#define DebugToggle(pin) {digitalWrite(pin, !digitalRead(pin));}
#define DebugWrite(pin, state) {digitalWrite(pin, state);}
#else
#define DebugToggle(pin) {;}
#define DebugWrite(pin, state) {;}
#endif
// Also define that we are using the AX12 driver
#define USE_AX12_DRIVER
#define OPT_BACKGROUND_PROCESS // The AX12 has a background process
#define OPT_CHECK_SERVO_RESET // Try to find single servo that reset it's ID...
#define OPT_GPPLAYER
#define OPT_SINGLELEG
//==================================================================================================================================
//==================================================================================================================================
//==================================================================================================================================
// PhantomX
//==================================================================================================================================
//[SERIAL CONNECTIONS]
//====================================================================
// XBEE on non mega???
#if defined(KINETISK)
#define XBeeSerial Serial2
#else
#if defined(UBRR2H)
#define XBeeSerial Serial2
#endif
#define XBeeSerial Serial
#endif
#define XBEE_BAUD 38400
//--------------------------------------------------------------------
//[Arbotix Pin Numbers]
#if defined(KINETISK)
#define SOUND_PIN 6
#else
#define SOUND_PIN 1 //0xff // Tell system we have no IO pin...
#define USER 0 // defaults to 13 but Arbotix on 0...
#endif
// Define Analog pin and minimum voltage that we will allow the servos to run
#if defined(KINETISK)
// Our Teensy board
#define cVoltagePin 0
#define CVADR1 402 // VD Resistor 1 - reduced as only need ratio... 40.2K and 10K
#define CVADR2 100 // VD Resistor 2
#define CVREF 330 // 3.3v
#endif
//#define cVoltagePin 7 // Use our Analog pin jumper here...
//#define CVADR1 1000 // VD Resistor 1 - reduced as only need ratio... 20K and 4.66K
//#define CVADR2 233 // VD Resistor 2
//#define cTurnOffVol 1000 // 10v
//#define cTurnOnVol 1100 // 11V - optional part to say if voltage goes back up, turn it back on...
//====================================================================
#define DEFAULT_GAIT_SPEED 35 // Default gait speed - Will depend on what Servos you are using...
#define DEFAULT_SLOW_GAIT 50 // Had a couple different speeds...
//====================================================================
// Defines for Optional XBee Init and configuration code.
//====================================================================
#define CHECK_AND_CONFIG_XBEE
#define DEFAULT_MY 0x101 // Swap My/DL on 2nd unit
#define DEFAULT_DL 0x102
#define DEFAULT_ID 0x3332
//--------------------------------------------------------------------
// Define which pins(sevo IDS go with which joint
#define cRRCoxaPin 8 //Rear Right leg Hip Horizontal
#define cRRFemurPin 10 //Rear Right leg Hip Vertical
#define cRRTibiaPin 12 //Rear Right leg Knee
#define cRMCoxaPin 14 //Middle Right leg Hip Horizontal
#define cRMFemurPin 16 //Middle Right leg Hip Vertical
#define cRMTibiaPin 18 //Middle Right leg Knee
#define cRFCoxaPin 2 //Front Right leg Hip Horizontal
#define cRFFemurPin 4 //Front Right leg Hip Vertical
#define cRFTibiaPin 6 //Front Right leg Knee
#define cLRCoxaPin 7 //Rear Left leg Hip Horizontal
#define cLRFemurPin 9 //Rear Left leg Hip Vertical
#define cLRTibiaPin 11 //Rear Left leg Knee
#define cLMCoxaPin 13 //Middle Left leg Hip Horizontal
#define cLMFemurPin 15 //Middle Left leg Hip Vertical
#define cLMTibiaPin 17 //Middle Left leg Knee
#ifdef OPT_CHECK_SERVO_RESET // Try to find single servo that reset it's ID...
#define cLFCoxaPin 19 //Front Left leg Hip Horizontal - Avoid servo 1
#else
#define cLFCoxaPin 1 //Front Left leg Hip Horizontal
#endif
#define cLFFemurPin 3 //Front Left leg Hip Vertical
#define cLFTibiaPin 5 //Front Left leg Knee
#define cTurretRotPin 20 // Turret Rotate Pin
#define cTurretTiltPin 21 // Turret Tilt pin
//--------------------------------------------------------------------
//[MIN/MAX ANGLES] - Start off assume same as Phoenix...
#define cRRCoxaMin1 -750
#define cRRCoxaMax1 750
#define cRRFemurMin1 -1000
#define cRRFemurMax1 1000
#define cRRTibiaMin1 -1020
#define cRRTibiaMax1 670
#define cRMCoxaMin1 -750 //Mechanical limits of the Right Middle Leg, decimals = 1
#define cRMCoxaMax1 750
#define cRMFemurMin1 -1000
#define cRMFemurMax1 1000
#define cRMTibiaMin1 -1020
#define cRMTibiaMax1 670
#define cRFCoxaMin1 -750 //Mechanical limits of the Right Front Leg, decimals = 1
#define cRFCoxaMax1 750
#define cRFFemurMin1 -1000
#define cRFFemurMax1 1000
#define cRFTibiaMin1 -1020
#define cRFTibiaMax1 670
#define cLRCoxaMin1 -750 //Mechanical limits of the Left Rear Leg, decimals = 1
#define cLRCoxaMax1 750
#define cLRFemurMin1 -1000
#define cLRFemurMax1 1000
#define cLRTibiaMin1 -1020
#define cLRTibiaMax1 670
#define cLMCoxaMin1 -1000 //Mechanical limits of the Left Middle Leg, decimals = 1
#define cLMCoxaMax1 1000
#define cLMFemurMin1 -1000
#define cLMFemurMax1 1000
#define cLMTibiaMin1 -1020
#define cLMTibiaMax1 670
#define cLFCoxaMin1 -750 //Mechanical limits of the Left Front Leg, decimals = 1
#define cLFCoxaMax1 750
#define cLFFemurMin1 -1000
#define cLFFemurMax1 1000
#define cLFTibiaMin1 -1020
#define cLFTibiaMax1 670
#define cTurretRotMin1 -1500
#define cTurretRotMax1 1500
#define cTurretTiltMin1 -125
#define cTurretTiltMax1 1100
//--------------------------------------------------------------------
//[Joint offsets]
// This allows us to calibrate servos to some fixed position, and then adjust them by moving theim
// one or more servo horn clicks. This requires us to adjust the value for those servos by 15 degrees
// per click. This is used with the T-Hex 4DOF legs
//First calibrate the servos in the 0 deg position using the SSC-32 reg offsets, then:
//--------------------------------------------------------------------
//[LEG DIMENSIONS]
//Universal dimensions for each leg in mm
#define cXXCoxaLength 52 // PhantomX leg dimensions.
#define cXXFemurLength 66 // MEASURE THIS!!! Guessed now :-)
#define cXXTibiaLength 133 // MEASURE THIS!!! Guessed now :-)
#define cRRCoxaLength cXXCoxaLength //Right Rear leg
#define cRRFemurLength cXXFemurLength
#define cRRTibiaLength cXXTibiaLength
#define cRMCoxaLength cXXCoxaLength //Right middle leg
#define cRMFemurLength cXXFemurLength
#define cRMTibiaLength cXXTibiaLength
#define cRFCoxaLength cXXCoxaLength //Rigth front leg
#define cRFFemurLength cXXFemurLength
#define cRFTibiaLength cXXTibiaLength
#define cLRCoxaLength cXXCoxaLength //Left Rear leg
#define cLRFemurLength cXXFemurLength
#define cLRTibiaLength cXXTibiaLength
#define cLMCoxaLength cXXCoxaLength //Left middle leg
#define cLMFemurLength cXXFemurLength
#define cLMTibiaLength cXXTibiaLength
#define cLFCoxaLength cXXCoxaLength //Left front leg
#define cLFFemurLength cXXFemurLength
#define cLFTibiaLength cXXTibiaLength
//--------------------------------------------------------------------
//[BODY DIMENSIONS]
#define cRRCoxaAngle1 -450 //Default Coxa setup angle, decimals = 1
#define cRMCoxaAngle1 0 //Default Coxa setup angle, decimals = 1
#define cRFCoxaAngle1 450 //Default Coxa setup angle, decimals = 1
#define cLRCoxaAngle1 -450 //Default Coxa setup angle, decimals = 1
#define cLMCoxaAngle1 0 //Default Coxa setup angle, decimals = 1
#define cLFCoxaAngle1 450 //Default Coxa setup angle, decimals = 1
#define X_COXA 60 // MM between front and back legs /2
#define Y_COXA 60 // MM between front/back legs /2
#define M_COXA 100 // MM between two middle legs /2
#define cRROffsetX -60 //Distance X from center of the body to the Right Rear coxa
#define cRROffsetZ 120 //Distance Z from center of the body to the Right Rear coxa
#define cRMOffsetX -100 //Distance X from center of the body to the Right Middle coxa
#define cRMOffsetZ 0 //Distance Z from center of the body to the Right Middle coxa
#define cRFOffsetX -60 //Distance X from center of the body to the Right Front coxa
#define cRFOffsetZ -120 //Distance Z from center of the body to the Right Front coxa
#define cLROffsetX 60 //Distance X from center of the body to the Left Rear coxa
#define cLROffsetZ 120 //Distance Z from center of the body to the Left Rear coxa
#define cLMOffsetX 100 //Distance X from center of the body to the Left Middle coxa
#define cLMOffsetZ 0 //Distance Z from center of the body to the Left Middle coxa
#define cLFOffsetX 60 //Distance X from center of the body to the Left Front coxa
#define cLFOffsetZ -120 //Distance Z from center of the body to the Left Front coxa
//--------------------------------------------------------------------
//[START POSITIONS FEET]
#define cHexInitXZ 147
#define CHexInitXZCos60 104 // COS(45) = .707
#define CHexInitXZSin60 104 // sin(45) = .707
#define CHexInitY 25 //30
// Lets try some multi leg positions depending on height settings.
#define CNT_HEX_INITS 2
#define MAX_BODY_Y 150
#ifdef DEFINE_HEX_GLOBALS
const byte g_abHexIntXZ[] PROGMEM = {cHexInitXZ, 144};
const byte g_abHexMaxBodyY[] PROGMEM = { 20, MAX_BODY_Y};
#else
extern const byte g_abHexIntXZ[] PROGMEM;
extern const byte g_abHexMaxBodyY[] PROGMEM;
#endif
#define cRRInitPosX CHexInitXZCos60 //Start positions of the Right Rear leg
#define cRRInitPosY CHexInitY
#define cRRInitPosZ CHexInitXZSin60
#define cRMInitPosX cHexInitXZ //Start positions of the Right Middle leg
#define cRMInitPosY CHexInitY
#define cRMInitPosZ 0
#define cRFInitPosX CHexInitXZCos60 //Start positions of the Right Front leg
#define cRFInitPosY CHexInitY
#define cRFInitPosZ -CHexInitXZSin60
#define cLRInitPosX CHexInitXZCos60 //Start positions of the Left Rear leg
#define cLRInitPosY CHexInitY
#define cLRInitPosZ CHexInitXZSin60
#define cLMInitPosX cHexInitXZ //Start positions of the Left Middle leg
#define cLMInitPosY CHexInitY
#define cLMInitPosZ 0
#define cLFInitPosX CHexInitXZCos60 //Start positions of the Left Front leg
#define cLFInitPosY CHexInitY
#define cLFInitPosZ -CHexInitXZSin60
// Turret initial position
#define cTurretRotInit 0
#define cTurretTiltInit 0
//--------------------------------------------------------------------
//[Tars factors used in formula to calc Tarsus angle relative to the ground]
#define cTarsst 720 //4DOF ONLY
#define cTarsMulti 2 //4DOF ONLY
#define cTarsFactorA 70 //4DOF ONLY
#define cTarsFactorB 60 //4DOF ONLY
#define cTarsFactorC 50 //4DOF ONLY
#endif // HEX_CFG_H