143 lines
3.9 KiB
C++
143 lines
3.9 KiB
C++
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
class RampChecker {
|
|
public:
|
|
uint64_t total_ticks;
|
|
uint32_t last_dt;
|
|
uint32_t min_dt;
|
|
bool increase_ok;
|
|
bool flat_ok;
|
|
bool decrease_ok;
|
|
bool first;
|
|
bool dir_high;
|
|
bool reversing_allowed;
|
|
uint32_t accelerate_till;
|
|
uint32_t coast_till;
|
|
uint32_t time_coasting;
|
|
uint32_t pos;
|
|
uint32_t ticks_since_last_step;
|
|
float avg_accel;
|
|
FILE *gp_file;
|
|
char filename[100];
|
|
|
|
void next_ramp() {
|
|
increase_ok = true;
|
|
decrease_ok = false;
|
|
last_dt = ~0;
|
|
min_dt = ~0;
|
|
first = true;
|
|
dir_high = true;
|
|
coast_till = 0;
|
|
time_coasting = 0;
|
|
accelerate_till = 0;
|
|
reversing_allowed = false;
|
|
}
|
|
RampChecker() {
|
|
ticks_since_last_step = 0;
|
|
avg_accel = 0;
|
|
gp_file = NULL;
|
|
total_ticks = 0;
|
|
pos = 0;
|
|
next_ramp();
|
|
}
|
|
void start_plot(char *fname) {
|
|
int n = strlen(fname) - 8; // remove .gnuplot
|
|
strncpy(filename, fname, n);
|
|
filename[n] = 0;
|
|
gp_file = fopen(fname, "w");
|
|
fprintf(gp_file, "$data <<EOF\n");
|
|
}
|
|
void finish_plot() {
|
|
if (gp_file != NULL) {
|
|
fprintf(gp_file, "EOF\n");
|
|
fprintf(gp_file, "set term pngcairo size 1600, 800\n");
|
|
fprintf(gp_file, "set output \"%s.png\"\n", filename);
|
|
fprintf(gp_file, "set multiplot layout 2,2\n");
|
|
fprintf(gp_file, "set title \"speed [steps/s] over time [s]\"\n");
|
|
fprintf(gp_file, "plot $data using 1:2 with lines notitle\n");
|
|
fprintf(gp_file, "set title \"speed [steps/s] over position\"\n");
|
|
fprintf(gp_file, "plot $data using 4:2 with lines notitle\n");
|
|
fprintf(gp_file, "set title \"position over time [s]\"\n");
|
|
fprintf(gp_file, "plot $data using 1:4 with lines notitle\n");
|
|
fprintf(gp_file,
|
|
"set title \"averaged (!) acceleration [steps/s*s] over time "
|
|
"[s]\"\n");
|
|
fprintf(gp_file, "plot $data using 1:5 with lines notitle\n");
|
|
//fprintf(gp_file, "pause -1\n");
|
|
fclose(gp_file);
|
|
gp_file = NULL;
|
|
}
|
|
}
|
|
void check_section(struct queue_entry *e) {
|
|
uint8_t steps = e->steps;
|
|
if (steps == 0) {
|
|
// Just a pause
|
|
if (ticks_since_last_step <= 0xffff0000) {
|
|
ticks_since_last_step += e->ticks;
|
|
}
|
|
total_ticks += e->ticks;
|
|
printf("process pause %d => %u\n", e->ticks, ticks_since_last_step);
|
|
return;
|
|
}
|
|
if (e->toggle_dir) {
|
|
assert(reversing_allowed);
|
|
dir_high = !dir_high;
|
|
increase_ok = true;
|
|
last_dt = ~0;
|
|
decrease_ok = false;
|
|
}
|
|
if (dir_high) {
|
|
pos += steps;
|
|
} else {
|
|
pos -= steps;
|
|
}
|
|
uint32_t curr_dt = ticks_since_last_step;
|
|
total_ticks += steps * e->ticks;
|
|
if (!first) {
|
|
min_dt = min(min_dt, curr_dt);
|
|
}
|
|
float accel = 0;
|
|
if (last_dt != ~0) {
|
|
accel = (16000000.0 / float(curr_dt) - 16000000.0 / float(last_dt)) /
|
|
(1.0 / 16000000.0 * float(steps * curr_dt));
|
|
avg_accel += (accel - avg_accel) / (steps * 20);
|
|
}
|
|
printf(
|
|
"process command in ramp checker @%.6fs: steps = %d last = %u current "
|
|
"= %u "
|
|
" min_dt "
|
|
"= %u accel=%.6f inc=%s dec=%s\n",
|
|
total_ticks / 16000000.0, steps, last_dt, curr_dt, min_dt, accel,
|
|
increase_ok ? "ALLOW" : "NO", decrease_ok ? "ALLOW" : "NO");
|
|
|
|
if (gp_file != NULL) {
|
|
fprintf(gp_file, "%.6f %.2f %d %d %f\n", total_ticks / 16000000.0,
|
|
16000000.0 / last_dt, last_dt, pos, avg_accel);
|
|
}
|
|
|
|
assert(first || (steps * curr_dt > 0));
|
|
|
|
if (last_dt > curr_dt) {
|
|
assert(increase_ok);
|
|
accelerate_till = total_ticks;
|
|
decrease_ok = true;
|
|
} else if (last_dt < curr_dt) {
|
|
if (increase_ok) {
|
|
coast_till = total_ticks - curr_dt;
|
|
}
|
|
assert(decrease_ok);
|
|
increase_ok = false;
|
|
} else {
|
|
time_coasting += steps * curr_dt;
|
|
}
|
|
|
|
if (!first) {
|
|
last_dt = curr_dt;
|
|
}
|
|
|
|
ticks_since_last_step = e->ticks;
|
|
first = false;
|
|
}
|
|
};
|