start traectories

LibSidServo/examples/traectories.c

@@ -0,0 +1,142 @@
+/*
+ * This file is part of the libsidservo 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/>.
+ */
+
+// some simplest traectories
+// all traectories runs increasing X and Y from starting point
+
+#include <stdio.h>
+#include <string.h>
+#include <usefull_macros.h>
+
+#include "simpleconv.h"
+#include "traectories.h"
+
+static traectory_fn *cur_traectory = NULL;
+// starting point of traectory
+static coords_t XYstart = {0};
+static double tstart = 0.;
+// convert Xe/Ye to approximate motor coordinates:
+// Xnew = Xcor+Xe; Ynew = Ycor+Ye; as Ye goes backwards to Ym, we have
+// Xcor = Xm0 - Xe0; Ycor = Xm0 + Ye0
+static coords_t XYcor = {0};
+
+/**
+ * @brief init_traectory - init traectory fn, sync starting positions of motor & encoders
+ * @param f - function calculating next point
+ * @param XY0 - starting point
+ * @return FALSE if failed
+ */
+int init_traectory(traectory_fn *f, coords_t *XY0){
+    if(!f || !XY0) return FALSE;
+    cur_traectory = f;
+    XYstart = *XY0;
+    tstart = cl_dtime();
+    mountdata_t mdata;
+    int ntries = 0;
+    for(; ntries < 10; ++ntries){
+        if(MCC_E_OK == Mount.getMountData(&mdata)) break;
+    }
+    if(ntries == 10) return FALSE;
+    XYcor.X = mdata.motposition.X - mdata.encposition.X;
+    XYcor.Y = mdata.motposition.X + mdata.encposition.Y;
+    DBG("STARTING POINTS: x=%g, y=%g degrees", DEG2RAD(XYcor.X), DEG2RAD(XYcor.Y));
+    return TRUE;
+}
+
+/**
+ * @brief traectory_point - get traectory point for given time
+ * @param nextpt (o) - next point coordinates
+ * @param t - UNIX-time of event
+ * @return FALSE if something wrong (e.g. X not in -90..90 or Y not in -180..180)
+ */
+int traectory_point(coords_t *nextpt, double t){
+    if(t < 0. || !cur_traectory) return FALSE;
+    coords_t pt;
+    if(!cur_traectory(&pt, t)) return FALSE;
+    pt.msrtime.tv_sec = floor(t);
+    pt.msrtime.tv_usec = (uint32_t) t - pt.msrtime.tv_sec;
+    if(nextpt) *nextpt = pt;
+    if(pt.X < -M_PI2 || pt.X > M_PI2 || pt.Y < -M_PI || pt.Y > M_PI) return FALSE;
+    return TRUE;
+}
+
+// current telescope position according to starting motor coordinates
+// @return FALSE if failed to get current coordinates
+int telpos(coords_t *curpos){
+    mountdata_t mdata;
+    int ntries = 0;
+    for(; ntries < 10; ++ntries){
+        if(MCC_E_OK == Mount.getMountData(&mdata)) break;
+    }
+    if(ntries == 10) return FALSE;
+    coords_t pt;
+    pt.X = XYcor.X + mdata.encposition.X;
+    pt.Y = XYcor.Y + mdata.encposition.Y;
+    pt.msrtime = mdata.encposition.msrtime;
+    if(curpos) *curpos = pt;
+    return TRUE;
+}
+
+// X=X0+1'/s, Y=Y0+15''/s
+int Linear(coords_t *nextpt, double t){
+    coords_t pt;
+    pt.X = XYstart.X + ASEC2RAD(1.) * (t - tstart);
+    pt.Y = XYstart.Y + ASEC2RAD(15.)* (t - tstart);
+    if(nextpt) *nextpt = pt;
+    return TRUE;
+}
+
+// X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)
+int SinCos(coords_t *nextpt, double t){
+    coords_t pt;
+    pt.X = XYstart.X + AMIN2RAD(5.) * sin((t-tstart)/30.*2*M_PI);
+    pt.Y = XYstart.Y + AMIN2RAD(10.)* cos((t-tstart)/200.*2*M_PI);
+    if(nextpt) *nextpt = pt;
+    return TRUE;
+}
+
+typedef struct{
+    traectory_fn *f;
+    const char *name;
+    const char *help;
+} tr_names;
+
+static tr_names names[] = {
+    {Linear, "linear", "X=X0+1'/s, Y=Y0+15''/s"},
+    {SinCos, "sincos", "X=X0+5'*sin(t/30*2pi), Y=Y0+10'*cos(t/200*2pi)"},
+    {NULL, NULL, NULL}
+};
+
+traectory_fn *traectory_by_name(const char *name){
+    traectory_fn *f = NULL;
+    for(int i = 0; ; ++i){
+        if(!names[i].f) break;
+        if(strcmp(names[i].name, name) == 0){
+            f = names[i].f;
+            break;
+        }
+    }
+}
+
+// print all acceptable traectories names with help
+void print_tr_names(){
+    for(int i = 0; ; ++i){
+        if(!names[i].f) break;
+        printf("%s: %s\n", names[i].name, names[i].help);
+    }
+}