Additional XL improvements

.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/launch-iX.sh

@@ -0,0 +1,16 @@
+#!/bin/sh
+# This script is an EXAMPLE and specific to my development setup. 
+# I intend it only as a basic reference on how to bring up all the XL components together
+# in the correct sequence.
+# You will want to copy it to your binary output folder and adapt it to your needs. 
+# Note - closing the bed will cause all other instances to be killed too.
+MODBED="070"
+echo "Launching iX w/ bed v ${MODBED}"
+MAIN_FW="firmware.bin"
+MODBED_BL="newbl/bootloader-v296-prusa_modular_bed-1.0.elf"
+USBARGS="-drive id=usbstick,file=fat:rw:sd2 -device usb-storage,drive=usbstick"
+rm bed.log
+clear && ./qemu-system-buddy -machine prusa-iX-027c -kernel ${MAIN_FW} ${USBARGS} -chardev stdio,id=stm32_itm  -icount 1 -S -s & sleep 1 &&
+screen -L -Logfile bed.log -SDm bed ./qemu-system-buddy -machine prusa-ix-bed-${MODBED} -kernel ${MODBED_BL} -icount 5 &\
+read -p "Press any key to quit..." -n1 -e
+killall qemu-system-buddy

hw/arm/prusa/launch-xl.sh

@@ -24,13 +24,19 @@ esac
 MAIN_FW="public.bin"
 MODBED_BL="newbl/bootloader-v296-prusa_modular_bed-1.0.elf"
 DWARF_BL="newbl/bootloader-v296-prusa_dwarf-1.0.elf"
-ESP_SERIAL="-chardev serial,id=stm32uart7,path=/dev/ttyUSB0"
-# ESP_SERIAL="-chardev pty,id=stm32uart7"
+#ESP_SERIAL="-chardev serial,id=stm32uart7,path=/dev/ttyUSB1"
+#ESP_SERIAL="-chardev socket,id=stm32uart7,path=/tmp/esp"
+#ESP_SERIAL="-chardev pty,id=stm32uart7"
+rm bed.log
+rm e0.log
+rm esp32.log
 clear && ./qemu-system-buddy -machine prusa-xl-${XL} -kernel ${MAIN_FW} ${ESP_SERIAL} -chardev stdio,id=stm32_itm -drive id=usbstick,file=fat:rw:sd2 -device usb-storage,drive=usbstick -icount 1 -S -s & sleep 1 &&
 # screen -SDm dwarf5 ./qemu-system-buddy -machine prusa-xl-extruder-g0-4 -kernel newbl/bootloader-v296-prusa_dwarf-1.0.elf -icount 5 &
 # screen -SDm dwarf4 ./qemu-system-buddy -machine prusa-xl-extruder-g0-3 -kernel newbl/bootloader-v296-prusa_dwarf-1.0.elf -icount 5 &
 # screen -SDm dwarf3 ./qemu-system-buddy -machine prusa-xl-extruder-g0-2 -kernel newbl/bootloader-v296-prusa_dwarf-1.0.elf -icount 5 &
 # screen -SDm dwarf2 ./qemu-system-buddy -machine prusa-xl-extruder-g0-1 -kernel newbl/bootloader-v296-prusa_dwarf-1.0.elf -icount 5 &
-screen -SDm dwarf  ./qemu-system-buddy -machine prusa-xl-extruder-${DWARF}-0 -kernel ${DWARF_BL} -icount 2 &
-sleep 2 && screen -SDm modbed ./qemu-system-buddy -machine prusa-xl-bed-${MODBED} -kernel ${MODBED_BL} -icount 6
+screen -L -Logfile e0.log -SDm dwarf  ./qemu-system-buddy -machine prusa-xl-extruder-${DWARF}-0 -kernel ${DWARF_BL} -icount 5 &
+sleep 2 && screen -L -Logfile bed.log -SDm bed ./qemu-system-buddy -machine prusa-xl-bed-${MODBED} -kernel ${MODBED_BL} -icount 4 \
+& sleep 1 && screen -L -Logfile esp32.log -SDm esp32 ./qemu-system-xtensa -machine prusa-xl-esp32
 killall qemu-system-buddy
+killall qemu-system-xtensa

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/CS30BL.c

@@ -37,6 +37,7 @@ struct CS30BLState {
     uint32_t gain;
     float current;
     uint32_t current_x1000;
+    uint32_t fullscale_mA;
 
     qemu_irq irq;
 };
