add moving model (start from simplest dumb mover)
This commit is contained in:
parent
8a336a688f
commit
17602a017f
@ -1,2 +1,5 @@
|
||||
# mountcontrol
|
||||
|
||||
LibSidServo - low-level library for SSII controller management
|
||||
|
||||
moving_model - model of one-axis moving with ideal reaction
|
||||
|
||||
141
moving_model/Dramp.c
Normal file
141
moving_model/Dramp.c
Normal file
@ -0,0 +1,141 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <math.h>
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Dramp.h"
|
||||
|
||||
static movestate_t state = ST_STOP;
|
||||
static moveparam_t target, Min, Max;
|
||||
static double T0 = -1., Xlast0; // time when move starts, last stage starting coordinate
|
||||
static moveparam_t curparams = {0}; // current coordinate/speed/acceleration
|
||||
static double T1 = -1.; // time to switch into minimal speed for best coord tolerance
|
||||
|
||||
typedef enum{
|
||||
STAGE_NORMALSPEED,
|
||||
STAGE_MINSPEED,
|
||||
STAGE_STOPPED
|
||||
} movingsage_t;
|
||||
|
||||
static movingsage_t movingstage = STAGE_STOPPED;
|
||||
|
||||
int initlims(limits_t *lim){
|
||||
if(!lim) return FALSE;
|
||||
Min = lim->min;
|
||||
Max = lim->max;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static int calc(moveparam_t *x, double t){
|
||||
DBG("target: %g, tagspeed: %g (maxspeed: %g, minspeed: %g)", x->coord, x->speed, Max.speed, Min.speed);
|
||||
if(!x || t < 0.) return FALSE;
|
||||
if(x->speed > Max.speed || x->speed < Min.speed || x->coord < Min.coord || x->coord > Max.coord) return FALSE;
|
||||
double adist = fabs(x->coord - curparams.coord);
|
||||
DBG("want dist: %g", adist);
|
||||
if(adist < coord_tolerance) return TRUE; // we are at place
|
||||
if(adist < time_tick * Min.speed) return FALSE; // cannot reach with current parameters
|
||||
target = *x;
|
||||
if(x->speed * time_tick > adist) target.speed = adist / (10. * time_tick); // take at least 10 ticks to reach position
|
||||
if(target.speed < Min.speed) target.speed = Min.speed;
|
||||
DBG("Approximate tag speed: %g", target.speed);
|
||||
T0 = t;
|
||||
// calculate time to switch into minimal speed
|
||||
T1 = -1.; // no min speed phase
|
||||
if(target.speed > Min.speed){
|
||||
double dxpertick = target.speed * time_tick;
|
||||
DBG("dX per one tick: %g", dxpertick);
|
||||
double ticks_need = floor(adist / dxpertick);
|
||||
DBG("ticks need: %g", ticks_need);
|
||||
if(ticks_need < 1.) return FALSE; // cannot reach
|
||||
if(fabs(ticks_need * dxpertick - adist) > coord_tolerance){
|
||||
DBG("Need to calculate slow phase; can't reach for %g ticks at current speed", ticks_need);
|
||||
double dxpersmtick = Min.speed * time_tick;
|
||||
DBG("dX per smallest tick: %g", dxpersmtick);
|
||||
while(--ticks_need > 1.){
|
||||
double part = adist - ticks_need * dxpertick;
|
||||
double smticks = floor(part / dxpersmtick);
|
||||
double least = part - smticks * dxpersmtick;
|
||||
if(least < coord_tolerance) break;
|
||||
}
|
||||
DBG("now BIG ticks: %g, T1=T0+%g", ticks_need, ticks_need*time_tick);
|
||||
T1 = t + ticks_need * time_tick;
|
||||
}
|
||||
}
|
||||
state = ST_CONSTSPEED;
|
||||
Xlast0 = curparams.coord;
|
||||
if(target.speed > Min.speed) movingstage = STAGE_NORMALSPEED;
|
||||
else movingstage = STAGE_MINSPEED;
|
||||
if(x->coord < curparams.coord) target.speed *= -1.; // real speed
|
||||
curparams.speed = target.speed;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
static void stop(){
|
||||
T0 = -1.;
|
||||
curparams.accel = 0.;
|
||||
curparams.speed = 0.;
|
||||
state = ST_STOP;
|
||||
movingstage = STAGE_STOPPED;
|
||||
}
|
||||
|
||||
static movestate_t proc(moveparam_t *next, double t){
|
||||
if(T0 < 0.) return ST_STOP;
|
||||
curparams.coord = Xlast0 + (t - T0) * curparams.speed;
|
||||
//DBG("coord: %g (dT: %g, speed: %g)", curparams.coord, t-T0, curparams.speed);
|
||||
int ooops = FALSE; // oops - we are over target!
|
||||
if(curparams.speed < 0.){ if(curparams.coord < target.coord) ooops = TRUE;}
|
||||
else{ if(curparams.coord > target.coord) ooops = TRUE; }
|
||||
if(ooops){
|
||||
DBG("OOOps! We are (%g) over target (%g) -> stop", curparams.coord, target.coord);
|
||||
stop();
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
}
|
||||
if(movingstage == STAGE_NORMALSPEED && T1 > 0.){ // check need of T1
|
||||
if(t >= T1){
|
||||
DBG("T1=%g, t=%g -->", T1, t);
|
||||
curparams.speed = (curparams.speed > 0.) ? Min.speed : -Min.speed;
|
||||
movingstage = STAGE_MINSPEED;
|
||||
Xlast0 = curparams.coord;
|
||||
T0 = T1;
|
||||
DBG("Go further with minimal speed");
|
||||
}
|
||||
}
|
||||
if(fabs(curparams.coord - target.coord) < coord_tolerance){ // we are at place
|
||||
DBG("OK, we are in place");
|
||||
stop();
|
||||
}
|
||||
if(next) *next = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
static movestate_t getst(moveparam_t *cur){
|
||||
if(cur) *cur = curparams;
|
||||
return state;
|
||||
}
|
||||
|
||||
movemodel_t dumb = {
|
||||
.init_limits = initlims,
|
||||
.calculate = calc,
|
||||
.proc_move = proc,
|
||||
.stop = stop,
|
||||
.emergency_stop = stop,
|
||||
.get_state = getst,
|
||||
};
|
||||
23
moving_model/Dramp.h
Normal file
23
moving_model/Dramp.h
Normal file
@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t dumb;
|
||||
57
moving_model/Makefile
Normal file
57
moving_model/Makefile
Normal file
@ -0,0 +1,57 @@
|
||||
# run `make DEF=...` to add extra defines
|
||||
PROGRAM := moving
|
||||
LDFLAGS := -fdata-sections -ffunction-sections -Wl,--gc-sections -Wl,--discard-all
|
||||
LDFLAGS += -lusefull_macros -lm
|
||||
SRCS := $(wildcard *.c)
|
||||
DEFINES := $(DEF) -D_GNU_SOURCE -D_XOPEN_SOURCE=1111
|
||||
OBJDIR := mk
|
||||
CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -std=gnu99
|
||||
OBJS := $(addprefix $(OBJDIR)/, $(SRCS:%.c=%.o))
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
TARGFILE := $(OBJDIR)/TARGET
|
||||
CC = gcc
|
||||
#TARGET := RELEASE
|
||||
|
||||
ifeq ($(shell test -e $(TARGFILE) && echo -n yes),yes)
|
||||
TARGET := $(file < $(TARGFILE))
|
||||
else
|
||||
TARGET := RELEASE
|
||||
endif
|
||||
|
||||
ifeq ($(TARGET), DEBUG)
|
||||
.DEFAULT_GOAL := debug
|
||||
endif
|
||||
|
||||
release: $(PROGRAM)
|
||||
|
||||
debug: CFLAGS += -DEBUG -Werror
|
||||
debug: TARGET := DEBUG
|
||||
debug: $(PROGRAM)
|
||||
|
||||
$(TARGFILE): $(OBJDIR)
|
||||
@echo -e "\t\tTARGET: $(TARGET)"
|
||||
@echo "$(TARGET)" > $(TARGFILE)
|
||||
|
||||
$(PROGRAM) : $(TARGFILE) $(OBJS)
|
||||
@echo -e "\t\tLD $(PROGRAM)"
|
||||
$(CC) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
|
||||
|
||||
$(OBJDIR):
|
||||
@mkdir $(OBJDIR)
|
||||
|
||||
ifneq ($(MAKECMDGOALS),clean)
|
||||
-include $(DEPS)
|
||||
endif
|
||||
|
||||
$(OBJDIR)/%.o: %.c
|
||||
@echo -e "\t\tCC $<"
|
||||
$(CC) $< -MD -c $(LDFLAGS) $(CFLAGS) $(DEFINES) -o $@
|
||||
|
||||
clean:
|
||||
@echo -e "\t\tCLEAN"
|
||||
@rm -rf $(OBJDIR) 2>/dev/null || true
|
||||
|
||||
xclean: clean
|
||||
@rm -f $(PROGRAM)
|
||||
|
||||
.PHONY: clean xclean
|
||||
27
moving_model/Sramp.c
Normal file
27
moving_model/Sramp.c
Normal file
@ -0,0 +1,27 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
// quasi non-jerk s-ramp
|
||||
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Sramp.h"
|
||||
|
||||
|
||||
movemodel_t s_shaped = { 0 };
|
||||
23
moving_model/Sramp.h
Normal file
23
moving_model/Sramp.h
Normal file
@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t s_shaped;
|
||||
29
moving_model/Tramp.c
Normal file
29
moving_model/Tramp.c
Normal file
@ -0,0 +1,29 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
// simplest trapezioidal ramp
|
||||
|
||||
#include <stdio.h>
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "Tramp.h"
|
||||
|
||||
|
||||
|
||||
|
||||
movemodel_t trapez = { 0 };
|
||||
23
moving_model/Tramp.h
Normal file
23
moving_model/Tramp.h
Normal file
@ -0,0 +1,23 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving_private.h"
|
||||
|
||||
extern movemodel_t trapez;
|
||||
118
moving_model/main.c
Normal file
118
moving_model/main.c
Normal file
@ -0,0 +1,118 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <usefull_macros.h>
|
||||
|
||||
#include "moving.h"
|
||||
|
||||
static movemodel_t *model = NULL;
|
||||
static FILE *coordslog = NULL;
|
||||
static double Tstart = 0.;
|
||||
|
||||
typedef struct{
|
||||
int help;
|
||||
char *ramptype;
|
||||
char *xlog;
|
||||
int dT;
|
||||
} pars;
|
||||
|
||||
static pars G = {
|
||||
.ramptype = "d",
|
||||
.dT = 100000,
|
||||
};
|
||||
|
||||
static limits_t limits = {
|
||||
.min = {.coord = -1e6, .speed = 0.1, .accel = 0.1},
|
||||
.max = {.coord = 1e6, .speed = 1e3, .accel = 10.},
|
||||
.jerk = 10.
|
||||
};
|
||||
|
||||
static myoption opts[] = {
|
||||
{"help", NO_ARGS, NULL, 'h', arg_int, APTR(&G.help), "show this help"},
|
||||
{"ramp", NEED_ARG, NULL, 'r', arg_string, APTR(&G.ramptype), "ramp type: \"d\", \"t\" or \"s\" - dumb, trapezoid, s-type"},
|
||||
{"deltat", NEED_ARG, NULL, 't', arg_int, APTR(&G.dT), "time interval for monitoring (microseconds, >0)"},
|
||||
{"xlog", NEED_ARG, NULL, 'l', arg_string, APTR(&G.xlog), "log file name for coordinates logging"},
|
||||
// TODO: add parameters for limits setting
|
||||
end_option
|
||||
};
|
||||
|
||||
static int move(moveparam_t *tag){
|
||||
if(!tag) ERRX("move(): needs target");
|
||||
moveparam_t curpos;
|
||||
movestate_t curstate = model->get_state(&curpos);
|
||||
if(curstate == ST_ERROR){
|
||||
WARNX("move(): got error state, can't move");
|
||||
return FALSE;
|
||||
}
|
||||
if(!move_to(tag)){
|
||||
WARNX("move(): can't move to %g with max speed %g", tag->coord, tag->speed);
|
||||
return FALSE;
|
||||
}
|
||||
green("Moving from %g (speed=%g, acc=%g) to %g with maximal speed %g\n",
|
||||
curpos.coord, curpos.speed, curpos.accel, tag->coord, tag->speed);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
// monitor moving with dump to file until T-Tnow == tnext or stop/error
|
||||
// show position every dT
|
||||
static void monit(double tnext){
|
||||
DBG("start monitoring");
|
||||
double t0 = nanot(), t = 0.;
|
||||
do{
|
||||
t = nanot();
|
||||
moveparam_t p;
|
||||
movestate_t st = model->get_state(&p);
|
||||
fprintf(coordslog, "%-9.4f\t%-10.4f\t%-10.4f\t%-10.4f\n",
|
||||
t - Tstart, p.coord, p.speed, p.accel);
|
||||
if(st == ST_STOP || st == ST_ERROR) break;
|
||||
usleep(G.dT);
|
||||
}while(nanot() - t0 < tnext);
|
||||
DBG("End of monitoring");
|
||||
}
|
||||
|
||||
int main(int argc, char **argv){
|
||||
initial_setup();
|
||||
parseargs(&argc, &argv, opts);
|
||||
if(G.help) showhelp(-1, opts);
|
||||
if(G.xlog){
|
||||
coordslog = fopen(G.xlog, "w");
|
||||
if(!coordslog) ERR("Can't open %s", G.xlog);
|
||||
} else coordslog = stdout;
|
||||
if(G.dT < 1) G.dT = 1;
|
||||
fprintf(coordslog, "# time coordinate speed acceleration\n");
|
||||
ramptype_t ramp = RAMP_AMOUNT;
|
||||
if(*G.ramptype == 'd' || *G.ramptype == 'D') ramp = RAMP_DUMB;
|
||||
else if(*G.ramptype == 't' || *G.ramptype == 'T') ramp = RAMP_TRAPEZIUM;
|
||||
else if(*G.ramptype == 's' || *G.ramptype == 'S') ramp = RAMP_S;
|
||||
else ERRX("Point \"d\" (dumb), \"s\" (s-type), or \"t\" (trapez) for ramp type");
|
||||
model = init_moving(ramp, &limits);
|
||||
if(!model) ERRX("Can't init moving model: check parameters");
|
||||
Tstart = nanot();
|
||||
moveparam_t target = {.speed = 100., .coord = 1000};
|
||||
if(move(&target)) monit(1.5);
|
||||
target.speed = 500.;
|
||||
if(move(&target)) monit(2.5);
|
||||
target.coord = -100.;
|
||||
if(move(&target)) monit(4.);
|
||||
target.coord = 0.; target.speed = -200.;
|
||||
if(move(&target)) monit(1e6);
|
||||
usleep(5000);
|
||||
fclose(coordslog);
|
||||
return 0;
|
||||
}
|
||||
124
moving_model/moving.c
Normal file
124
moving_model/moving.c
Normal file
@ -0,0 +1,124 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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 <usefull_macros.h>
|
||||
#include <pthread.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "moving.h"
|
||||
#include "moving_private.h"
|
||||
#include "Dramp.h"
|
||||
#include "Sramp.h"
|
||||
#include "Tramp.h"
|
||||
|
||||
//static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static movemodel_t *model = NULL;
|
||||
double coord_tolerance = COORD_TOLERANCE_DEFAULT;
|
||||
double time_tick = TIME_TICK_DEFAULT;
|
||||
|
||||
// difference of time from first call, using nanoseconds
|
||||
double nanot(){
|
||||
static struct timespec *start = NULL;
|
||||
struct timespec now;
|
||||
if(!start){
|
||||
start = MALLOC(struct timespec, 1);
|
||||
if(!start) return -1.;
|
||||
if(clock_gettime(CLOCK_REALTIME, start)) return -1.;
|
||||
}
|
||||
if(clock_gettime(CLOCK_REALTIME, &now)) return -1.;
|
||||
//DBG("was: %ld, now: %ld", start->tv_nsec, now.tv_nsec);
|
||||
double nd = ((double)now.tv_nsec - (double)start->tv_nsec) * 1e-9;
|
||||
double sd = (double)now.tv_sec - (double)start->tv_sec;
|
||||
return sd + nd;
|
||||
}
|
||||
|
||||
static void* thread(void _U_ *u){
|
||||
if(!model) return NULL;
|
||||
DBG("START thread");
|
||||
double t = 0.;
|
||||
while(1){
|
||||
t = nanot();
|
||||
//pthread_mutex_lock(&mutex);
|
||||
movestate_t curstate = model->get_state(NULL);
|
||||
moveparam_t curmove;
|
||||
if(curstate != ST_STOP && curstate != ST_ERROR){
|
||||
curstate = model->proc_move(&curmove, t);
|
||||
}
|
||||
//pthread_mutex_unlock(&mutex);
|
||||
while(nanot() - t < time_tick);
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void chkminmax(double *min, double *max){
|
||||
if(*min <= *max) return;
|
||||
double t = *min;
|
||||
*min = *max;
|
||||
*max = t;
|
||||
}
|
||||
|
||||
movemodel_t *init_moving(ramptype_t type, limits_t *l){
|
||||
if(!l) return FALSE;
|
||||
pthread_t t;
|
||||
switch(type){
|
||||
case RAMP_DUMB:
|
||||
model = &dumb;
|
||||
break;
|
||||
case RAMP_TRAPEZIUM:
|
||||
model = &trapez;
|
||||
break;
|
||||
case RAMP_S:
|
||||
model = &s_shaped;
|
||||
break;
|
||||
default:
|
||||
return FALSE;
|
||||
}
|
||||
if(!model->init_limits) return NULL;
|
||||
moveparam_t *max = &l->max, *min = &l->min;
|
||||
if(min->speed < 0.) min->speed = -min->speed;
|
||||
if(max->speed < 0.) max->speed = -max->speed;
|
||||
if(min->accel < 0.) min->accel = -min->accel;
|
||||
if(max->accel < 0.) max->accel = -max->accel;
|
||||
chkminmax(&min->coord, &max->coord);
|
||||
chkminmax(&min->speed, &max->speed);
|
||||
chkminmax(&min->accel, &max->accel);
|
||||
if(!model->init_limits(l)) return NULL;
|
||||
if(pthread_create(&t, NULL, thread, NULL)) return NULL;
|
||||
pthread_detach(t);
|
||||
return model;
|
||||
}
|
||||
|
||||
int move_to(moveparam_t *target){
|
||||
if(!target || !model) return FALSE;
|
||||
// only positive velocity
|
||||
if(target->speed < 0.) target->speed = -target->speed;
|
||||
// don't mind about acceleration - user cannot set it now
|
||||
return model->calculate(target, nanot());
|
||||
}
|
||||
|
||||
int init_coordtol(double tolerance){
|
||||
if(tolerance < COORD_TOLERANCE_MIN || tolerance > COORD_TOLERANCE_MAX) return FALSE;
|
||||
coord_tolerance = tolerance;
|
||||
return TRUE;
|
||||
}
|
||||
int init_timetick(double tick){
|
||||
if(tick < TIME_TICK_MIN || tick > TIME_TICK_MAX) return FALSE;
|
||||
time_tick = tick;
|
||||
return TRUE;
|
||||
}
|
||||
70
moving_model/moving.h
Normal file
70
moving_model/moving.h
Normal file
@ -0,0 +1,70 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
// tolerance, time ticks
|
||||
#define COORD_TOLERANCE_DEFAULT (0.01)
|
||||
#define COORD_TOLERANCE_MIN (0.0001)
|
||||
#define COORD_TOLERANCE_MAX (10.)
|
||||
#define TIME_TICK_DEFAULT (0.0001)
|
||||
#define TIME_TICK_MIN (1e-9)
|
||||
#define TIME_TICK_MAX (10.)
|
||||
|
||||
typedef enum{
|
||||
RAMP_DUMB, // no ramp: infinite acceleration/deceleration
|
||||
RAMP_TRAPEZIUM, // trapezium ramp
|
||||
RAMP_S, // s-shaped ramp
|
||||
RAMP_AMOUNT
|
||||
} ramptype_t;
|
||||
|
||||
typedef enum{
|
||||
ST_STOP, // stopped
|
||||
ST_ACC, // accelerated
|
||||
ST_CONSTSPEED, // moving with constant speed
|
||||
ST_DEC, // decelerated
|
||||
ST_ERROR, // some error -> stop
|
||||
ST_AMOUNT
|
||||
} movestate_t;
|
||||
|
||||
typedef struct{
|
||||
double coord;
|
||||
double speed;
|
||||
double accel;
|
||||
} moveparam_t;
|
||||
|
||||
typedef struct{
|
||||
moveparam_t min;
|
||||
moveparam_t max;
|
||||
double jerk;
|
||||
} limits_t;
|
||||
|
||||
typedef struct{
|
||||
int (*init_limits)(limits_t *lim); // init values of limits, jerk
|
||||
int (*calculate)(moveparam_t *target, double t); // calculate stages of traectory beginning from t
|
||||
movestate_t (*proc_move)(moveparam_t *next, double t); // calculate next model point for time t
|
||||
movestate_t (*get_state)(moveparam_t *cur); // get current moving state
|
||||
void (*stop)(); // stop by ramp
|
||||
void (*emergency_stop); // stop with highest acceleration
|
||||
} movemodel_t;
|
||||
|
||||
double nanot();
|
||||
movemodel_t *init_moving(ramptype_t type, limits_t *l);
|
||||
int init_coordtol(double tolerance);
|
||||
int init_timetick(double tick);
|
||||
int move_to(moveparam_t *target);
|
||||
1
moving_model/moving_model.cflags
Normal file
1
moving_model/moving_model.cflags
Normal file
@ -0,0 +1 @@
|
||||
-std=c17
|
||||
2
moving_model/moving_model.config
Normal file
2
moving_model/moving_model.config
Normal file
@ -0,0 +1,2 @@
|
||||
// Add predefined macros for your project here. For example:
|
||||
// #define THE_ANSWER 42
|
||||
1
moving_model/moving_model.creator
Normal file
1
moving_model/moving_model.creator
Normal file
@ -0,0 +1 @@
|
||||
[General]
|
||||
184
moving_model/moving_model.creator.user
Normal file
184
moving_model/moving_model.creator.user
Normal file
@ -0,0 +1,184 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE QtCreatorProject>
|
||||
<!-- Written by QtCreator 16.0.0, 2025-03-18T22:08:04. -->
|
||||
<qtcreator>
|
||||
<data>
|
||||
<variable>EnvironmentId</variable>
|
||||
<value type="QByteArray">{7bd84e39-ca37-46d3-be9d-99ebea85bc0d}</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||
<value type="qlonglong">0</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="bool" key="EditorConfiguration.AutoDetect">true</value>
|
||||
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||
<value type="QString" key="language">Cpp</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||
<value type="QString" key="language">QmlJS</value>
|
||||
<valuemap type="QVariantMap" key="value">
|
||||
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||
<value type="QByteArray" key="EditorConfiguration.Codec">KOI8-R</value>
|
||||
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||
<value type="int" key="EditorConfiguration.LineEndingBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.MarginColumn">80</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseHiding">true</value>
|
||||
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||
<value type="int" key="EditorConfiguration.PreferAfterWhitespaceComments">0</value>
|
||||
<value type="bool" key="EditorConfiguration.PreferSingleLineComments">false</value>
|
||||
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||
<value type="bool" key="EditorConfiguration.ShowMargin">false</value>
|
||||
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
||||
<value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
|
||||
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||
<value type="bool" key="EditorConfiguration.UseIndenter">false</value>
|
||||
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanIndentation">false</value>
|
||||
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||
<value type="QString" key="EditorConfiguration.ignoreFileTypes">*.md, *.MD, Makefile</value>
|
||||
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||
<value type="bool" key="EditorConfiguration.skipTrailingWhitespace">true</value>
|
||||
<value type="bool" key="EditorConfiguration.tintMarginArea">true</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<valuemap type="QVariantMap" key="AutoTest.ActiveFrameworks">
|
||||
<value type="bool" key="AutoTest.Framework.Boost">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.CTest">false</value>
|
||||
<value type="bool" key="AutoTest.Framework.Catch">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.GTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtQuickTest">true</value>
|
||||
<value type="bool" key="AutoTest.Framework.QtTest">true</value>
|
||||
</valuemap>
|
||||
<value type="bool" key="AutoTest.ApplyFilter">false</value>
|
||||
<valuemap type="QVariantMap" key="AutoTest.CheckStates"/>
|
||||
<valuelist type="QVariantList" key="AutoTest.PathFilters"/>
|
||||
<value type="int" key="AutoTest.RunAfterBuild">0</value>
|
||||
<value type="bool" key="AutoTest.UseGlobal">true</value>
|
||||
<valuemap type="QVariantMap" key="ClangTools">
|
||||
<value type="bool" key="ClangTools.AnalyzeOpenFiles">true</value>
|
||||
<value type="bool" key="ClangTools.BuildBeforeAnalysis">true</value>
|
||||
<value type="QString" key="ClangTools.DiagnosticConfig">Builtin.DefaultTidyAndClazy</value>
|
||||
<value type="int" key="ClangTools.ParallelJobs">8</value>
|
||||
<value type="bool" key="ClangTools.PreferConfigFile">true</value>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedDirs"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SelectedFiles"/>
|
||||
<valuelist type="QVariantList" key="ClangTools.SuppressedDiagnostics"/>
|
||||
<value type="bool" key="ClangTools.UseGlobalSettings">true</value>
|
||||
</valuemap>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||
<valuemap type="QVariantMap">
|
||||
<value type="QString" key="DeviceType">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{65a14f9e-e008-4c1b-89df-4eaa4774b6e3}</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/Big/Data/00__Small_tel/moving_model</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">all</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Build</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||
</valuemap>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||
<valuelist type="QVariantList" key="GenericProjectManager.GenericMakeStep.BuildTargets">
|
||||
<value type="QString">clean</value>
|
||||
</valuelist>
|
||||
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericMakeStep</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Clean</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.CustomParsers"/>
|
||||
<value type="bool" key="ProjectExplorer.BuildConfiguration.ParseStandardOutput">false</value>
|
||||
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">GenericProjectManager.GenericBuildConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||
<value type="qlonglong" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Deploy</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||
</valuemap>
|
||||
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.DeployConfiguration.CustomData"/>
|
||||
<value type="bool" key="ProjectExplorer.DeployConfiguration.CustomDataEnabled">false</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
|
||||
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
|
||||
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
|
||||
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||
<value type="QList<int>" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
|
||||
<valuelist type="QVariantList" key="CustomOutputParsers"/>
|
||||
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
|
||||
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
|
||||
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
|
||||
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
|
||||
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||
</valuemap>
|
||||
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||
</valuemap>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||
<value type="qlonglong">1</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
<data>
|
||||
<variable>Version</variable>
|
||||
<value type="int">22</value>
|
||||
</data>
|
||||
</qtcreator>
|
||||
1
moving_model/moving_model.cxxflags
Normal file
1
moving_model/moving_model.cxxflags
Normal file
@ -0,0 +1 @@
|
||||
-std=c++17
|
||||
10
moving_model/moving_model.files
Normal file
10
moving_model/moving_model.files
Normal file
@ -0,0 +1,10 @@
|
||||
Dramp.c
|
||||
Dramp.h
|
||||
Sramp.c
|
||||
Sramp.h
|
||||
Tramp.c
|
||||
Tramp.h
|
||||
main.c
|
||||
moving.c
|
||||
moving.h
|
||||
moving_private.h
|
||||
0
moving_model/moving_model.includes
Normal file
0
moving_model/moving_model.includes
Normal file
26
moving_model/moving_private.h
Normal file
26
moving_model/moving_private.h
Normal file
@ -0,0 +1,26 @@
|
||||
/*
|
||||
* This file is part of the moving_model project.
|
||||
* Copyright 2025 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "moving.h"
|
||||
|
||||
extern double coord_tolerance;
|
||||
extern double time_tick;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user