- Add hall sensors for dock/pick

- Add dock movement keys
- Implement support for dock position calibration
- Widen display for XL Y axis w/ docks

.vscode/launch.json

@@ -29,9 +29,9 @@
             "type": "cppdbg",
             "request": "launch",
             "program": "${workspaceFolder}/build/buddy-softmmu/qemu-system-buddy",
-            "args": ["--machine", "prusa-mini", "-kernel", "mini_debug_noboot.bin"],
+            "args": ["-machine", "prusa-xl-040", "-kernel", "minifw.bin"],
             "stopAtEntry": false,
-            "cwd": "${workspaceFolder}/build/buddy-softmmu/",
+            "cwd": "${workspaceFolder}/out/",
             "environment": [],
             "externalConsole": false,
             "MIMode": "gdb",

hw/arm/prusa/parts/2d-dashboard.c

@@ -38,7 +38,7 @@
 
 // 3 extra lines for fan, therm, indicators.
 #define DPY_MAX_ROWS (LINE_HEIGHT *(N_MOTORS + 4U))
-#define DPY_MAX_COLS 480
+#define DPY_MAX_COLS 550
 #define LED_HT LINE_HEIGHT
 
 #define LED_W DPY_MAX_COLS/N_LEDS
@@ -163,7 +163,7 @@ static void dashboard_2d_update_display(void *opaque)
             snprintf(pos, sizeof(pos),"%8.3f%c", m[i]->current_pos, m[i]->status.dir? '<' : '>');
             for (int j=0; j<9; j++)
             {
-                vga_putcharxy(s->con, (60-9)+j, i+1, pos[j], attr_norm);
+                vga_putcharxy(s->con, (68-9)+j, i+1, pos[j], attr_norm);
             }
         }
     }

hw/arm/prusa/parts/corexy_helper.c

@@ -26,13 +26,35 @@
 #include "hw/irq.h"
 #include "qom/object.h"
 #include "../utility/p404_motor_if.h"
+#include "../utility/p404_keyclient.h"
 #include "hw/sysbus.h"
 #include "hw/qdev-properties.h"
 
 #define TYPE_COREXY "corexy-helper"
 
 OBJECT_DECLARE_SIMPLE_TYPE(CoreXYState, COREXY)
 
