Monitor power

firmware/accelstepper/AccelStepper.cpp

@@ -221,10 +221,11 @@ unsigned long AccelStepper::computeNewSpeed()
 // You must call this at least once per step, preferably in your main loop
 // If the motor is in the desired position, the cost is very small
 // returns true if the motor is still running to the target position.
-boolean AccelStepper::run()
+boolean AccelStepper::run(float *speed)
 {
     if (runSpeed())
 	computeNewSpeed();
+    *speed = _speed;
     return _speed != 0.0 || distanceToGo() != 0;
 }
 
@@ -678,7 +679,8 @@ void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3In
 // Blocks until the target position is reached and stopped
 void AccelStepper::runToPosition()
 {
-    while (run())
+    float dummy;
+    while (run(&dummy))
 	YIELD; // Let system housekeeping occur
 }
 

firmware/accelstepper/AccelStepper.h

@@ -421,7 +421,7 @@ class AccelStepper
     /// preferably in your main loop. Note that each call to run() will make at most one step, and then only when a step is due,
     /// based on the current speed and the time since the last step.
     /// \return true if the motor is still running to the target position.
-    boolean run();
+    boolean run(float *speed);
 
     /// Poll the motor and step it if a step is due, implementing a constant
     /// speed as set by the most recent call to setSpeed(). You must call this as

firmware/main.cpp

@@ -8,6 +8,7 @@
 #include <istream>
 
 #include "hardware/gpio.h"
+#include "hardware/structs/bus_ctrl.h"
 #include "pico/binary_info.h"
 #include "pico/multicore.h"
 #include "pico/stdlib.h"
@@ -26,14 +27,15 @@
 
 // #define DEBUG
 #define FIRMWARE_VER "0.1.0"
-#define BOARD_REV "G"
+#define BOARD_REV "H"
 
 #ifdef DEBUG
     uint16_t button_counter = 0;
 #endif
 
 #define MAX_GEAR_RATIO 100
 #define MAX_SERIAL_BUFFER_LENGTH 1024u
+#define STALL_DETECTION_THRESHOLD_RPM 20.0
 
 // ISR flag for capacitative touch detected
 volatile bool alert_flag = false;
@@ -42,6 +44,8 @@ static void io_alert_irq_callback(unsigned int gpio, long unsigned int events)
     alert_flag = true;
 };
 
+volatile float speed_rpm;
+
 // NB: The gear ratio is configurable without recompilation using
 //      `picotool config -s gear_ratio 3.14`
 // or similar.
@@ -52,6 +56,8 @@ struct context_t {
     bool enable = false;
     bool led = true;
     uint8_t last_sensor_input_status = BUTTON_RELEASE;
+    bool stall = false;
+    bool power_good = false;
 };
 
 // Thread-safe queue and command types that are shared state between cores
@@ -126,21 +132,29 @@ static int process_button_touches(context_t *ctx)
                 ctx->led = !ctx->led;
                 rgb_set_auto(ctx->enable, ctx->led);
                 break;
+
+            if (!ctx->power_good) return ERROR_REMOTE_INTERFACE_LOCKED;
+
             case ENABLE_BUTTON_PRESS:
                 ctx->enable = !ctx->enable;
+                if (ctx->enable) {
+                    ctx->stall = false; 
+                    ctx->power_good = true;
+                    rgb_set_breathing(false);
+                }
                 rotor_cmd = {.tag = rotor_cmd_tag::ENABLE, .value = {.enable = ctx->enable}};
                 queue_add_blocking(&rotor_cmd_queue, &rotor_cmd);
                 rgb_set_auto(ctx->enable, ctx->led);
                 break;
             case CW_BUTTON_PRESS:
                 rotor_cmd = {.tag = rotor_cmd_tag::STOP};
                 queue_add_blocking(&rotor_cmd_queue, &rotor_cmd);
-                rc = queue_add_turn_cmd_blocking(ctx, -100);
+                rc = queue_add_turn_cmd_blocking(ctx, -INFINITY);
                 break;
             case CCW_BUTTON_PRESS:
                 rotor_cmd = {.tag = rotor_cmd_tag::STOP};
                 queue_add_blocking(&rotor_cmd_queue, &rotor_cmd);
-                rc = queue_add_turn_cmd_blocking(ctx, 100);
+                rc = queue_add_turn_cmd_blocking(ctx, INFINITY);
                 break;
             case BUTTON_RELEASE:
                 if (ctx->last_sensor_input_status & (CW_BUTTON_PRESS | CCW_BUTTON_PRESS))