@@ -52,6 +53,15 @@ static void cs30bl_update_irqs(CS30BLState *s) {
     qemu_set_irq(s->irq,ma_sense);
 }
 
+static void cs30bl_set_pwm(void *opaque, int n, int level)
+{
+    CS30BLState *s = CS30BL(opaque);
+    // Calculate PWM-scaled current value and output it.
+    s->current = (((float)level/255.f) * s->fullscale_mA)/1000.f;
+    // printf("New current: %f\n",s->current);
+    cs30bl_update_irqs(s);
+}
+
 static void cs30bl_read_request(void *opaque, int n, int level)
 {
     CS30BLState *s = CS30BL(opaque);
@@ -94,6 +104,7 @@ static void cs30bl_init(Object *obj)
 
     qdev_init_gpio_out_named(DEVICE(obj), &s->irq, "a_sense", 1);
     qdev_init_gpio_in_named(DEVICE(obj),cs30bl_read_request,"adc_read_request",1);
+    qdev_init_gpio_in_named(DEVICE(obj),cs30bl_set_pwm,"pwm-in",1);
 
     script_handle pScript = script_instance_new(P404_SCRIPTABLE(obj), TYPE_CS30BL);
     script_register_action(pScript, "SetA","Sets the voltage readout to a given value.",ActSetA);
@@ -106,6 +117,7 @@ static Property cs30bl_properties[] = {
     DEFINE_PROP_UINT32("mA", CS30BLState, start_current,0),
     DEFINE_PROP_UINT32("mOhmR", CS30BLState, rcsense,22),
     DEFINE_PROP_UINT32("gain", CS30BLState, gain,50),
+    DEFINE_PROP_UINT32("fullscale-mA", CS30BLState, fullscale_mA,1700),
     DEFINE_PROP_END_OF_LIST(),
 };
 

hw/arm/prusa/parts/corexy_helper.c

@@ -26,12 +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 >*/
@@ -42,16 +65,33 @@
     uint32_t x_max_um;
 	uint32_t y_max_um;
 
+    bool swap_calc;
+
 	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)];
+
 };
 
-OBJECT_DEFINE_TYPE_SIMPLE_WITH_INTERFACES(CoreXYState, corexy, COREXY, SYS_BUS_DEVICE, {TYPE_P404_MOTOR_IF}, {NULL});
+#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}, {TYPE_P404_KEYCLIENT}, {NULL});
 
 static void corexy_finalize(Object *obj)
 {
@@ -64,23 +104,93 @@
     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;
 	}
 	int32_t xpos = (s->pos_a_um + s->pos_b_um)/2.f;
 	int32_t ypos = (s->pos_a_um - s->pos_b_um)/2.f;
-	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;
+    if (s->swap_calc)
+    {
+        int32_t tmp = xpos;
+        xpos = ypos;
+        ypos = tmp;
+    }
+    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)
@@ -94,6 +204,31 @@
 	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);
@@ -108,21 +243,43 @@
     return s->vis;
 }
 
+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);
+	s->vis_y.status.changed = true;
+}
+
 static void corexy_init(Object *obj)
 {
     CoreXYState *s = COREXY(obj);
-	s->x_max_um = 365*1000;
-	s->y_max_um = 365*1000;
-	s->vis_x.max_pos = 365;
+	s->vis_x.max_pos = s->x_max_um/(1000U);
 	s->vis_x.label = 'X';
 	s->vis_x.status.enabled = true;
 	s->vis_x.status.changed = true;
-	s->vis_y.max_pos = 365;
+	s->vis_y.max_pos = s->y_max_um/(1000U);
 	s->vis_y.label = 'Y';
 	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 = {
@@ -137,12 +294,24 @@
     }
 };
 
+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, 466*1000),
+    DEFINE_PROP_BOOL("swap-calc", CoreXYState, swap_calc, false),
+    DEFINE_PROP_END_OF_LIST(),
+};
+
 static void corexy_class_init(ObjectClass *oc, void *data)
 {
     DeviceClass *dc = DEVICE_CLASS(oc);
     dc->reset = corexy_reset;
+    dc->realize = corexy_realize;
     dc->vmsd = &vmstate_corexy;
+    device_class_set_props(dc, corexy_properties);
 
 	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;
 }