+typedef struct dock_pos
+{
+    unsigned char key;
+    int32_t x;
+    int32_t y;
+} dock_pos;
+
+// Coordinates need to be "offset" by the amount the axes are allowed
+// to move in the negative direction.
+#define Y_NEG_SIZE 10
+#define X_NEG_SIZE 8
+
+// Per comments in code, x is ~25+*n*82, y is ~451
+static const dock_pos dock_positions[] = {
+    {'z', (X_NEG_SIZE + 25+(0*82)),    455 + Y_NEG_SIZE},
+    {'x', (X_NEG_SIZE + 25+(1*82)),    455 + Y_NEG_SIZE},
+    {'c', (X_NEG_SIZE + 25+(2*82)),    455 + Y_NEG_SIZE},
+    {'v', (X_NEG_SIZE + 25+(3*82)),    455 + Y_NEG_SIZE},
+    {'b', (X_NEG_SIZE + 25+(4*82)),    455 + Y_NEG_SIZE}
+};
+
 struct CoreXYState {
     SysBusDevice parent_obj;
     /*< private >*/
@@ -47,14 +69,29 @@ struct CoreXYState {
 
 	bool irq_state;
     qemu_irq endstop[2];
+    qemu_irq pos_change[2];
+    qemu_irq tool_pick_ctl[ARRAY_SIZE(dock_positions)];
 
 	p404_motorif_status_t vis_x;
 	p404_motorif_status_t vis_y;
 	p404_motorif_status_t* vis;
 
+    uint8_t tool_states[ARRAY_SIZE(dock_positions)];
+
+};
+
+#define MAG_SENSE_DISTANCE_MM 1
+
+// These values should be consistent with the 
+// tool mag sensor states, i.e. docked = p0 set, picked = p1 set, in-a-change = both set.
+enum TOOL_STATES {
+    TOOL_LOST = 0,
+    TOOL_PARKED = 1,
+    TOOL_PICKED = 2,
+    TOOL_CHANGING = 3,
 };
 
-OBJECT_DEFINE_TYPE_SIMPLE_WITH_INTERFACES(CoreXYState, corexy, COREXY, SYS_BUS_DEVICE, {TYPE_P404_MOTOR_IF}, {NULL});
+OBJECT_DEFINE_TYPE_SIMPLE_WITH_INTERFACES(CoreXYState, corexy, COREXY, SYS_BUS_DEVICE, {TYPE_P404_MOTOR_IF}, {TYPE_P404_KEYCLIENT}, {NULL});
 
 static void corexy_finalize(Object *obj)
 {
@@ -67,14 +104,66 @@ static void corexy_reset(DeviceState *dev)
     qemu_set_irq(s->endstop[1],0);
 }
 
+static void update_tool_states(CoreXYState *s)
+{
+    // No picking/parking can happen if the tool doesn't travel far enough to the rear.
+    if (s->vis_y.current_pos < 445)
+    {
+        return;
+    }
+
+    // First, check if the tool head is within 1mm of a parked tool position, and if so, it means both the dock 
+    // and carriage sensors must be active.
+    for(int i = 0; i < ARRAY_SIZE(dock_positions); i++)
+    {
+        bool x_in_range = abs(s->vis_x.current_pos - dock_positions[i].x) <= MAG_SENSE_DISTANCE_MM;
+        bool y_in_range = abs(s->vis_y.current_pos - dock_positions[i].y) <= MAG_SENSE_DISTANCE_MM;
+        if (y_in_range && x_in_range)
+        {
+            if (s->tool_states[i] != TOOL_CHANGING)
+            {
+                for (int j = 0; j < ARRAY_SIZE(dock_positions); j++)
+                {
+                    // all other tools must be parked...
+                    s->tool_states[j] = j == i ? TOOL_CHANGING : TOOL_PARKED;
+                    qemu_set_irq(s->tool_pick_ctl[j], s->tool_states[j]);
+                    printf("Tool %u state: %u\n", j, s->tool_states[j]);
+                }
+            }
+        }
+        // If the tool is in the changing state (picked and parked and we have moved out of the "sense zone",
+        // decide new state based on whether:
+        // 1. X has remained the same: carriage moved in -Y and the tool has been unloaded.
+        // 2. Y has remained steady: carriage has moved in -X and tool has been picked up
+        // This is stupidly simple, but it should work and results in simpler logic
+        // than trying to interpret the direction the head is moving.
+        else if (s->tool_states[i] == TOOL_CHANGING)
+        {
+            if (x_in_range)
+            {
+                printf("Tool %u state: parked\n", i);
+                s->tool_states[i] = TOOL_PARKED;
+                qemu_set_irq(s->tool_pick_ctl[i], TOOL_PARKED);
+            }
+            else if (y_in_range)
+            {
+                printf("Tool %u state: picked\n", i);
+                s->tool_states[i] = TOOL_PICKED;
+                qemu_set_irq(s->tool_pick_ctl[i], TOOL_PICKED);
+            }
+        }
+    }
+
+}
+
 static void corexy_move(void *opaque, int n, int level)
 {
     CoreXYState *s = COREXY(opaque);
     if (n == 0)
 	{
 		s->pos_a_um = level;
 	}
-	else
+	else if (n == 1)
 	{
 		s->pos_b_um = level;
 	}
@@ -86,10 +175,22 @@ static void corexy_move(void *opaque, int n, int level)
         xpos = ypos;
         ypos = tmp;
     }
-	s->vis_x.current_pos = (float)xpos/1000.f;
-	s->vis_y.current_pos = (float)ypos/1000.f;
-	s->vis_x.status.stalled = xpos > s->x_max_um || xpos < 0;
-	s->vis_y.status.stalled = ypos < 0 || ypos > s->y_max_um;
+    float newx = ((float)xpos/1000.f);    
+    float newy = ((float)ypos/1000.f);
+
+    float delta_x = newx - s->vis_x.current_pos;
+    float delta_y = newy - s->vis_y.current_pos;
+
+	s->vis_x.current_pos = newx;
+	s->vis_y.current_pos = newy;
+
+    update_tool_states(s);
+
+	s->vis_x.status.stalled = (xpos > s->x_max_um && delta_x > 0) || 
+                                (xpos < 0 && delta_x < 0);
+    s->vis_y.status.stalled = (ypos > s->y_max_um && delta_y > 0) ||
+                                (ypos < 0 && delta_y < 0);
+
 	bool hit = ( s->vis_x.status.stalled || s->vis_y.status.stalled );
 
 	if (hit ^ s->irq_state || hit)
@@ -103,6 +204,31 @@ static void corexy_move(void *opaque, int n, int level)
 	s->vis_y.status.changed = true;
 }
 
