Commit 2b0d4234 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Changes to dt software after testing on 20130901

ctrl & param file: Update to match old dt configuration (with limited speeds)
Small fixes to stoeber code for correct initialization (pdo settings)
Add check for 'zero' state in esc.c
Make sure recover mode overrides all needed variables in el safety block
parent 243edc05
......@@ -311,8 +311,8 @@ params {
{ "elevation_servo_state", "max_a", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
{ "elevation_torsion_spg", "max_x", (float) 0.01 }
{ "elevation_torsion_spg", "min_x", (float)-0.01 }
{ "elevation_torsion_spg", "max_x", (float) 0.00 }
{ "elevation_torsion_spg", "min_x", (float)-0.00 }
{ "elevation_torsion_spg", "max_v", (float) rpm2rads(0.001) }
{ "elevation_torsion_spg", "max_a", (float) 0.00004 }
{ "elevation_torsion_spg", "max_j", (float) 0.00001 }
......
......@@ -6,4 +6,7 @@ params {
{ "elevation_pid", "kp", (float)1 }
{ "elevation_pid", "ki", (float)0 }
{ "elevation_pid", "kd", (float)0 }
# limit spg to 1000rpm
{ "azimuth_spg", "max_v", (float) rpm2rads(1000.0)/$(azimuth_gear) }
}
......@@ -258,8 +258,10 @@ static void calculate(struct controller_block *safety)
}
if (fabsf(torsion_in) >= safety->private->torsion_recover_max) {
out0 = 0.0;
out1 = 0.0;
if (!safety->private->recover) {
out0 = 0.0;
out1 = 0.0;
}
safe = false;
if (!safety->private->err_torsion_recover) {
safety->private->err_torsion_recover = true;
......@@ -274,7 +276,7 @@ static void calculate(struct controller_block *safety)
/* torsion to big */
if (fabsf(torsion_in) >= safety->private->torsion_max) {
/* moving back? */
if (position_in_r > position_in_l) {
if (!safety->private->recover && position_in_r > position_in_l) {
if (out0 > 0.0) {
out0 = 0.0;
safe = false;
......@@ -284,7 +286,7 @@ static void calculate(struct controller_block *safety)
safe = false;
}
}
if (position_in_r < position_in_l) {
if (!safety->private->recover && position_in_r < position_in_l) {
if (out0 < 0.0) {
out0 = 0.0;
safe = false;
......@@ -356,7 +358,7 @@ static void calculate(struct controller_block *safety)
safety->private->speed_out_r = out0;
safety->private->speed_out_l = -out1;
safety->private->safe_out = safe;
safety->private->safe_out = safe || safety->private->recover;
}
......
......@@ -308,26 +308,26 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
/* I80 (4bytes) */
leval32 = STOEBER_PDO_MAP('I', 80, 0, 32);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x0, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x0, &leval32, 4);
/* E91 (4bytes) */
leval32 = STOEBER_PDO_MAP('E', 91, 0, 32);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x1, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x1, &leval32, 4);
/* E90 (2bytes) */
leval32 = STOEBER_PDO_MAP('E', 90, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x2, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x2, &leval32, 4);
/* E19 (2bytes) */
leval32 = STOEBER_PDO_MAP('E', 19, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x3, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x3, &leval32, 4);
/* E10 (2bytes) */
leval32 = STOEBER_PDO_MAP('E', 10, 0, 16);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x4, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x4, &leval32, 4);
leval32 = 0;
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 232), 0x5, &leval32, 4);
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 233), 0x5, &leval32, 4);
/* Receive pdo: */
......
......@@ -171,6 +171,10 @@ int esc_al_error_ack(struct ec_dgram_addr *addr)
return -1;
ctrl = state;
if (!state)
return 0;
log_send(LOG_T_DEBUG, "state: %d", ctrl);
state_add.addr.position.off = ESC_ADDR_MAP_AL_CONTROL;
ctrl |= htole16(ESC_ADDR_MAP_AL_CONTROL_ERROR_ACK);
ret = ec_datagram_write(&state_add, &ctrl, 2);
......@@ -185,6 +189,11 @@ int esc_init(struct ec_dgram_addr *addr)
{
uint8_t nr_syncmgr;
int ret, i;
struct timespec timeout = { 1, 0 };
if (esc_al_state_set(addr, ESC_AL_STATE_INIT, &timeout) < 0)
log_send(LOG_T_ERROR, "Could not go to state init");
addr->addr.position.off = ESC_ADDR_MAP_SYNCMANAGERS_SUPPORTED;
ret = ec_datagram_read(addr, &nr_syncmgr, 1);
......@@ -215,6 +224,7 @@ int esc_init(struct ec_dgram_addr *addr)
ec_datagram_write(addr, &val16, 2);
}
log_send(LOG_T_DEBUG, "ACK any errors");
/* Try to ack any errors */
esc_al_error_ack(addr);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment