Fixes for supporting simulator; switch to mosquitto_loop_start interface

Author: pbosetti

c-cnc23.code-workspace

@@ -13,7 +13,9 @@
       "*.h": "c",
       "*.rmd": "markdown",
       "*.hh": "c++",
-      "__node_handle": "c"
+      "__node_handle": "c",
+      "cmath": "c",
+      "complex": "c"
     }
   },
   "launch": {

machine.ini

@@ -5,10 +5,10 @@ A = 5.0
 max_error = 0.005
 # Sampling time (in seconds!)
 tq = 0.005
-# Machine initial position
-zero = [0, 0, 500]
 # Workpiece origin coordinates
 offset = [400, 400, 200]
+# Machine initial position (in workpiece coordinates!)
+zero = [0, 0, 500]
 # Maximum feed rate in mm/min
 fmax = 10000
 # Tool table
@@ -25,37 +25,37 @@ sub_topic = "c-cnc/status/#"
 
 # SI units!
 [X]
-length = 1           # m
-friction = 1000      #
-mass = 150           # kg
-max_torque = 20      # N m
+length = 1            # m
+friction = 1000       #
+mass = 150            # kg
+max_torque = 20       # N m
 pitch = 0.01          # m/rev
-gravity = 0          # m/s^2
-integration_dt = 5   # microseconds
-p = 30              # PID parameters
-i = 0               # PID parameters
-d = 19              # PID parameters
+gravity = 0           # m/s^2
+integration_dt = 5    # microseconds
+p = 1400              # PID parameters
+i = 0                 # PID parameters
+d = 360               # PID parameters
 
 [Y]
-length = 1           # m
-friction = 1000     #
-mass = 150          # kg
-max_torque = 20    # N m
+length = 1            # m
+friction = 1000       #
+mass = 150            # kg
+max_torque = 20       # N m
 pitch = 0.01          # m/rev
-gravity = 0          # m/s^2
-integration_dt = 5   # microseconds
-p = 700              # PID parameters
-i = 0               # PID parameters
-d = 180              # PID parameters
+gravity = 0           # m/s^2
+integration_dt = 5    # microseconds
+p = 700               # PID parameters
+i = 0                 # PID parameters
+d = 180               # PID parameters
 
 [Z]
-length = 1           # m
-friction = 1000      #
-mass = 100           # kg
-max_torque = 15      # N m
+length = 1            # m
+friction = 1000       #
+mass = 100            # kg
+max_torque = 15       # N m
 pitch = 0.01          # m/rev
-gravity = 0          # m/s^2
-integration_dt = 5   # microseconds
-p = 350              # PID parameters
-i = 0               # PID parameters
-d = 90              # PID parameters
+gravity = 0           # m/s^2
+integration_dt = 5    # microseconds
+p = 350               # PID parameters
+i = 0                 # PID parameters
+d = 90                # PID parameters

src/axis.c

@@ -293,7 +293,8 @@ int main(int argc, char const **argv) {
   axis_run(ax);
   axis_run(ay);
   axis_run(az);
-  a = az;
+  a = ax;
+  fprintf(stderr, "axis %s: p %.3f i %.3f d %.3f\n", a->name, a->p, a->i, a->d);
   while (a->time < atof(argv[1])) {
     if (a->time < 1) {
       axis_set_setpoint(a, 0);

src/fsm.c

@@ -17,6 +17,8 @@ Functions and types have been generated with prefix "ccnc_"
 #include <stdio.h>
 #include <termios.h> // setting terminal attributes
 #include <unistd.h>
+#include <math.h>
+#include <sys/param.h>
 #include "defines.h"
 #include "fsm.h"
 #include "block.h"
@@ -201,6 +203,8 @@ ccnc_state_t ccnc_do_idle(ccnc_state_data_t *data) {
   // 2. Reset times
   data->t_blk = data->t_tot = 0;
 
+  machine_sync(data->machine, 1);
+
   switch (next_state) {
   case CCNC_NO_CHANGE:
   case CCNC_STATE_IDLE:
@@ -425,10 +429,11 @@ ccnc_state_t ccnc_do_rapid_motion(ccnc_state_data_t *data) {
          point_z(pos));
 
   // 5.  print progress percentage
-  fprintf(stderr, "\b\b\b\b\b\b\b\b");
-  fflush(stderr);
-  fprintf(stderr, "[%5.1f%%]",
-          (1 - machine_error(data->machine) / block_length(b)) * 100);
+  // fprintf(stderr, "\b\b\b\b\b\b\b\b");
+  // fflush(stderr);
+  fprintf(stderr, "\r[%5.1f%%]", fabs(
+              1.0 - MIN(machine_error(data->machine) / block_length(b), 1.0)) *
+              100);
 
   // 6. increment times
   data->t_blk += tq;
@@ -573,7 +578,7 @@ void ccnc_begin_rapid(ccnc_state_data_t *data) {
   machine_sync(data->machine, 1);
 
   // 4. Print INITIAL value for progress string (8 chars)
-  fprintf(stderr, "Rapid block length: %f\n", block_length(b));
+  // fprintf(stderr, "Rapid block length: %f\n", block_length(b));
   fprintf(stderr, "[  0.0%%]");
 }
 

src/machine.c

@@ -231,26 +231,18 @@ void machine_print_params(machine_t const *m) {
 
 int machine_connect(machine_t *m, machine_on_message callback) {
   assert(m);
-  int count = 0;
   m->mqt = mosquitto_new(NULL, 1, m);
   if (!m->mqt) {
     perror(BRED"Could not create MQTT"CRESET);
     return EXIT_FAILURE;
   }
   mosquitto_connect_callback_set(m->mqt, on_connect);
   mosquitto_message_callback_set(m->mqt, callback ? callback : on_message);
+  mosquitto_loop_start(m->mqt);
   if (mosquitto_connect(m->mqt, m->broker_address, m->broker_port, 10) != MOSQ_ERR_SUCCESS) {
     perror(BRED"Invalid broker connection parameters"CRESET);
     return EXIT_FAILURE;
   }
-  // wait for the connection to be established
-  while (m->connecting) {
-    wprintf("loop: %d\n", mosquitto_loop(m->mqt, -1, 1));
-    if (++count >= 5) {
-      eprintf("Could not connect to broker\n");
-      return EXIT_FAILURE;
-    }
-  }
   return EXIT_SUCCESS;
 }
 
@@ -269,9 +261,6 @@ int machine_sync(machine_t *m, int rapid) {
     perror(BRED"Could not sent message"CRESET);
     return EXIT_FAILURE;
   }
-  if(mosquitto_loop(m->mqt, 0, 1) != MOSQ_ERR_SUCCESS) {
-    perror(BRED"mosquitto_loop error"CRESET);
-  }
   return EXIT_SUCCESS;
 }
 
@@ -282,7 +271,7 @@ int machine_listen_start(machine_t *m) {
     return EXIT_FAILURE;
   }
   m->error = m->max_error * 10.0;
-  wprintf("Subscribed to topic %s\n", m->sub_topic);
+  // wprintf("Subscribed to topic %s\n", m->sub_topic);
   return EXIT_SUCCESS;
 }
 
@@ -292,16 +281,16 @@ int machine_listen_stop(machine_t *m) {
     perror(BRED"Could not unsubscribe"CRESET);
     return EXIT_FAILURE;
   }
-  wprintf("Unsubscribed from topic %s\n", m->sub_topic);
+  // wprintf("Unsubscribed from topic %s\n", m->sub_topic);
   return EXIT_SUCCESS;
 }
 
 void machine_disconnect(machine_t *m) {
   assert(m && m->mqt);
   while (mosquitto_want_write(m->mqt)) {
-    mosquitto_loop(m->mqt, 0, 1);
     usleep(10000);
   }
+  mosquitto_loop_stop(m->mqt, 1);
   mosquitto_disconnect(m->mqt);
   m->connecting = 1;
 }
@@ -315,7 +304,7 @@ static void on_connect(struct mosquitto *mqt, void *obj, int rc) {
   machine_t *m = (machine_t *)obj;
   // check rc: if CONNACK_ACCEPTED, then the connection succeeded
   if (rc == CONNACK_ACCEPTED) {
-    wprintf("-> Connected to %s:%d\n", m->broker_address, m->broker_port);
+    wprintf("-> Connected to %s:%d\n\r", m->broker_address, m->broker_port);
     // subscribe to topic
     if (mosquitto_subscribe(mqt, NULL, m->sub_topic, 0) != MOSQ_ERR_SUCCESS) {
       perror("Could not subsccribe");

src/main/simulate.c

@@ -29,6 +29,7 @@ typedef struct {
   struct mosquitto *mqtt;
   int connecting;
   int rapid;
+  int program_run;
 } sim_t;
 
 // empty signal handler
@@ -43,7 +44,7 @@ sim_t *sim_new(char *ini_path) {
   sim_t *sim = malloc(sizeof(sim_t));
   memset(sim, 0, sizeof(*sim));
   sim->connecting = 1;
-  sim->rapid = 1;
+  sim->rapid = 0;
 
   // init axes
   sim->ax = axis_new(ini_path, "X");
@@ -140,6 +141,7 @@ static void on_message(struct mosquitto *m, void *obj,
   sim_t *sim = (sim_t *)obj;
   char *substr = NULL;
   data_t x, y, z;
+  sim->program_run = 1;
   if (strcmp(msg->topic, sim->sub_topic) == 0) {
     substr = strchr(msg->payload, 'x');
     x = atof(substr + 3) / 1000.0;
@@ -239,8 +241,13 @@ int main(int argc, char const *argv[]) {
   axis_reset(ax, 0);
   axis_reset(ay, 0);
   axis_reset(az, 0);
+  axis_set_setpoint(ax, 0);
+  axis_set_setpoint(ay, 0);
+  axis_set_setpoint(az, 0);
 
-#define LOG_HEADER "t qx sx x vx qy sy y vy qz sz z vz\n"
+#define LOG_HEADER                                                             \
+  "        t        qx        sx        x         vx        qy        sy     " \
+  "   y         vy        qz        sz        z         vz        d  r\n"
   printf(LOG_HEADER);
   if (logfile) {
     fprintf(logfile, LOG_HEADER);
@@ -262,15 +269,14 @@ int main(int argc, char const *argv[]) {
     y = axis_position(ay) * 1000;
     z = axis_position(az) * 1000;
     delta = sqrt(pow(x - sx, 2) + pow(y - sy, 2) + pow(z - sz, 2));
-    sprintf(payload, "%f", delta);
     printf("\r");
-    printf("%f " RED "%9.3f %9.3f %9.3f %9.3f " GRN
+    printf("%9.4f " RED "%9.3f %9.3f %9.3f %9.3f " GRN
            "%9.3f %9.3f %9.3f %9.3f " BLU "%9.3f %9.3f %9.3f %9.3f" YEL
            " %9.3f" CRESET " %c",
            axis_time(ax), axis_torque(ax), sx, x, axis_speed(ax),
            axis_torque(ay), sy, y, axis_speed(ay), axis_torque(az), sz, z,
            axis_speed(az), delta, sim->rapid ? 'R' : 'I');
-    if (logfile) {
+    if (logfile && sim->program_run) {
       fprintf(logfile,
               "%f %9.3f %9.3f %9.3f %9.3f %9.3f %9.3f %9.3f %9.3f %9.3f %9.3f "
               "%9.3f %9.3f %9.3f %c\n",
@@ -279,7 +285,8 @@ int main(int argc, char const *argv[]) {
               axis_speed(az), delta, sim->rapid ? 'R' : 'I');
     }
     fflush(stdout);
-    if (sim->rapid) {
+    if (sim->rapid && sim->program_run) {
+      sprintf(payload, "%f", delta);
       mosquitto_publish(sim->mqtt, NULL, sim->pub_topic_err, strlen(payload),
                         payload, 0, 0);
       sprintf(payload, "%f,%f,%f", x, y, z);