ec_stoeber.c 10.2 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
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
/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2009
	Copyright Stichting C.A. Muller Radioastronomiestation, 2008, 2009

	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 3 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, see <http://www.gnu.org/licenses/>.

 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "esc.h"
#include "ec_stoeber.h"

int ec_stoeber_init(struct ec_stoeber *stbr)
{
	int status;
	uint32_t leval32;
	uint16_t leval16;
	uint8_t val8;
	struct esc_mailbox *mb;
	struct esc_syncmanager_info mb_rd, mb_wr, pdo_tx, pdo_rx;
	struct canopen_dev *canopen_dev;
	uint32_t vendorid, productcode, serialno;
	
//	if (ec_init(stbr->dev))
//		return -1;
	
	vendorid = esc_esi_vendorid_get(&stbr->addr);
	productcode = esc_esi_productcode_get(&stbr->addr);
	serialno = esc_esi_serialno_get(&stbr->addr);
	printf("VendorId: 0x%04x, ProductCode: 0x%04x, SerialNo: 0x%04x\n",
	    vendorid, productcode, serialno);
	/* TODO: check values */
	
	if (esc_init(&stbr->addr))
		return -1;
	
	status = esc_al_status_code_get(&stbr->addr);
	if (status != 0) {
		printf("status code: 0x%x\n", status);
		esc_al_error_ack(&stbr->addr);
	}
	
	if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_INIT) < 0)
		return -1;
	
	printf("Initializing mailbox\n");
	
	
	esc_dc_init(&stbr->addr);
	esc_dc_sync0_cycle_time(&stbr->addr, 4000*1000);


	stbr->addr.addr.position.off = 0x0000;
	
	
	/* mailbox read (slave->master) @0x18f6 */
	mb_rd.start = 0x18f6;
	mb_rd.len = 0x0040;
	mb_rd.sm = 1;
	
	/* mailbox write (master->slave) @0x1800 */
	mb_wr.start = 0x1800;
	mb_wr.len = 0x0040;
	mb_wr.sm = 0;
	
	mb = esc_mailbox_create(&stbr->addr, &mb_rd, &mb_wr);
	canopen_dev = esc_coe_create(mb);
	
	printf("Going to state pre-operational\n");
	
	if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_PRE_OPERATIONAL) < 0)
		return -1;

	/* A180 device control, disable it for now */
	val8 = 0x00;
90
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
92
93

	/* A61 fault reset source to parameter */
	val8 = 2;
94
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 61), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
95
96
97
	usleep(3000);
	/* A180 device control, reset faults */
	val8 = 0x02;
98
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
99
100
101
	usleep(50000);
	/* A180 device control, reset faults */
	val8 = 0x00;
102
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 180), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
103
104
105

	/* A21 brake resistor */
	leval16 = htole16(stbr->brake_resistor * 10.0);
106
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 21), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
107
108
	/* A22 brake resistor power */
	leval16 = htole16(stbr->brake_power);
109
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 22), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
110
111
112

	/* A29 fault quickstop */
	val8 = 1;
113
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 29), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
114
115
116

	/* A34 auto start */
	val8 = 1;
117
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 34), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
118
119
120
	
	/* A45*/
	val8 = 1;
121
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 45), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
122
123
124
	
	/* A60*/
	val8 = 1;
125
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 60), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
126
127
128
	
	/* A150 cycle time */
	val8 = 0x06;
129
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 150), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
131
132

	/* A258 set PDO timeout */
	leval16 = htole16(1000);
133
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 258), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
134
135
136

	/* G90 activate PLL */
	val8 = 1;
137
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 90), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
138
139
140

	/* G91 pll phase offset */
	leval32 = htole32(0);
141
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 91), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
142
143
144

	/* G92 pll gain pk */
	leval32 = htole32(10);
145
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 92), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
146
147
148

	/* G93 pll low pass */
	leval32 = htole32(999);
149
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 93), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
150
151
152

	/* G98 cycle to 4ms */
	leval16 = htole16(4000);
153
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('G', 98), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
154
155
156

	/* Set I06 to 2 decimal digits */
	val8 = 2;
157
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 6), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
158
159
160

	/* C32 (motor integrator) to 3000 ms */
	leval16 = htole16(32000);
161
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 32), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
162
163
164

	/* C33 low pass reference speed 1 ms */
	leval32 = htole32(10);
165
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 33), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
166
167
168

	/* C130 torque limit source to parameter */
	val8 = 4;
169
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 130), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
170
171
172
173

	/* Set I10 to maximum speed */
	leval32 = htole32(stbr->max_speed / (2*M_PI) * 360 * 100);
//	leval32 = htole32(3000 * 360 / 60 * 100);
174
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 10), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
175
176
177
178

	/* Set I11 to maximum acceleration */
//	leval32 = htole32(2000000 * 100);
	leval32 = htole32(stbr->max_accel * 100);
179
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 11), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
180
181
182
		
	/* Set I25 to 100% speed ff */
	leval16 = htole16(100);
183
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 25), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
184
185
186

	/* Set I30 to 1 (encoder signal 0) */
	val8 = 1;
187
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 30), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
188
189
190

	/* Set I37 to 1 (active) */
	val8 = 1;
