add motors emulation

BTA_dome_modbus/motors.c

@@ -34,20 +34,50 @@ speed=xx - (sg)
 current=xx - (sg) ÕÓÔÁ×ËÁ ÔÏËÁ
 #endif
 
+static motor_state_t motstates[MOTORS_AMOUNT] = {0};
+
 // set points
-static double currentSet = 0., speedSet = 0.;
+static double currentSet = DEFAULT_CURRENT, speedSet = 0.;
+// flags for main routine
+static union{
+    struct{
+        uint32_t change_speed : 1;
+        uint32_t change_current : 1;
+        uint32_t stop : 1;
+    };
+    uint32_t all;
+} flags = {0};
+
+// emulation mode parameters
+/*static struct{
+    double tlast;
+
+} emulpar = {0};*/
+
 // current motor for status etc. getters (0..MOTORS_AMOUNT-1)
 static int motindex = 0;
 
-// stop all
-void motors_stop(){
-    ;
+// open modbus @ given speed; return FALSE if failed
+static int modbus_open_m(const char _U_ *path, int _U_ speed){
+    return FALSE;
+}
+static int modbus_open_e(const char _U_ *path, int _U_ speed){
+    return TRUE;
 }
 
 // close modbus connection
-void modbus_close(){
+static void modbus_close_m(){
     ;
 }
+static void modbus_close_e(){
+    ;
+}
+
+
+// stop all
+void motors_stop(){
+    flags.stop = 1;
+}
 
 // current setpoint getter
 double motors_get_curntsetpoint(){
@@ -56,7 +86,9 @@ double motors_get_curntsetpoint(){
 // set setpoint of current
 int motors_set_curntsetpoint(double val){
     if(val < 0. || val > MAX_CURRENT) return FALSE;
-    // do something
+    DBG("Change max current to %g", val);
+    currentSet = val;
+    flags.change_current = 1;
     return TRUE;
 }
 
@@ -68,7 +100,9 @@ double motors_get_speedsetpoint(){
 int motors_set_speedsetpoint(double val){
     double absval = fabs(val);
     if(absval > MAX_SPEED) return FALSE;
-    // do something
+    DBG("Change speed setpoint to %g", val);
+    speedSet = val;
+    flags.change_speed = 1;
     return TRUE;
 }
 
@@ -84,18 +118,74 @@ int motors_set_activenum(int N){
 
 // get current value for active motor
 int motors_get_actcurrent(double *val){
-    if(val) *val = 0.;
+    if(val) *val = motstates[motindex].current;
     return TRUE;
 }
 
 // get speed for active motor
 int motors_get_actspeed(double *val){
-    if(val) *val = 0.;
+    if(val) *val = motstates[motindex].speed;
     return TRUE;
 }
 
 // get status for active motor
 int motors_get_actstatus(int *val){
-    if(val) *val = 0;
+    if(val) *val = motstates[motindex].status;
     return TRUE;
 }
+
+// main motors processing routine
+static void motors_process_m(){
+    ;
+}
+static void motors_process_e(){
+    static double t0 = -1., curspeed = 0.;
+    double curt = sl_dtime(), dt = curt - t0, curcurrent = 0.;
+    int curstatus = motstates[0].status;
+    if(t0 < 0.){
+        t0 = curt;
+        // init state ("turn motors on")
+        for(int i = 0; i < MOTORS_AMOUNT; ++i) motstates[i].status = MOT_SLEEP;
+        return;
+    }
+    if(flags.all){
+        if(flags.stop){
+            speedSet = 0.;
+            if(curstatus != MOT_RUN) curstatus = MOT_SLEEP;
+        }
+        flags.all = 0;
+    }
+    if(fabs(curspeed - speedSet) > SPEED_TOLERANCE){ // need to calculate new speed value
+        double acceleration = (curspeed < speedSet) ? EMUL_ACCEL : -EMUL_ACCEL;
+        double newspeed = curspeed + acceleration * dt / 60.; // acceleration in min^{-2}
+        if(acceleration > 0.){
+            if(newspeed > speedSet) newspeed = speedSet;
+        }else{
+            if(newspeed < speedSet) newspeed = speedSet;
+        }
+        curspeed = newspeed;
+        curcurrent = currentSet;
+        curstatus = MOT_RUN;
+        DBG("Now speed = %g", curspeed);
+    }else{
+        if(fabs(curspeed) < SPEED_TOLERANCE) curstatus = MOT_SLEEP; // stopped
+        else curcurrent = currentSet * 0.6;
+    }
+    t0 = curt;
+    for(int i = 0; i < MOTORS_AMOUNT; ++i){
+        motstates[i].status = curstatus;
+        motstates[i].current = curcurrent;
+        motstates[i].speed = curspeed;
+    }
+}
+
+int (*modbus_open)(const char *, int) = modbus_open_m;
+void (*modbus_close)() = modbus_close_m;
+void (*motors_process)() = motors_process_m;
+
+void set_emulation_mode(){
+    LOGMSG("Set emulation mode");
+    modbus_open =  modbus_open_e;
+    modbus_close = modbus_close_e;
+    motors_process = motors_process_e;
+}