@@ -188,7 +202,8 @@ static int build_serial_buffer(char *serial_buffer)
 static int process_serial_commands(context_t *ctx)
 {
     static char serial_buffer[MAX_SERIAL_BUFFER_LENGTH] = {0};
-
+    if (!ctx->power_good) return ERROR_REMOTE_INTERFACE_LOCKED;
+
     int rc = build_serial_buffer(serial_buffer);
     if (rc){ return rc; }
 
@@ -212,6 +227,11 @@ static int process_serial_commands(context_t *ctx)
     if (receive["enable"].is<bool>())
     {
         ctx->enable = receive["enable"];
+        if (ctx->enable) {
+            ctx->stall = false;
+            ctx->power_good = true;
+            rgb_set_breathing(false);
+        }
         rotor_cmd = {.tag = rotor_cmd_tag::ENABLE, .value = {.enable = ctx->enable}};
         queue_add_blocking(&rotor_cmd_queue, &rotor_cmd);
         rgb_set_auto(ctx->enable, ctx->led);
@@ -246,9 +266,10 @@ static int process_serial_commands(context_t *ctx)
         doc["board_rev"] = BOARD_REV;
         doc["firmware"] = FIRMWARE_VER;
         doc["enable"] = ctx->enable;
+        doc["stall"] = ctx->stall;
         doc["led"] = ctx->led;
         doc["charge_current"] = ltc4425_charge_current();
-        doc["power_good"] = ltc4425_power_good();
+        doc["power_good"] = ctx->power_good;
         serializeJson(doc, std::cout);
         std::cout << std::endl;
     }
@@ -278,19 +299,23 @@ static void core1_entry()
                     rotor_enable(&rotor, rotor_cmd.value.enable);
                     break;
                 case rotor_cmd_tag::TURN:
+                    rotor.motor.setAcceleration(MAX_ACCEL_SPSS(rotor.gear_ratio));
                     rotor_move(&rotor, rotor_cmd.value.turns);
                     break;
                 case rotor_cmd_tag::STOP:
-                    rotor_stop_and_reset(&rotor);
+                    rotor.motor.setAcceleration(MAX_ACCEL_SPSS(rotor.gear_ratio)*2);
+                    rotor.motor.stop();
                     break;
             }
         }
 
         // If the motor has completed its motion and is stopped, reset the internal position counter
-        if (!rotor.motor.run())
+        float speed_sps;
+        if (!rotor.motor.run(&speed_sps))
         {
             rotor_stop_and_reset(&rotor);
         }
+        speed_rpm = speed_sps / DETENTS / 16.0 * 60.0;
     }
 }
 
@@ -320,17 +345,22 @@ int main()
     rgb_set_breathing(true);
     ltc4425_init();
     cap1296_init();
-    while (!ltc4425_power_good()) { tight_loop_contents(); } // Wait for super caps to charge
+    while (!(ctx.power_good = ltc4425_power_good())) { tight_loop_contents(); } // Wait for super caps to charge
     rgb_set_breathing(false);
     rgb_set_auto(ctx.enable, ctx.led);
 
+    // give core1 priority on bus  if there's ever a contention (i.e. if core
+    // tries to write to speed_rpm while core0 tries to read speed_rpm)
+    bus_ctrl_hw->priority = BUSCTRL_BUS_PRIORITY_PROC1_BITS;
     // Launch motor driver loop in core1
     multicore_launch_core1(core1_entry);
 
     // Enable button alert ISR and clear any button touches registered during initialization
     gpio_set_irq_enabled_with_callback(CAP1296_ALERT, GPIO_IRQ_EDGE_FALL, true, &io_alert_irq_callback);
     cap1296_clear_int_bit_in_main_control_register();
 
+    bool power_good_prev;
+
     // Decode commands and buttons
     while (true)
     {
@@ -346,6 +376,32 @@ int main()
 #endif
         }
 