191
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 37), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
192
193
194

	/* Set I100 to parameter (execute source) */
	val8 = 2;
195
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 100), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
196
197
		
	/* B17 Torque M0 */
198
	canopen_read_param(canopen_dev, STOEBER_PARAM2INDEX('B', 17), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
199
200
201
202
203
204
	printf("B17: %08x %d\n", le32toh(leval32), le32toh(leval32));
	stbr->standstill_torque = (float)(le32toh(leval32)) / 1000.0;
	printf("Standstill torque: %f\n", stbr->standstill_torque);

	/* C03 max pos torque 150% of standstill */
	leval32 = htole32(150);
205
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 3), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
206
207
208

	/* C05 max neg torque -150% of standstill */
	leval32 = htole32(-150);
209
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('C', 5), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
210
211
212
213
214
215
216
217

//	/* A180 device control, enable it to make sure we stay
//	   at the same position */
//	val8 = 0x01;
//	canopen_write_param(canopen_dev, 0x20b4, 0x0, &val8, 1);

	/* I211 command byte to 4 (move_velocity) */
	val8 = 0x04;
218
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 211), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
219
220
	/* I215 v to 0 */
	leval16 = htole16(0);
221
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 215), 0x0, &leval16, 2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
222
223
	/* I210 latch command */
	val8 = 0x00;
224
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 210), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
225
	val8 = 0x01;
226
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('I', 210), 0x0, &val8, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
227
228
229
230

	/* Transmit pdo: */

	/* I80 (4bytes) */
231
232
	leval32 = STOEBER_PDO_MAP('I', 80, 0, 32);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
233
234
		
	/* E91 (4bytes) */
235
236
	leval32 = STOEBER_PDO_MAP('E', 91, 0, 32);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x1, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
237
238

	/* E90 (2bytes) */
239
240
	leval32 = STOEBER_PDO_MAP('E', 90, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x2, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
241
242
		
	/* E19 (2bytes) */
243
244
	leval32 = STOEBER_PDO_MAP('E', 19, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x3, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
245
246

	/* E10 (2bytes) */
247
248
	leval32 = STOEBER_PDO_MAP('E', 10, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x4, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
249
250

	leval32 = 0;
251
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x5, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
252
253
254
255

	/* Receive pdo: */

	/* A180 (1byte) */
256
257
	leval32 = STOEBER_PDO_MAP('A', 180, 0, 32);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x0, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
258
259
		
	/* F210 (1byte) */
260
261
	leval32 = STOEBER_PDO_MAP('F', 210, 0, 32);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x1, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
262
263

	/* I210 (2bytes) */
264
265
	leval32 = STOEBER_PDO_MAP('I', 210, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x2, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
266
267
		
	/* I213 (4bytes) */
268
269
	leval32 = STOEBER_PDO_MAP('I', 213, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x3, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
270
271

	/* I215 (2bytes) */
272
273
	leval32 = STOEBER_PDO_MAP('I', 215, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x4, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
274
275

	/* C230 (2bytes) */
276
277
	leval32 = STOEBER_PDO_MAP('C', 230, 0, 16);
	canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x5, &leval32, 4);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
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

	esc_coe_destroy(canopen_dev);
	esc_mailbox_destroy(mb);
	

	printf("Setup PDOs\n");

	/* sm2 tx pdo (master->slave) @0x1600 */

	pdo_tx.start = 0x1600;
	pdo_tx.len = 0x000c;
	pdo_tx.sm = 2;

	/* sm3 rx pdo (slave->master) @0x1a00 */

	pdo_rx.start = 0x1a00;
	pdo_rx.len = 0x000e;
	pdo_rx.sm = 3;
		
	esc_pdo_tx_set(&stbr->addr, &pdo_tx, &stbr->tx_buffer, false);
	esc_pdo_rx_set(&stbr->addr, &pdo_rx, &stbr->rx_buffer);
	
	printf("going to safe operational\n");
	
	if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_SAFE_OPERATIONAL) < 0)
		return -1;

	stbr->rx_I80 = (uint32_t*)(stbr->rx_buffer + STOEBER_PDO_I80);
	stbr->rx_E91 = (uint32_t*)(stbr->rx_buffer + STOEBER_PDO_E91);
	stbr->rx_E90 = (uint16_t*)(stbr->rx_buffer + STOEBER_PDO_E90);
	stbr->rx_E19 = (uint16_t*)(stbr->rx_buffer + STOEBER_PDO_E19);
	stbr->rx_E10 = (uint16_t*)(stbr->rx_buffer + STOEBER_PDO_E10);
	
	stbr->tx_A180 = (uint8_t*)(stbr->tx_buffer + STOEBER_PDO_A180);
	stbr->tx_F210 = (uint8_t*)(stbr->tx_buffer + STOEBER_PDO_F210);
	stbr->tx_I210 = (uint16_t*)(stbr->tx_buffer + STOEBER_PDO_I210);
	stbr->tx_I213 = (uint32_t*)(stbr->tx_buffer + STOEBER_PDO_I213);
	stbr->tx_I215 = (uint16_t*)(stbr->tx_buffer + STOEBER_PDO_I215);
	stbr->tx_C230 = (uint16_t*)(stbr->tx_buffer + STOEBER_PDO_C230);

	printf("Done\n");
	return 0;

}