Commit 4863fafa authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Remove some debugging stuff and cleanup comments

parent 17ceb08c
......@@ -237,14 +237,12 @@ static void calculate(struct controller_block *spg)
if (priv->current_command.done) {
if (!priv->next_command.done) {
printf("%d next\n", controller_time_samplenr);
priv->current_command = priv->next_command;
priv->next_command.done = 1;
priv->current_command.start = 0;
} else {
if (priv->current_command.type ==
BLOCK_SPG_SETPOINT_TIME) {
printf("%d v = 0.0\n", controller_time_samplenr);
priv->cmd_v = 0.0;
priv->cmd_x =
priv->current_command.setpoint;
......@@ -284,10 +282,6 @@ static void calculate(struct controller_block *spg)
(priv->current_command.setpoint -
priv->cmd_x) / (t+1);
priv->current_command.start = 1;
printf("%d t: %g cmd_v: %.8g diff: %.8g\n",
controller_time_samplenr,
t, priv->cmd_v,
priv->current_command.setpoint - priv->cmd_x);
}
}
......@@ -297,7 +291,6 @@ static void calculate(struct controller_block *spg)
priv->cmd_x =
priv->current_command.setpoint -
priv->cmd_v;
printf("%d done\n", controller_time_samplenr);
}
break;
}
......@@ -324,7 +317,6 @@ static void calculate(struct controller_block *spg)
good_v = almost_equal(priv->cur_v, priv->cmd_v);
if (!good_v || !good_x) {
double error_x_cur;
double error_x;
double req_x_1;
double error_x_jpos;
......@@ -365,7 +357,6 @@ static void calculate(struct controller_block *spg)
j_from_pos = 0;
}
error_x_cur = req_x - x;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(priv, 0, priv->max_a);
......@@ -510,29 +501,20 @@ static void calculate(struct controller_block *spg)
error_x_at_v = req_x - x;
printf("%s req_x_1=%.8g req_v=%.8g cur_x=%.8g cur_j=%g %g %g %g %g %g", spg->name, req_x_1, req_v, priv->cur_x, priv->cur_j, error_x_cur, error_x_at_v, error_x, j, j_from_pos);
if (fabs(error_x_at_v) < fabs(error_x) ||
(signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) ||
ignore_x) {
printf(" diff=%g", fabs(error_x_at_v) - fabs(error_x));
if (!ignore_x &&
// fabs(error_x_at_v) >= fabs(priv->cur_v) &&
signbit(error_x_at_v) == signbit(error_x) ) {
priv->cur_j = j_from_pos;
printf(" j_from_pos");
// /* position got better, but not yet enough */
// priv->cur_j = copysign(priv->max_j, error_x);
} else {
if (state_to_max_a) {
priv->cur_j = j;
printf(" to max a");
} else if (state_at_max_a) {
priv->cur_j = 0;
printf(" at max a");
} else {
priv->cur_j = -j;
printf(" acc down");
}
}
} else {
......@@ -541,7 +523,6 @@ static void calculate(struct controller_block *spg)
*/
priv->cur_j = j_from_pos;
}
printf(" j=%g\n", priv->cur_j);
}
......
Supports Markdown
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