+        uint32_t motor_status = tmc2130_status();
+        if (!(motor_status & (1 << 31)) && (motor_status & (1 << 24)) && (abs(speed_rpm) > STALL_DETECTION_THRESHOLD_RPM)) {
+#ifdef DEBUG
+            printf("stall: speed=%.2f sg_result=%d\n", speed_rpm, motor_status & 0x3FF);
+#endif
+            ctx.enable = false;
+            ctx.stall = true;
+            rotor_cmd_t rotor_cmd = {.tag = rotor_cmd_tag::ENABLE, .value = {.enable = ctx.enable}};
+            queue_add_blocking(&rotor_cmd_queue, &rotor_cmd);
+            rgb_set_auto(ctx.enable, ctx.led);
+        }
+
+        if (!ltc4425_power_good()) {
+            if (power_good_prev) {
+                ctx.power_good = false;
+                ctx.enable = false;
+                rotor_cmd_t rotor_cmd = {.tag = rotor_cmd_tag::ENABLE, .value = {.enable = ctx.enable}};
+                queue_add_blocking(&rotor_cmd_queue, &rotor_cmd);
+                rgb_set_auto(ctx.enable, ctx.led);
+                rgb_set_breathing(true);
+            }
+        }
+        else if (!power_good_prev) rgb_set_breathing(false);
+
+        power_good_prev = ctx.power_good;
+
 #ifdef DEBUG
         printf("%f\n", ltc4425_charge_current());
 #endif

firmware/rotor.cpp

@@ -3,7 +3,6 @@
 #include <math.h>
 #include "AccelStepper.h"
 
-
 void rotor_enable(rotor_t *rotor, bool enable)
 {
     if (!enable)
@@ -26,10 +25,18 @@ void rotor_init(rotor_t *rotor)
 
 int rotor_move(rotor_t *rotor, double turns)
 {
-    if (turns >= MAX_TURNS(rotor->gear_ratio))
+    if (std::isinf(turns))
+    {
+        double dir = std::signbit(turns) ? -1.0 : 1.0;
+        rotor->target_position = (rotor->motor.currentPosition() / (double)USTEPS_PER_REV / rotor->gear_ratio) + (dir * 100.0);
+    }
+
+    else if (abs(turns) >= MAX_TURNS(rotor->gear_ratio))
         return -1;
 
-    rotor->target_position += turns;
+    else 
+        rotor->target_position += turns;
+
     long target_position_steps = lround(rotor->target_position * (double)USTEPS_PER_REV * rotor->gear_ratio);
     rotor->motor.moveTo(target_position_steps);
 

firmware/tmc2130.cpp

@@ -52,14 +52,17 @@ void tmc2130_init()
     gpio_put(TMC2130_DIR, 0);
     gpio_put(TMC2130_STEP, 0);
 
-
-    // Copied from page 84
-    //tmc2130_write(REG_CHOPCONF, 0x05008008UL);
-    tmc2130_write(REG_CHOPCONF, 0x050100C3); // TOFF=3, HSTRT=4, HEND=1, CHM=0 (SpreadCycle), TBL=2, VSENSE=0, MRES=8, INTPOL=0
-    tmc2130_write(REG_IHOLD_IRUN, 0x00061313); // IHOLD = 0x13 (275 mA), IRUN = 0x13 (275 mA RMS)
+    tmc2130_write(REG_CHOPCONF, 0x050300C3); // TOFF=3, HSTRT=4, HEND=1, CHM=0 (SpreadCycle), TBL=2, VSENSE=1, MRES=8, INTPOL=0
+    tmc2130_write(REG_IHOLD_IRUN, 0x00061F1F); 
+    // IHOLD = 0x1F (1.37 W measured from the output of 12V regulator, Rev H)
+    // IRUN = 0x1F (1.08 W measured from the output of the 12V regulator, Rev H)
+    // these values provide margin for 12v regulator inefficiency (~80% worst case) and surprise current spikes
     tmc2130_write(REG_TPOWERDOWN, 0x0000000A);
     tmc2130_write(REG_GCONF, 0x00000004);
     tmc2130_write(REG_PWMCONF, 0x000401C8);
+    tmc2130_write(REG_COOLCONF, 0x000B0000);
+    tmc2130_write(REG_TCOOLTHRS, 0x1500);
+    tmc2130_write(REG_TPWMTHRS, 100);
 
     // Start in disabled state
     tmc2130_enable(false);

firmware/tmc2130.h

@@ -11,6 +11,7 @@
 #define REG_TPOWERDOWN          0x11
 #define REG_TSTEP               0x12
 #define REG_TPWMTHRS            0x13
+#define REG_TCOOLTHRS           0x14
 #define REG_CHOPCONF            0x6C
 #define REG_COOLCONF            0x6D
 #define REG_DCCTRL              0x6E