less squares 4 speed + fixed some bugs (but found more)

LibSidServo/movingmodel.h

@@ -17,12 +17,13 @@
  */
 
 #pragma once
+#include <pthread.h>
 
 #include "sidservo.h"
 
 // tolerance, time ticks
-#define COORD_TOLERANCE_DEFAULT (0.01)
-#define COORD_TOLERANCE_MIN     (0.0001)
+#define COORD_TOLERANCE_DEFAULT (1e-6)
+#define COORD_TOLERANCE_MIN     (1e-8)
 #define COORD_TOLERANCE_MAX     (10.)
 #define TIME_TICK_DEFAULT       (0.0001)
 #define TIME_TICK_MIN           (1e-9)
@@ -46,14 +47,29 @@ typedef struct{
     double acceleration;
 } 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)(double t);                                 // stop by ramp
-    void (*emergency_stop)(double t);                       // stop with highest acceleration
-    double (*stoppenanotime)();                                // time when moving will ends
+typedef enum{
+    STAGE_ACCEL,        // start from zero speed and accelerate to Max speed
+    STAGE_MAXSPEED,     // go with target speed
+    STAGE_DECEL,        // go from target speed to zero
+    STAGE_STOPPED,      // stop
+    STAGE_AMOUNT
+} movingstage_t;
+
+typedef struct movemodel{
+    moveparam_t Min;
+    moveparam_t Max;
+    movingstage_t movingstage;
+    movestate_t state;
+    double Times[STAGE_AMOUNT];
+    moveparam_t Params[STAGE_AMOUNT];
+    moveparam_t curparams;                    // init values of limits, jerk
+    int (*calculate)(struct movemodel *m, moveparam_t *target, double t);        // calculate stages of traectory beginning from t
+    movestate_t (*proc_move)(struct movemodel *m, moveparam_t *next, double t);  // calculate next model point for time t
+    movestate_t (*get_state)(struct movemodel *m, moveparam_t *cur);             // get current moving state
+    void (*stop)(struct movemodel *m, double t);                                 // stop by ramp
+    void (*emergency_stop)(struct movemodel *m, double t);                       // stop with highest acceleration
+    double (*stoppedtime)(struct movemodel *m);                                // time when moving will ends
+    pthread_mutex_t mutex;
 } movemodel_t;
 
 movemodel_t *model_init(limits_t *l);