This repository has been archived by the owner on Jun 21, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathrcb4.h
197 lines (180 loc) · 7.96 KB
/
rcb4.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
/**
* Kondo RCB-4 Library (Header)
*
* Copyright 2010 - Christopher Vo ([email protected])
* George Mason University - Autonomous Robotics Laboratory
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef __LIBKONDO_H__
#define __LIBKONDO_H__
#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <ctype.h>
#include <sys/time.h>
#include "ftdi.h"
// general --------------------------------------------------------------------
#define RCB4_SECOND 1000000
#define RCB4_15MS 15000
#define RCB4_50MS 50000
// communcation ---------------------------------------------------------------
#define RCB4_USB_VID 0x165c
#define RCB4_USB_PID 0x0007
//#define RCB4_USB_VID 0x0403 // FTDI Ltd. Devices
//#define RCB4_USB_PID 0x6001 // FT232RL (Kondo USB-UART)
//#define RCB4_USB_PID 0x6011 // FT4232H (4-port USB-UART)
//#define RCB4_USB_PID 0x6010 // FT2232H/D (2-port USB-UART)
#define RCB4_BAUD 115200
#define RCB4_SWAP_SIZE 256
#define RCB4_RX_TIMEOUT 1000000
// rcb4 -----------------------------------------------------------------------
// byte codes
#define RCB4_ACK_BYTE 0x06 // acknowledge byte
#define RCB4_NCK_BYTE 0x15 // negative acknowledge byte
// sub-commands
#define RCB4_CMD_ACK 0xFE // ACK command
#define RCB4_CMD_MOV 0x00 // MOV command
#define RCB4_CMD_CALL 0x0C // CALL command
#define RCB4_CMD_VERS 0xFD // Version command
#define RCB4_CMD_ICS 0x10 // ICS frame command
// device types
#define RCB4_DEV_RAM 0x00 // RAM
#define RCB4_DEV_DEV 0x01 // Device
#define RCB4_DEV_COM 0x02 // COM port
#define RCB4_DEV_ROM 0x03 // ROM
#define RCB4_RAM_TO_COM ((RCB4_DEV_COM << 4) | RCB4_DEV_RAM)
#define RCB4_COM_TO_RAM ((RCB4_DEV_RAM << 4) | RCB4_DEV_COM)
// option bits
#define RCB4_OPT_BYTES 2 // number of option bytes
#define RCB4_OPT_SIO (1 << 0) // ICS on/off
#define RCB4_OPT_EEPROM (1 << 1) // EEPROM on/off
#define RCB4_OPT_RESP (1 << 2) // Servo response on/off
#define RCB4_OPT_VEC (1 << 3) // Vector jump flag
#define RCB4_OPT_FRAME_10MS (0x0 << 4) // Frame interval
#define RCB4_OPT_FRAME_20MS (0x1 << 4)
#define RCB4_OPT_FRAME_15MS (0x2 << 4)
#define RCB4_OPT_FRAME_25MS (0x3 << 4)
#define RCB4_OPT_COM_BAUD_125200 (0x0 << 6) // COM Baud Rates
#define RCB4_OPT_COM_BAUD_625000 (0x2 << 6)
#define RCB4_OPT_COM_BAUD_1250000 (0x1 << 6)
#define RCB4_OPT_ZERO (1 << 8) // zero flag
#define RCB4_OPT_CARRY (1 << 9) // carry flag
#define RCB4_OPT_ERROR (1 << 10) // program error
#define RCB4_OPT_ICS_BAUD_125200 (0x0 << 13) // ICS Baud Rates
#define RCB4_OPT_ICS_BAUD_625000 (0x2 << 13)
#define RCB4_OPT_ICS_BAUD_1250000 (0x1 << 13)
#define RCB4_OPT_GREEN_LED (1 << 15) // Green LED
// hard-coded option bytes
#define RCB4_OPT_LOW 0x11 // hard-coded option bytes
#define RCB4_OPT_HIGH 0x0
// addresses stuff in ROM / RAM
#define RCB4_ADDR_OPT 0x0000 // Option bytes
#define RCB4_ADDR_PGC 0x0002 // program counter address (rw)
#define RCB4_ADDR_EEPROM_FLAG 0x0007 // ROM address update flag (ro)
#define RCB4_ADDR_AD_REF_BASE 0xC // analog reference value address (rw)
#define RCB4_ADDR_AD_READ_BASE 0x22 // analog reading address (ro)
#define RCB4_NUM_ANALOGS 11 // number of analog ports (AD0-AD10)
#define RCB4_ADDR_PIO_SET 0x38 // PIO direction setup (rw) (1=Out, 0=In)
#define RCB4_ADDR_PIO_OUTPUT 0x3A // PIO output (rw) (1=GND, 0=Vcc)
#define RCB4_ADDR_TIMER_0 0x3C // Timer 0 (rw)
#define RCB4_ADDR_TIMER_1 0x3E // Timer 1 (rw)
#define RCB4_ADDR_TIMER_2 0x40 // Timer 2 (rw)
#define RCB4_ADDR_TIMER_3 0x42 // Timer 3 (rw)
#define RCB4_ADDR_ICS_BASE 0x44 // ICS base address
#define RCB4_NUM_ICS 36 // number of ICS addresses
#define RCB4_ADDR_MAIN 0x03FD // main program loop
#define RCB4_ADDR_SERVO 0x90 // Servo data address (20 bytes ea)
#define RCB4_SERVO_DATA_SIZE 20 // Bytes per servo
#define RCB4_NUM_SERVOS 34 // Max servos
#define RCB4_ADDR_KRI 0x034C // KRI data start
#define RCB4_ADDR_BTN_L 0x50 // KRI button data start (0x350)
#define RCB4_ADDR_BTN_M 0x03 //
#define RCB4_ADDR_BTN_H 0x00 //
#define RCB4_ADDR_MOT_BASE 3000 // motion base address
#define RCB4_MOT_SIZE 2048 // motion slot num bytes
#define RCB4_MOT_MAX 241336 // highest motion
#define RCB4_SERVO_ID_OFFSET 1 // data offset for ID
#define RCB4_SERVO_SETPOS_OFFSET 6 // data offset for set pos
#define RCB4_SERVO_POS_OFFSET 4 // data offset for curr pos
#define RCB4_SERVO_TRIM_OFFSET 2 // data offset for TRIM
#define RCB4_ADDR_COUNTER_BASE 0x457 // counter base address
#define RCB4_NUM_COUNTERS 11 // number of counters (0-10)
// button codes
#define RCB4_BTN_NP 0 // nothing
#define RCB4_BTN_LU (1 << 0) // left pad up
#define RCB4_BTN_LD (1 << 1) // left pad down
#define RCB4_BTN_LR (1 << 2) // left pad right
#define RCB4_BTN_LL (1 << 3) // left pad left
#define RCB4_BTN_RU (1 << 4) // right pad up (triangle)
#define RCB4_BTN_RD (1 << 5) // right pad down (cross)
#define RCB4_BTN_RR (1 << 6) // right pad right (circle)
#define RCB4_BTN_RL (1 << 8) // right pad left (square)
#define RCB4_BTN_S1 (1 << 9) // shift (L trigger U)
#define RCB4_BTN_S2 (1 << 10) // shift (L trigger D)
#define RCB4_BTN_S3 (1 << 11) // shift (R trigger D)
#define RCB4_BTN_S4 (1 << 12) // shift (R trigger U)
#ifndef __UCHAR__
#define __UCHAR__
typedef unsigned char UCHAR;
#endif
#ifndef __UINT__
#define __UINT__
typedef unsigned int UINT;
#endif
// instance data --------------------------------------------------------------
typedef struct
{
struct ftdi_context ftdic; // ftdi context
UCHAR swap[RCB4_SWAP_SIZE]; // swap space
char error[128]; // error messages
UCHAR opt[RCB4_OPT_BYTES]; // option bytes
int debug; // whether to print debug info
} KondoInstance;
// typedefs -------------------------------------------------------------------
typedef KondoInstance* KondoRef;
// initialization / deinitialization ------------------------------------------
int kondo_init(KondoRef ki);
int kondo_init_custom(KondoRef ki, int baud, int vid, int pid, int interface);
int kondo_close(KondoRef ki);
// basic communication --------------------------------------------------------
int kondo_write(KondoRef ki, int n);
int kondo_read(KondoRef ki, int n);
int kondo_read_timeout(KondoRef ki, int n, long timeout);
int kondo_purge(KondoRef ki);
int kondo_trx(KondoRef ki, int out_bytes, int in_bytes);
// rcb4 commands --------------------------------------------------------------
int kondo_ack(KondoRef ki);
int kondo_get_options(KondoRef ki);
int kondo_play_motion(KondoRef ki, UINT num, long max_wait);
int kondo_stop_motion(KondoRef ki);
int kondo_krc3_buttons(KondoRef ki, UINT cc, UCHAR a1, UCHAR a2, UCHAR a3,
UCHAR a4);
int kondo_read_analog(KondoRef ki, int * result, UINT num);
int kondo_set_pio_direction(KondoRef ki, UINT bitset);
int kondo_get_pio_direction(KondoRef ki, UINT * bitset);
int kondo_read_pio(KondoRef ki, UINT * result);
int kondo_write_pio(KondoRef ki, UINT bitset);
int kondo_set_counter(KondoRef ki, UINT num, UCHAR val);
int kondo_get_counter(KondoRef ki, UCHAR * result, UINT num);
int kondo_send_ics_pos(KondoRef ki, UCHAR servos[5], UINT frame);
int kondo_get_servo_data(KondoRef ki, UINT servo_idx, UINT offset);
int kondo_get_servo_trim(KondoRef ki, UINT servo_idx);
int kondo_get_servo_setpos(KondoRef ki, UINT servo_idx);
int kondo_get_servo_pos(KondoRef ki, UINT servo_idx);
int kondo_get_servo_id(KondoRef ki, UINT servo_idx);
// utility --------------------------------------------------------------------
UCHAR kondo_checksum(KondoRef ki, int n);
int kondo_verify_checksum(KondoRef ki, int n);
int kondo_load_asciihex(KondoRef ki, const char * hex);
#endif // __LIBKONDO_H__