+static void corexy_handle_key(P404KeyIF *opaque, Key keycode)
+{
+    CoreXYState *s = COREXY(opaque);
+    for (int i = 0; i < ARRAY_SIZE(dock_positions); i++)
+    {
+        if (dock_positions[i].key == keycode)
+        {
+            printf("Key %c - dock %u\n", keycode, i);
+            // Need to reverse the X/Y calculations to determine A/B absolute positions.
+            float pos_a = (dock_positions[i].x + dock_positions[i].y);
+            float pos_b = (dock_positions[i].x - dock_positions[i].y);
+            if (s->swap_calc)
+            {
+                float tmp = pos_a;
+                pos_a = pos_b;
+                pos_b = tmp;
+            }
+
+            qemu_set_irq(s->pos_change[0], pos_a*1000);
+            qemu_set_irq(s->pos_change[1], pos_b*1000);
+            break;
+        }
+    }
+}
+
 static const p404_motorif_status_t* corexy_get_status(P404MotorIF* p)
 {
     CoreXYState *s = COREXY(p);
@@ -120,6 +246,11 @@ static const p404_motorif_status_t* corexy_get_status(P404MotorIF* p)
 static void corexy_realize(DeviceState *dev, Error **errp)
 {
     CoreXYState *s = COREXY(dev);
+    for (int i = 0; i < ARRAY_SIZE(dock_positions); i++)
+    {
+        s->tool_states[i] = TOOL_PARKED;
+        qemu_set_irq(s->tool_pick_ctl[i], TOOL_PARKED);
+    }
 	s->vis_x.max_pos = s->x_max_um/(1000U);
 	s->vis_x.status.changed = true;
 	s->vis_y.max_pos = s->y_max_um/(1000U);
@@ -138,7 +269,17 @@ static void corexy_init(Object *obj)
 	s->vis_y.status.enabled = true;
 	s->vis_y.status.changed = true;
     qdev_init_gpio_out(DEVICE(obj), s->endstop, 2);
+    qdev_init_gpio_out_named(DEVICE(obj), s->pos_change, "motor-move", 2);
+    qdev_init_gpio_out_named(DEVICE(obj), s->tool_pick_ctl, "tool-pick", ARRAY_SIZE(dock_positions));
     qdev_init_gpio_in(DEVICE(obj),corexy_move,2);
+
+    p404_key_handle pKey = p404_new_keyhandler(P404_KEYCLIENT(obj));
+    p404_register_keyhandler(pKey, 'z',"Places head at dock 1");
+    p404_register_keyhandler(pKey, 'x',"Places head at dock 2");
+    p404_register_keyhandler(pKey, 'c',"Places head at dock 3");
+    p404_register_keyhandler(pKey, 'v',"Places head at dock 4");
+    p404_register_keyhandler(pKey, 'b',"Places head at dock 5");
+
 }
 
 static const VMStateDescription vmstate_corexy = {
@@ -155,7 +296,7 @@ static const VMStateDescription vmstate_corexy = {
 
 static Property corexy_properties[] = {
     DEFINE_PROP_UINT32("x-max-um", CoreXYState, x_max_um, 365*1000),
-    DEFINE_PROP_UINT32("y-max-um", CoreXYState, y_max_um, 365*1000),
+    DEFINE_PROP_UINT32("y-max-um", CoreXYState, y_max_um, 466*1000),
     DEFINE_PROP_BOOL("swap-calc", CoreXYState, swap_calc, false),
     DEFINE_PROP_END_OF_LIST(),
 };
@@ -170,4 +311,7 @@ static void corexy_class_init(ObjectClass *oc, void *data)
 
 	P404MotorIFClass *mc = P404_MOTOR_IF_CLASS(oc);
     mc->get_current_status = corexy_get_status;
+
+    P404KeyIFClass *kc = P404_KEYCLIENT_CLASS(oc);
+    kc->KeyHandler = corexy_handle_key;
 }

hw/arm/prusa/parts/tmc2130.c

@@ -444,6 +444,19 @@ static const p404_motorif_status_t* tmc2130_get_status(P404MotorIF* p)
     return &s->vis;
 }
 
+static void tmc2130_um_in(void *opaque, int n, int level)
+{
+    tmc2130_state *s = TMC2130(opaque);
+    float pos = ((float)level)/1000.f;
+    printf("Motor %c position change %f -> %f\n",s->id,s->current_position,pos);
+    s->current_position = pos;
+    s->current_step = pos*(float)(s->max_steps_per_mm);
+    s->vis.current_pos = pos;
+    s->vis.status.changed = true;
+    qemu_set_irq(s->position_out, s->current_step);
+    qemu_set_irq(s->um_out, s->current_position*1000.f);
+}
+
 static void tmc2130_finalize(Object *obj){
 }
 
@@ -484,6 +497,7 @@ static void tmc2130_init(Object *obj){
     qdev_init_gpio_in_named( DEVICE(obj),tmc2130_step, "step",1);
     qdev_init_gpio_in_named( DEVICE(obj),tmc2130_enable, "enable",1);
     qdev_init_gpio_in_named( DEVICE(obj),tmc2130_ext_stall, "ext-stall",1);
+    qdev_init_gpio_in_named( DEVICE(obj),tmc2130_um_in, "um-in", 1);
     qdev_init_gpio_out_named(DEVICE(obj),&s->irq_diag, "diag", 1);
 	qdev_init_gpio_out_named(DEVICE(obj),&s->stall_indicator, "stall-indicator", 1);
     qdev_init_gpio_out_named(DEVICE(obj),&s->hard_out, "hard", 1);

hw/arm/prusa/parts/xl_bridge.c

@@ -66,12 +66,13 @@ typedef union gpio_state_t{
 		uint8_t nAC    :1;
 		uint8_t reset  :1;
 		uint8_t z_um   :1;
-		uint8_t _reserved :3;
+        uint8_t pick_state :1;
+		uint8_t pick_p0 :1;
+		uint8_t pick_p1 :1;
 	} bits;
 	uint8_t byte;
 } gpio_state_t;
 
-
 struct XLBridgeState {
     SysBusDevice parent_obj;
     /*< private >*/
@@ -282,6 +283,13 @@ static void xl_bridge_gpio_receive(void *opaque, const uint8_t *buf, int size)
 				s->data_remaining = 4; // read 4 more bytes to get actual position.
 			}
 		}
