nxt

LibSidServo/main.c

@@ -21,6 +21,7 @@
 
 #include "dbg.h"
 #include "serial.h"
+#include "ssii.h"
 
 conf_t Conf = {0};
 
@@ -29,6 +30,7 @@ conf_t Conf = {0};
  */
 static void quit(){
     DBG("Close serial devices");
+    for(int i = 0; i < 10; ++i) if(SSemergStop()) break;
     closeSerial();
     DBG("Exit");
 }
@@ -39,6 +41,7 @@ static void quit(){
  * @return error code
  */
 static mcc_errcodes_t init(conf_t *c){
+    FNAME();
     if(!c) return MCC_E_BADFORMAT;
     Conf = *c;
     mcc_errcodes_t ret = MCC_E_OK;
@@ -58,17 +61,39 @@ static mcc_errcodes_t init(conf_t *c){
             ret = MCC_E_ENCODERDEV;
         }
     }
-    if(Conf.MountReqInterval < 1 || Conf.MountReqInterval > 1000000){
+    if(Conf.MountReqInterval > 1. || Conf.MountReqInterval < 0.001){
         DBG("Bad value of MountReqInterval");
-        retr = MCC_E_FATAL;
+        ret = MCC_E_BADFORMAT;
     }
     if(ret != MCC_E_OK) quit();
     return ret;
 }
 
+/**
+ * @brief move2 - simple move to given point and stop
+ * @param X - new X coordinate (radians: -pi..pi)
+ * @param Y - new Y coordinate (radians: -pi..pi)
+ * @return error code
+ */
+static mcc_errcodes_t move2(double X, double Y){
+    if(X > M_PI || X < -M_PI || Y > M_PI || Y < -M_PI){
+        DBG("Wrong coords: X=%g, Y=%g", X, Y);
+        return MCC_E_BADFORMAT;
+    }
+    if(!SSXmoveto(X) || !SSYmoveto(Y)) return MCC_E_FAILED;
+    return MCC_E_OK;
+}
+
+static mcc_errcodes_t emstop(){
+    if(!SSemergStop()) return MCC_E_FAILED;
+    return MCC_E_OK;
+}
+
 // init mount class
 mount_t Mount = {
     .init = init,
     .quit = quit,
-    .getMountData = getMD
+    .getMountData = getMD,
+    .moveTo = move2,
+    .emergStop = emstop,
 };