-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathMightyZap.h
332 lines (286 loc) · 11.9 KB
/
MightyZap.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
/*
* wiringSerial.h:
* Handle a serial port
***********************************************************************
* This file is part of wiringPi:
* https://projects.drogon.net/raspberry-pi/wiringpi/
*
* wiringPi is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* wiringPi 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with wiringPi. If not, see <http://www.gnu.org/licenses/>.
***********************************************************************
*/
#ifdef __cplusplus
extern "C" {
#endif
enum stroke_type{
Short =0,
Lowest =0,
Highest=1,
Long=1,
Center=2,
Middle =2
};
enum level_type{
Low =0,
High,
};
enum gain{
dGain,
iGain,
pGain
};
enum RGB{
off=0,
RED=2,
GREEN=4,
BLUE =8
};
//typedef enum{false,true}bool;
#define IRP_RX_BUF_SIZE 255
#define IRP_PARAMETER_BUF_SIZE 128
#define MightyZapSheild 4
//int MightyZap_sheild=0;
int fd;
int MightyZap_DirPin;
int MightyZap_DirPin_Level_Tx;
int MightyZap_DirPin_Level_Rx;
int mRxBuffer[IRP_RX_BUF_SIZE];
int mTxBuffer[IRP_RX_BUF_SIZE];
int mParamBuffer[IRP_PARAMETER_BUF_SIZE];
int mBusUsed;
int mRxLength;
// additions to return proper COMM_* status
int mIRPtxrxStatus;
// additions to permit non-default Status Return Level settings without returning errors
int gbIRPStatusReturnLevel;
// additions to adjust number of txrx attempts
int gbIRPNumberTxRxAttempts;
int mPacketType;
int mPktIdIndex;
int mPktLengthIndex;
int mPktInstIndex;
int mPktErrorIndex;
//int mRxLengthOffset;
int mbLengthForPacketMaking;
int mbIDForPacketMaking;
int mbInstructionForPacketMaking;
int mCommStatus;
int SmartDelayFlag;
#define IRP_BROADCAST_ID (254) /* 0xFE */
enum IRP_INSTRUCTION{
CMD_PING = 0xF1,
CMD_READ = 0xF2,
CMD_WRITE = 0xF3,
CMD_REG_WRITE = 0xF4,
CMD_ACTION = 0xF5,
CMD_FACTORY_RESET = 0xF6,
CMD_REBOOT = 0xF8,
CMD_SYSTEM_WRITE = 0xF9, // 0x0D
CMD_STATUS = 0x55,
CMD_SYNC_READ = 0x82,
CMD_SYNC_WRITE = 0x73, // 0x83
CMD_BULK_READ = 0x92,
CMD_BULK_WRITE = 0x93
};
/*
* defines error message for protocol 1.0
* */
#define ERRBIT_VOLTAGE (1)
#define ERRBIT_ANGLE (2)
#define ERRBIT_OVERHEAT (4)
#define ERRBIT_RANGE (8)
#define ERRBIT_CHECKSUM (16)
#define ERRBIT_OVERLOAD (32)
#define ERRBIT_INSTRUCTION (64)
/*
* defines error message for protocol 2.0
* */
#define ERRBIT_RESULT_FAIL (1)
#define ERRBIT_CMD_ERROR (2)
#define ERRBIT_CRC (4)
#define ERRBIT_DATA_RANGE (8)
#define ERRBIT_DATA_LENGTH (16)
#define ERRBIT_DATA_LIMIT (32)
#define ERRBIT_ACCESS (64)
/*
* defines message of communication
* */
#define COMM_TXSUCCESS (0)
#define COMM_RXSUCCESS (1)
#define COMM_TXFAIL (2)
#define COMM_RXFAIL (3)
#define COMM_TXERROR (4)
#define COMM_RXWAITING (5)
#define COMM_RXTIMEOUT (6)
#define COMM_RXCORRUPT (7)
/* timing defines */
#define RX_ENABLE_TIME 350
#define RX_TIMEOUT_COUNT2 (1600L) //(1000L) //porting//1600
#define NANO_TIME_DELAY (12000)
//#define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*90L)// -> 110msec before Ver 1.11e
#define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*128L)// -> 156msec for ID 110 safe access after Ver 1.11f //porting ydh
/*
*
*
*
* */
#define IRP_MODEL_NUMBER_L 0x00
#define IRP_MODEL_NUMBER_H 0x01
#define IRP_VERSION_OF_FIRMWARE 0x02
#define IRP_ACTUATOR_ID 0x03
#define IRP_ACTUATOR_BAUDRATE 0x04
#define IRP_RETURN_DELAY_TIME 0x05
#define IRP_CW_ANGLE_LIMIT_L 0x06
#define IRP_CW_ANGLE_LIMIT_H 0x07
#define IRP_CCW_ANGLE_LIMIT_L 0x08
#define IRP_CCW_ANGLE_LIMIT_H 0x09
#define IRP_LIMIT_TEMPERATURE 0x0b
#define IRP_DOWN_LIMIT_VOLTAGE 0x0c
#define IRP_UP_LIMIT_VOLTAGE 0x0d
#define IRP_MAX_TORQUE_L 0x0e
#define IRP_MAX_TORQUE_H 0x0f
#define IRP_STATUS_RETURN_LEVEL 0x10
#define IRP_ALARM_LED 0x11
#define IRP_ALARM_SHUTDOWN 0x12
#define IRP_RESOLUTION_DIVIDER 0x16
#define IRP_DOWN_CALIBRATION_L 0x18
#define IRP_DOWN_CALIBRATION_H 0x19
#define IRP_UP_CALIBRATION_L 0x1a
#define IRP_UP_CALIBRATION_H 0x1b
#define IRP_CALIBRATION_DIFFER_L 0x1c
#define IRP_CALIBRATION_DIFFER_H 0x1d
#define IRP_ACC_RATE 0x21
#define IRP_DEC_RATE 0x22
#define IRP_MAX_ACC_TIME 0x23
#define IRP_MAX_DEC_TIME 0x24
#define IRP_D_GAIN 0x25
#define IRP_I_GAIN 0x26
#define IRP_P_GAIN 0x27
#define IRP_PW_SHORT_STROKE_L 0x28
#define IRP_PW_SHORT_STROKE_H 0x29
#define IRP_PW_LONG_STROKE_L 0x2A
#define IRP_PW_LONG_STROKE_H 0x2B
#define IRP_PW_MID_STROKE_L 0x2C
#define IRP_PW_MID_STROKE_H 0x2D
#define IRP_PULSE_INERTIA_RATE 0x2E
#define IRP_PULSE_INERTIA_RANGE_L 0x2F
#define IRP_PULSE_INERTIA_RANGE_H 0x30
#define IRP_PULSE_MODE_DISABLE 0x31
#define IRP_CENTER_DIFFER_L 0x32
#define IRP_CENTER_DIFFER_H 0x33
#define IRP_PUNCH_INIT_L 0x34
#define IRP_PUNCH_INIT_H 0x35
#define IRP_TORQUE_ENABLE 0x80
#define IRP_LED_COLOR 0x81
#define IRP_CW_COMPLIANCE_MARGIN 0x82
#define IRP_CCW_COMPLIANCE_MARGIN 0x83
#define IRP_CW_COMPLIANCE_SLOPE 0x84
#define IRP_CCW_COMPLIANCE_SLOPE 0x85
#define IRP_GOAL_POSITION_L 0x86
#define IRP_GOAL_POSITION_H 0x87
#define IRP_MOVING_SPEED_L 0x88
#define IRP_MOVING_SPEED_H 0x89
#define IRP_TORQUE_LIMIT_L 0x8A
#define IRP_TORQUE_LIMIT_H 0x8B
#define IRP_PRESENT_POSITION_L 0x8C
#define IRP_PRESENT_POSITION_H 0x8D
#define IRP_PRESENT_SPEED_L 0x8E
#define IRP_PRESENT_SPEED_H 0x8F
#define IRP_PRESENT_LOAD_L 0x90
#define IRP_PRESENT_LOAD_H 0x91
#define IRP_PRESENT_VOLTAGE 0x92
#define IRP_PRESENT_TEMPERATURE 0x93
#define IRP_REGISTERED_INSTRUCTION 0x94
#define IRP_RESERVED_45 0x95
#define IRP_MOVING 0x96
#define IRP_LOCK 0x97
#define IRP_PUNCH_L 0x98
#define IRP_PUNCH_H 0x99
#define IRP_CURRENT_L 0xAC
#define IRP_CURRENT_H 0xAD
#define IRP_TORGUE_CONTROL_MODE_ENABLE 0xAE
#define IRP_GOAL_TORQUE_L 0xAF
#define IRP_GOAL_TORQUE_H 0xB0
#define IRP_GOAL_ACCELERATION 0xB1
/*Half Duflex UART ENABLE PIN ----------------------------------------------------------------------------------------------*/
//#define PORT_TXRX_DIRECTION GPIOC
//#define PIN_TXRX_DIRECTION 4
///////////////// utility for value ///////////////////////////
#define IRP_MAKEint(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8))
#define IRP_MAKEDint(a, b) ((unsigned long)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned long)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16))
#define IRP_LOint(l) ((unsigned short)(((unsigned long)(l)) & 0xffff))
#define IRP_HIint(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff))
#define IRP_LOBYTE(w) ((unsigned char)(((unsigned short)(w)) & 0xff))
#define IRP_HIBYTE(w) ((unsigned char)((((unsigned short)(w)) >> 8) & 0xff))
#define IRP_PACKET_TYPE1 1 //IRP protocol 1.0 packet type
#define IRP_PACKET_TYPE2 2 //IRP protocol 2.0 packet type
extern int readRaw(void);
extern void writeRaw(char value);
extern void irpTxEnable(void);
extern void irpTxDisable(void);
extern void clearBuffer(void);
extern void setPacketType(int type);
extern int getPacketType(void);
extern int checkPacketType(void);
extern int getTxRxStatus(void);
extern int getResult(void);
extern int getError( int errbit );
extern int txPacket(int bID, int bInstruction, int bParameterLength);
extern int rxPacket(int bRxLength);
extern void printBuffer(int *bpPrintBuffer, int bLength);
extern int txRxPacket(int bID, int bInst, int bTxParaLen);
extern int Dummy(int tmp);
extern void uDelay(int uTime);
extern void nDelay(int nTime);
extern int ping(int bID );
extern int writeByte(int bID, int bAddress, int bData);
extern int readByte(int bID, int bAddress);
extern int writeint(int bID, int bAddress, short wData);
extern int readint(int bID, int bAddress);
extern int setPosition(int bID, int Position, int Speed);
extern int bulkRead(int *param, int param_length);
extern void setTxPacketId(int id);
extern void setTxPacketInstruction(int instruction);
extern void setTxPacketParameter( int index, int value );
extern void setTxPacketLength( int length);
extern int txrxPacket(void);
extern int getRxPacketParameter( int index );
extern int getRxPacketLength(void);
// Data memory Map
extern int MightyZapSetup(int DrectionPin,int Level);
extern int OpenMightyZap (const char *device, const int baud);
extern void CloseMightyZap (const int fd);
extern void goalPosition(int bID,int position);
extern void forceEnable(int bID,int enable);
extern void movingSpeed(int bID,int value);
extern int presentPosition(int bID);
extern void setAlarmLed(int bID, int flag);
extern int getAlarmLed(int bID);
extern void setAlarmShutdown(int bID,int flag);
extern int getAlarmShutdown(int bID);
extern void write_Addr(int bID,int addr, int size,int Data);
extern int read_Addr(int bID, int addr ,int size);
extern int readError(int bID);
extern int fast_rxPacket(int bRxLength);
extern int fast_txRxPacket(int bID, int bInst, int bTxParaLen);
extern int getFastPresentPosition(int bID);
extern int flushPacket(int fd);
extern void serialFlush (const int fd);
extern void serialPutchar (const int fd, const unsigned char c);
extern void serialPuts (const int fd, const char *s);
extern void serialPrintf (const int fd, const char *message, ...);
extern int available (const int fd);
extern int serialGetchar (const int fd);
#ifdef __cplusplus
}
#endif