+        if (state.bits.pick_state)
+        {
+            printf("Puppy %s pick state %u %u\n", shm_names[s->id], state.bits.pick_p0, state.bits.pick_p1);
+            qemu_set_irq(s->gpio_out[XLBRIDGE_PIN_E_P0_PARKED],state.bits.pick_p0);
+            qemu_set_irq(s->gpio_out[XLBRIDGE_PIN_E_P1_PICKED],state.bits.pick_p1);
+            return;
+        }
 		PROCESS_BIT(XLBRIDGE_PIN_E_DIR, e_dir);
 		PROCESS_BIT(XLBRIDGE_PIN_E_STEP, e_step);
 		PROCESS_BIT(XLBRIDGE_PIN_nAC_FAULT, nAC);
@@ -312,6 +320,17 @@ static void xl_bridge_reset_in(void *opaque, int n, int level)
 	// if (level) printf("Sent reset to %02x, %02x\n", n, s->gpio_states[n].byte);//, shm_names[target]);
 }
 
+static void xl_bridge_pick_in(void *opaque, int n, int level)
+{
+	XLBridgeState *s = XLBRIDGE(opaque);
+    // N is tool number, level is pick state bitmask.
+    gpio_state_t state;
+    state.bits.pick_state = 1;
+    state.bits.pick_p0 = (level & 1) > 0;
+    state.bits.pick_p1 = (level & 2) > 0;
+	qemu_chr_fe_write_all(&s->gpio[XL_DEV_T0 + n],&state.byte, 1);
+}
+
 static void xl_bridge_gpio_in(void *opaque, int n, int level)
 {
 	XLBridgeState *s = XLBRIDGE(opaque);
@@ -430,6 +449,9 @@ static void xl_bridge_realize(DeviceState *dev, Error **errp)
 		gchar* io_name = g_strdup_printf("%s-io",shm_names[s->id]);
 		Chardev* d2=qemu_chr_find(io_name);
 		g_free(io_name);
+
+        // Set the default sensor state to parked.
+        qemu_set_irq(s->gpio_out[XLBRIDGE_PIN_E_P0_PARKED], 1);
 		// TODO - just create the sockets directly with options here rather than expect the user to get it right.
 		if (d)
 		{
@@ -505,6 +527,7 @@ static void xl_bridge_init(Object *obj)
 	qdev_init_gpio_in_named(dev, xl_bridge_tx_assert, "tx-assert", 1);
 	qdev_init_gpio_out_named(dev, s->byte_receive, "byte-receive", XLBRIDGE_UART_COUNT);
 
+	qdev_init_gpio_in_named(dev, xl_bridge_pick_in, "pick-in", 6);
 	qdev_init_gpio_in_named(dev, xl_bridge_gpio_in, "gpio-in",XLBRIDGE_PIN_COUNT);
 	qdev_init_gpio_in_named(dev, xl_bridge_reset_in, "reset-in", XL_BRIDGE_COUNT);
 	qdev_init_gpio_out_named(dev, s->gpio_out, "gpio-out",XLBRIDGE_PIN_COUNT);

hw/arm/prusa/parts/xl_bridge.h

@@ -49,7 +49,10 @@ enum {
 	XLBRIDGE_PIN_RESET,
 	XLBRIDGE_PIN_Z_UM,
     XLBRIDGE_PIN_ESP_GPIO0,
-	XLBRIDGE_PIN_COUNT
+    XLBRIDGE_PIN_PICK_STATE,
+    XLBRIDGE_PIN_E_P0_PARKED,
+    XLBRIDGE_PIN_E_P1_PICKED,
+	XLBRIDGE_PIN_COUNT,
 };
 
 enum {