From 0bda787e698e1ee106ea826a67b4f7885d1a24e5 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 13:15:10 +0900 Subject: [PATCH 01/20] refactor: Add `System` config structure --- src/Config.zig | 181 ++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 180 insertions(+), 1 deletion(-) diff --git a/src/Config.zig b/src/Config.zig index 3c2e59a..5e8897e 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -6,6 +6,185 @@ pub const MAX_AXES = 3; const Config = @This(); +/// CTO driver configuration structure. +pub const System = union { + set: System.Set, + get: System.Get, + + const Set = packed struct { + rs: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + ls: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + kf: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + kbm: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + max_curr: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + magnet_pitch: f32, + slider_mass: f32, + slider_length: f32, + axis_length: f32, + home_exist: bool, + _home_exist_padding: u7, + has_neighbor: packed struct { + backward: bool, + _backward_padding: u7, + forward: bool, + _forward_padding: u7, + }, + use_axis: packed struct { + axis2: bool, + _axis2_padding: u7, + axis3: bool, + _axis3_padding: u7, + }, + /// Driver ID on the line + id: u16, + /// Whether there is space on the edge of driver during calibration + calibration_spare: packed struct { + backward: bool, + _backward_padding: u7, + forward: bool, + _forward_padding: u7, + }, + collision_avoidance: bool, + _collision_avoidance_padding: u7, + _unused: u16, + continuous_current: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + line_axes: u32, + voltage_warmup: f32, + retry_count: u8, + station: u16, + cc_link_speed: enum(u8) { + @"156 kbps", + @"625 kbps", + @"2.5 Mbps", + @"5 Mbps", + @"10 Mbps", + }, + hall_cutoff_freq: f32, + overcurrent_timeout: f32, + pos_offset: f32, + calibration_use: bool, + _calibration_use_padding: u7, + xts: bool, + _xts_use_padding: u7, + right_sensor_distance: f32, + flip: bool, + _flip_padding: u7, + swap: bool, + _swap_padding: u7, + }; + const Get = packed struct { + /// Bytes 7..31 are not used + _: u200, + rs: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + ls: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + kf: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + kbm: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + max_curr: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + magnet_pitch: f32, + slider_mass: f32, + slider_length: f32, + axis_length: f32, + home_exist: bool, + _home_exist_padding: u7, + has_neighbor: packed struct { + backward: bool, + _backward_padding: u7, + forward: bool, + _forward_padding: u7, + }, + use_axis: packed struct { + axis2: bool, + _axis2_padding: u7, + axis3: bool, + _axis3_padding: u7, + }, + /// Driver ID on the line + id: u16, + cc_link_speed: enum(u8) { + @"156 kbps", + @"625 kbps", + @"2.5 Mbps", + @"5 Mbps", + @"10 Mbps", + }, + /// Whether there is space on the edge of driver during calibration + calibration_spare: packed struct { + backward: bool, + _backward_padding: u7, + forward: bool, + _forward_padding: u7, + }, + collision_avoidance: bool, + _collision_avoidance_padding: u7, + /// Bytes 119..127 are not used + _1: u72, + continuous_current: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + line_axes: u32, + voltage_warmup: f32, + retry_count: u8, + station: u16, + overcurrent_timeout: f32, + hall_cutoff_freq: f32, + pos_offset: f32, + calibration_use: bool, + _calibration_use_padding: u7, + xts: bool, + _xts_padding: u7, + right_sensor_distance: f32, + flip: bool, + _flip_padding: u7, + swap: bool, + _swap_padding: u7, + }; +}; /// Driver configuration field. These fields are used directly in messages /// with driver; their ordering matches firmware field kind ordering. /// Names reflect nested structure within `Config` struct, and types represent @@ -801,7 +980,7 @@ zero_position: f32, hall_sensors: [6]HallSensor, -pub const CurrentGain = struct { +pub const _CurrentGain = struct { /// Current P-gain. By default, inductance * Wcc. p: f32, /// Current I-gain. By default, resistance * Wcc. From 229b586fb8e6a450ee118f003fdc6b604ec43f45 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 13:32:46 +0900 Subject: [PATCH 02/20] refactor: Add `SpeedGain` config structure --- src/Config.zig | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) diff --git a/src/Config.zig b/src/Config.zig index 5e8897e..1a6cef3 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -185,6 +185,71 @@ pub const System = union { _swap_padding: u7, }; }; + +pub const SpeedGain = union { + set: SpeedGain.Set, + get: SpeedGain.Get, + + const Set = packed struct(u384) { + axis1: packed struct { + p: f32, + i: f32, + }, + axis2: packed struct { + p: f32, + i: f32, + }, + axis3: packed struct { + p: f32, + i: f32, + }, + denominator: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + denominator_pi: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + /// SHDrv just send 1 without reasons in the code. + _const_a: u32 = 1, + _const_b: u32 = 1, + /// SHDrv just send 100 without reasons in the code. + _const_c: u16 = 100, + _const_d: u16 = 100, + }; + + const Get = packed struct(u376) { + _: u8, + axis1: packed struct { + p: f32, + i: f32, + }, + axis2: packed struct { + p: f32, + i: f32, + }, + axis3: packed struct { + p: f32, + i: f32, + }, + _1: u64, + denominator: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + _2: u16, + denominator_pi: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + }; +}; + /// Driver configuration field. These fields are used directly in messages /// with driver; their ordering matches firmware field kind ordering. /// Names reflect nested structure within `Config` struct, and types represent From 5c86e793743f6be90137696e6c596688a8ded31a Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 13:36:15 +0900 Subject: [PATCH 03/20] refactor: Add `PosGain` config structure --- src/Config.zig | 48 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/src/Config.zig b/src/Config.zig index 1a6cef3..c0d9a2d 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -250,6 +250,54 @@ pub const SpeedGain = union { }; }; +pub const PosGain = union { + set: PosGain.Set, + get: PosGain.Get, + + const Set = packed struct(u288) { + p: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + deno_wpc: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + arrival_threshold: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + /// SHDrv just send 1 without reasons in the code. + _const_a: u32 = 1, + /// SHDrv just send 100 without reasons in the code. + _const_b: u16 = 100, + }; + + const Get = packed struct(u296) { + _: u8, + p: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + _1: u32, + denominator: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + _2: u16, + arrival_threshold: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + }; +}; + /// Driver configuration field. These fields are used directly in messages /// with driver; their ordering matches firmware field kind ordering. /// Names reflect nested structure within `Config` struct, and types represent From 907f68fe3911ec8948c44ab95255d748ee89f75e Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 13:37:00 +0900 Subject: [PATCH 04/20] refactor: Add `CurrentGain` config structure --- src/Config.zig | 54 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/src/Config.zig b/src/Config.zig index c0d9a2d..a6e5bb2 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -298,6 +298,60 @@ pub const PosGain = union { }; }; +pub const CurrentGain = union { + set: CurrentGain.Set, + get: CurrentGain.Get, + + const Set = packed struct(u320) { + axis1: packed struct { + p: f32, + i: f32, + }, + axis2: packed struct { + p: f32, + i: f32, + }, + axis3: packed struct { + p: f32, + i: f32, + }, + denominator: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + /// SHDrv just send 1 without reasons in the code. + _const_a: u32 = 1, + _const_b: u32 = 1, + /// SHDrv just send 100 without reasons in the code. + _const_deno: u16 = 100, + }; + + const Get = packed struct(u312) { + /// Byte 7 is not used + _: u8, + axis1: packed struct { + p: f32, + i: f32, + }, + axis2: packed struct { + p: f32, + i: f32, + }, + axis3: packed struct { + p: f32, + i: f32, + }, + /// Bytes 32..39 are not used. + _1: u64, + denominator: packed struct { + axis1: u16, + axis2: u16, + axis3: u16, + }, + }; +}; + /// Driver configuration field. These fields are used directly in messages /// with driver; their ordering matches firmware field kind ordering. /// Names reflect nested structure within `Config` struct, and types represent From 57645a8a27e89c92906a68e8603e0c8efd315074 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 13:38:28 +0900 Subject: [PATCH 05/20] fix: avoid error test now --- src/Config.zig | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/Config.zig b/src/Config.zig index a6e5bb2..382a5a9 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -186,9 +186,9 @@ pub const System = union { }; }; -pub const SpeedGain = union { - set: SpeedGain.Set, - get: SpeedGain.Get, +pub const _SpeedGain = union { + set: _SpeedGain.Set, + get: _SpeedGain.Get, const Set = packed struct(u384) { axis1: packed struct { @@ -250,9 +250,9 @@ pub const SpeedGain = union { }; }; -pub const PosGain = union { - set: PosGain.Set, - get: PosGain.Get, +pub const _PositionGain = union { + set: _PositionGain.Set, + get: _PositionGain.Get, const Set = packed struct(u288) { p: packed struct { @@ -298,9 +298,9 @@ pub const PosGain = union { }; }; -pub const CurrentGain = union { - set: CurrentGain.Set, - get: CurrentGain.Get, +pub const _CurrentGain = union { + set: _CurrentGain.Set, + get: _CurrentGain.Get, const Set = packed struct(u320) { axis1: packed struct { @@ -1147,7 +1147,7 @@ zero_position: f32, hall_sensors: [6]HallSensor, -pub const _CurrentGain = struct { +pub const CurrentGain = struct { /// Current P-gain. By default, inductance * Wcc. p: f32, /// Current I-gain. By default, resistance * Wcc. From 1e4e6a5047186de06db3f92bfdbd0c93db66f4b8 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 15:04:35 +0900 Subject: [PATCH 06/20] refactor: Add `State` structure This is a read-only data provided by the driver --- src/Config.zig | 208 +++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 200 insertions(+), 8 deletions(-) diff --git a/src/Config.zig b/src/Config.zig index 382a5a9..64e483f 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -11,7 +11,7 @@ pub const System = union { set: System.Set, get: System.Get, - const Set = packed struct { + pub const Set = packed struct { rs: packed struct { axis1: f32, axis2: f32, @@ -96,7 +96,7 @@ pub const System = union { swap: bool, _swap_padding: u7, }; - const Get = packed struct { + pub const Get = packed struct { /// Bytes 7..31 are not used _: u200, rs: packed struct { @@ -186,11 +186,203 @@ pub const System = union { }; }; +pub const State = union { + get: State.Get, + + pub const Fsm = enum(u8) { + none = 0, + warm_up = 1, + warm_up_comp = 2, + warm_up_fault = 3, + curr_bias = 4, + curr_bias_comp = 5, + fwd_ramp = 8, + fwd_ramp_comp = 9, + fwd_ramp_fault = 10, + bwd_ramp = 11, + bwd_ramp_comp = 12, + bwd_ramp_fault = 13, + curr_step = 20, + curr_step_comp = 21, + curr_step_fault = 22, + speed_step = 23, + speed_step_comp = 24, + speed_step_fault = 25, + pos_step = 26, + pos_step_comp = 27, + pos_step_fault = 28, + pos_prof = 29, + pos_prof_comp = 30, + pos_prof_fault = 31, + fwd_calib = 32, + fwd_calib_comp = 33, + bwd_calib = 34, + bwd_calib_comp = 35, + speed_prof = 40, + speed_prof_comp = 41, + speed_prof_fault = 42, + fwd_slave = 43, + fwd_slave_comp = 44, + bwd_slave = 45, + bwd_slave_comp = 46, + over_charge = 50, + synch_com_error = 51, + _, + }; + + pub const Entrance = enum(u8) { + none = 0, + left = 1, + right = 2, + _, + }; + + const Get = packed struct(u1608) { + /// Byte 7: not read by the host app (status/flag byte). + _: u8, + is_servo_on: bool, + _is_servo_on_padding: u7, + vdc: f32, + thermo: f32, + slide_no: packed struct { + axis1: u8, + axis2: u8, + axis3: u8, + }, + fwd_lsen_off_pos: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + /// Stride is 8, not 4: bytes 36..39, 44..47, 52..57 are not read. + base_pos: packed struct { + axis1: f32, + _axis1_padding: u32, + axis2: f32, + _axis2_padding: u32, + axis3: f32, + _axis3_padding: u48, + }, + theta_offset: f32, + slide_state: packed struct { + axis1: Fsm, + axis2: Fsm, + axis3: Fsm, + }, + is_calibrate: bool, + _is_calibrate_padding: u7, + bias: packed struct { + axis1: packed struct { + a: f32, + b: f32, + }, + axis2: packed struct { + a: f32, + b: f32, + }, + axis3: packed struct { + a: f32, + b: f32, + }, + }, + sys_version: f32, + is_cclink_on: bool, + _is_cclink_on_padding: u7, + mecha_pos: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + /// Byte 107: not read. + _1: u8, + sensor: packed struct { + home: u8, + id0: u8, + id1: u8, + id2: u8, + }, + entrance: packed struct { + axis1: Entrance, + axis2: Entrance, + axis3: Entrance, + }, + pitch_cnt: packed struct { + axis1: i16, + axis2: i16, + axis3: i16, + }, + /// Bytes 121..122: not read. + _2: u16, + enable: packed struct { + axis1: bool, + _axis1_padding: u7, + axis2: bool, + _axis2_padding: u7, + axis3: bool, + _axis3_padding: u7, + }, + /// Bytes 126..129: not read (was CaliHome, commented out). + _3: u32, + restart_section_cnt: packed struct { + axis1: i16, + axis2: i16, + axis3: i16, + }, + bwd_rsen_off_pos: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + rsen_off_section_cnt: packed struct { + axis1: i16, + axis2: i16, + axis3: i16, + }, + fwd_syncr_info_req_pos: f32, + bwd_syncl_info_req_pos: f32, + /// Bytes 162..165: not read. + _4: u32, + over_charge_state: packed struct { + axis1: Fsm, + axis2: Fsm, + axis3: Fsm, + }, + fwd_rsen_on_pos: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + fwd_rsen_on_section_cnt: packed struct { + axis1: i16, + axis2: i16, + axis3: i16, + }, + bwd_lsen_on_pos: packed struct { + axis1: f32, + axis2: f32, + axis3: f32, + }, + bwd_lsen_on_section_cnt: packed struct { + axis1: i16, + axis2: i16, + axis3: i16, + }, + primary_axis: packed struct { + axis1: bool, + _axis1_padding: u7, + axis2: bool, + _axis2_padding: u7, + axis3: bool, + _axis3_padding: u7, + }, + }; +}; + pub const _SpeedGain = union { set: _SpeedGain.Set, get: _SpeedGain.Get, - const Set = packed struct(u384) { + pub const Set = packed struct(u384) { axis1: packed struct { p: f32, i: f32, @@ -221,7 +413,7 @@ pub const _SpeedGain = union { _const_d: u16 = 100, }; - const Get = packed struct(u376) { + pub const Get = packed struct(u376) { _: u8, axis1: packed struct { p: f32, @@ -254,7 +446,7 @@ pub const _PositionGain = union { set: _PositionGain.Set, get: _PositionGain.Get, - const Set = packed struct(u288) { + pub const Set = packed struct(u288) { p: packed struct { axis1: f32, axis2: f32, @@ -276,7 +468,7 @@ pub const _PositionGain = union { _const_b: u16 = 100, }; - const Get = packed struct(u296) { + pub const Get = packed struct(u296) { _: u8, p: packed struct { axis1: f32, @@ -302,7 +494,7 @@ pub const _CurrentGain = union { set: _CurrentGain.Set, get: _CurrentGain.Get, - const Set = packed struct(u320) { + pub const Set = packed struct(u320) { axis1: packed struct { p: f32, i: f32, @@ -327,7 +519,7 @@ pub const _CurrentGain = union { _const_deno: u16 = 100, }; - const Get = packed struct(u312) { + pub const Get = packed struct(u312) { /// Byte 7 is not used _: u8, axis1: packed struct { From d13dd37bd70af0c6fb07ca1fd952125e44a29546 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 22 Sep 2026 16:07:57 +0900 Subject: [PATCH 07/20] refactor: New message format --- src/Config.zig | 2 +- src/message.zig | 104 ++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 105 insertions(+), 1 deletion(-) diff --git a/src/Config.zig b/src/Config.zig index 64e483f..a462992 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -237,7 +237,7 @@ pub const State = union { _, }; - const Get = packed struct(u1608) { + pub const Get = packed struct(u1608) { /// Byte 7: not read by the host app (status/flag byte). _: u8, is_servo_on: bool, diff --git a/src/message.zig b/src/message.zig index 9d6fd17..439ded1 100644 --- a/src/message.zig +++ b/src/message.zig @@ -3,6 +3,110 @@ const std = @import("std"); const Config = @import("Config.zig"); const Log = @import("Log.zig"); +pub const _Message = extern struct { + stx: u8, + kind: Kind, // 8 bits size + len: u32, + sequence: u8, // Sequence ID will be wrapped back to zero + payload: Payload, + etx: u8, + bcc: u8, + + pub const Payload = extern union { + none: void, // Excluded from the Kind + get_config: Config.System.Get, + get_system_state: Config.State.Get, + get_gain_current: Config._CurrentGain.Get, + get_gain_speed: Config._SpeedGain.Get, + get_gain_position: Config._PositionGain.Get, + set_servo_on: bool, + set_config: Config.System.Set, + set_gain_current: Config._CurrentGain.Set, + set_gain_speed: Config._SpeedGain.Set, + set_gain_position: Config._PositionGain.Set, + }; + + pub const Kind = b: { + const ti = @typeInfo(Payload).@"union"; + const tag_type = u8; + var field_names: [ti.fields.len - 1][]const u8 = undefined; + var field_values: [ti.fields.len - 1]tag_type = undefined; + var val: tag_type = 1; + // Do not include none payload as the Kind field + for (ti.fields[1..], 0..) |field, i| { + if (std.mem.eql(u8, "get_config", field.name)) { + val = 0x02; + } + // TODO: Skip any jumping payload values in future + field_names[i] = field.name; + field_values[i] = val; + val += 1; + } + break :b @Enum(tag_type, .nonexhaustive, &field_names, &field_values); + }; + + pub fn PayloadType(comptime kind: Kind) type { + const ti = @typeInfo(Payload).@"union"; + @setEvalBranchQuota(4000); + inline for (ti.fields) |field| { + if (std.mem.eql(u8, "get", field.name[0..3])) { + return void; + } else if (std.mem.eql(u8, field.name, @tagName(kind))) { + return field.type; + } + } + } + + pub fn init( + comptime kind: Kind, + sequence: u16, + p: PayloadType(kind), + ) _Message { + return switch (kind) { + inline else => b: { + var msg: _Message = .{ + .stx = 0x02, + .kind = kind, + .len = @sizeOf(PayloadType(kind)) + 9, + .sequence = sequence, + .payload = undefined, + .etx = 0x03, + .bcc = undefined, + }; + var _payload: Payload = .{ .none = .{} }; + @field(_payload, @tagName(kind)) = p; + msg.payload = @bitCast(_payload); + std.mem.byteSwapAllFields( + PayloadType(kind), + &@field(msg.payload, @tagName(kind)), + ); + msg.bcc = msg.getBcc(); + break :b msg; + }, + }; + } + + pub fn getPayload( + self: *const _Message, + comptime kind: Kind, + ) PayloadType(kind) { + const _payload: Payload = @bitCast(self._payload); + return switch (kind) { + inline else => @field(_payload, @tagName(kind)), + }; + } + + pub fn getBcc(self: *const _Message) u8 { + const bytes: []const u8 = std.mem.asBytes(self); + var bcc: u8 = 0; + // Excluding the STX and BCC value + for (bytes[1 .. bytes.len - 1]) |b| { + bcc ^= b; + } + return bcc; + } +}; + pub const Message = packed struct { kind: Kind, sequence: u16, From 883a70cbf43e174ce82cc4113ad3133749cc1cb4 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 30 Sep 2026 16:36:57 +0900 Subject: [PATCH 08/20] refactor: New `Message` and `Config` structure John's configuration structure is still keep for now before the migration is fully finished --- src/Config.zig | 538 --------------------------- src/_Config.zig | 96 +++++ src/_Message.zig | 904 ++++++++++++++++++++++++++++++++++++++++++++++ src/drivercom.zig | 2 + src/message.zig | 104 ------ 5 files changed, 1002 insertions(+), 642 deletions(-) create mode 100644 src/_Config.zig create mode 100644 src/_Message.zig diff --git a/src/Config.zig b/src/Config.zig index a462992..3c2e59a 100644 --- a/src/Config.zig +++ b/src/Config.zig @@ -6,544 +6,6 @@ pub const MAX_AXES = 3; const Config = @This(); -/// CTO driver configuration structure. -pub const System = union { - set: System.Set, - get: System.Get, - - pub const Set = packed struct { - rs: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - ls: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - kf: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - kbm: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - max_curr: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - magnet_pitch: f32, - slider_mass: f32, - slider_length: f32, - axis_length: f32, - home_exist: bool, - _home_exist_padding: u7, - has_neighbor: packed struct { - backward: bool, - _backward_padding: u7, - forward: bool, - _forward_padding: u7, - }, - use_axis: packed struct { - axis2: bool, - _axis2_padding: u7, - axis3: bool, - _axis3_padding: u7, - }, - /// Driver ID on the line - id: u16, - /// Whether there is space on the edge of driver during calibration - calibration_spare: packed struct { - backward: bool, - _backward_padding: u7, - forward: bool, - _forward_padding: u7, - }, - collision_avoidance: bool, - _collision_avoidance_padding: u7, - _unused: u16, - continuous_current: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - line_axes: u32, - voltage_warmup: f32, - retry_count: u8, - station: u16, - cc_link_speed: enum(u8) { - @"156 kbps", - @"625 kbps", - @"2.5 Mbps", - @"5 Mbps", - @"10 Mbps", - }, - hall_cutoff_freq: f32, - overcurrent_timeout: f32, - pos_offset: f32, - calibration_use: bool, - _calibration_use_padding: u7, - xts: bool, - _xts_use_padding: u7, - right_sensor_distance: f32, - flip: bool, - _flip_padding: u7, - swap: bool, - _swap_padding: u7, - }; - pub const Get = packed struct { - /// Bytes 7..31 are not used - _: u200, - rs: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - ls: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - kf: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - kbm: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - max_curr: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - magnet_pitch: f32, - slider_mass: f32, - slider_length: f32, - axis_length: f32, - home_exist: bool, - _home_exist_padding: u7, - has_neighbor: packed struct { - backward: bool, - _backward_padding: u7, - forward: bool, - _forward_padding: u7, - }, - use_axis: packed struct { - axis2: bool, - _axis2_padding: u7, - axis3: bool, - _axis3_padding: u7, - }, - /// Driver ID on the line - id: u16, - cc_link_speed: enum(u8) { - @"156 kbps", - @"625 kbps", - @"2.5 Mbps", - @"5 Mbps", - @"10 Mbps", - }, - /// Whether there is space on the edge of driver during calibration - calibration_spare: packed struct { - backward: bool, - _backward_padding: u7, - forward: bool, - _forward_padding: u7, - }, - collision_avoidance: bool, - _collision_avoidance_padding: u7, - /// Bytes 119..127 are not used - _1: u72, - continuous_current: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - line_axes: u32, - voltage_warmup: f32, - retry_count: u8, - station: u16, - overcurrent_timeout: f32, - hall_cutoff_freq: f32, - pos_offset: f32, - calibration_use: bool, - _calibration_use_padding: u7, - xts: bool, - _xts_padding: u7, - right_sensor_distance: f32, - flip: bool, - _flip_padding: u7, - swap: bool, - _swap_padding: u7, - }; -}; - -pub const State = union { - get: State.Get, - - pub const Fsm = enum(u8) { - none = 0, - warm_up = 1, - warm_up_comp = 2, - warm_up_fault = 3, - curr_bias = 4, - curr_bias_comp = 5, - fwd_ramp = 8, - fwd_ramp_comp = 9, - fwd_ramp_fault = 10, - bwd_ramp = 11, - bwd_ramp_comp = 12, - bwd_ramp_fault = 13, - curr_step = 20, - curr_step_comp = 21, - curr_step_fault = 22, - speed_step = 23, - speed_step_comp = 24, - speed_step_fault = 25, - pos_step = 26, - pos_step_comp = 27, - pos_step_fault = 28, - pos_prof = 29, - pos_prof_comp = 30, - pos_prof_fault = 31, - fwd_calib = 32, - fwd_calib_comp = 33, - bwd_calib = 34, - bwd_calib_comp = 35, - speed_prof = 40, - speed_prof_comp = 41, - speed_prof_fault = 42, - fwd_slave = 43, - fwd_slave_comp = 44, - bwd_slave = 45, - bwd_slave_comp = 46, - over_charge = 50, - synch_com_error = 51, - _, - }; - - pub const Entrance = enum(u8) { - none = 0, - left = 1, - right = 2, - _, - }; - - pub const Get = packed struct(u1608) { - /// Byte 7: not read by the host app (status/flag byte). - _: u8, - is_servo_on: bool, - _is_servo_on_padding: u7, - vdc: f32, - thermo: f32, - slide_no: packed struct { - axis1: u8, - axis2: u8, - axis3: u8, - }, - fwd_lsen_off_pos: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - /// Stride is 8, not 4: bytes 36..39, 44..47, 52..57 are not read. - base_pos: packed struct { - axis1: f32, - _axis1_padding: u32, - axis2: f32, - _axis2_padding: u32, - axis3: f32, - _axis3_padding: u48, - }, - theta_offset: f32, - slide_state: packed struct { - axis1: Fsm, - axis2: Fsm, - axis3: Fsm, - }, - is_calibrate: bool, - _is_calibrate_padding: u7, - bias: packed struct { - axis1: packed struct { - a: f32, - b: f32, - }, - axis2: packed struct { - a: f32, - b: f32, - }, - axis3: packed struct { - a: f32, - b: f32, - }, - }, - sys_version: f32, - is_cclink_on: bool, - _is_cclink_on_padding: u7, - mecha_pos: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - /// Byte 107: not read. - _1: u8, - sensor: packed struct { - home: u8, - id0: u8, - id1: u8, - id2: u8, - }, - entrance: packed struct { - axis1: Entrance, - axis2: Entrance, - axis3: Entrance, - }, - pitch_cnt: packed struct { - axis1: i16, - axis2: i16, - axis3: i16, - }, - /// Bytes 121..122: not read. - _2: u16, - enable: packed struct { - axis1: bool, - _axis1_padding: u7, - axis2: bool, - _axis2_padding: u7, - axis3: bool, - _axis3_padding: u7, - }, - /// Bytes 126..129: not read (was CaliHome, commented out). - _3: u32, - restart_section_cnt: packed struct { - axis1: i16, - axis2: i16, - axis3: i16, - }, - bwd_rsen_off_pos: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - rsen_off_section_cnt: packed struct { - axis1: i16, - axis2: i16, - axis3: i16, - }, - fwd_syncr_info_req_pos: f32, - bwd_syncl_info_req_pos: f32, - /// Bytes 162..165: not read. - _4: u32, - over_charge_state: packed struct { - axis1: Fsm, - axis2: Fsm, - axis3: Fsm, - }, - fwd_rsen_on_pos: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - fwd_rsen_on_section_cnt: packed struct { - axis1: i16, - axis2: i16, - axis3: i16, - }, - bwd_lsen_on_pos: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - bwd_lsen_on_section_cnt: packed struct { - axis1: i16, - axis2: i16, - axis3: i16, - }, - primary_axis: packed struct { - axis1: bool, - _axis1_padding: u7, - axis2: bool, - _axis2_padding: u7, - axis3: bool, - _axis3_padding: u7, - }, - }; -}; - -pub const _SpeedGain = union { - set: _SpeedGain.Set, - get: _SpeedGain.Get, - - pub const Set = packed struct(u384) { - axis1: packed struct { - p: f32, - i: f32, - }, - axis2: packed struct { - p: f32, - i: f32, - }, - axis3: packed struct { - p: f32, - i: f32, - }, - denominator: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - denominator_pi: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - /// SHDrv just send 1 without reasons in the code. - _const_a: u32 = 1, - _const_b: u32 = 1, - /// SHDrv just send 100 without reasons in the code. - _const_c: u16 = 100, - _const_d: u16 = 100, - }; - - pub const Get = packed struct(u376) { - _: u8, - axis1: packed struct { - p: f32, - i: f32, - }, - axis2: packed struct { - p: f32, - i: f32, - }, - axis3: packed struct { - p: f32, - i: f32, - }, - _1: u64, - denominator: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - _2: u16, - denominator_pi: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - }; -}; - -pub const _PositionGain = union { - set: _PositionGain.Set, - get: _PositionGain.Get, - - pub const Set = packed struct(u288) { - p: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - deno_wpc: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - arrival_threshold: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - /// SHDrv just send 1 without reasons in the code. - _const_a: u32 = 1, - /// SHDrv just send 100 without reasons in the code. - _const_b: u16 = 100, - }; - - pub const Get = packed struct(u296) { - _: u8, - p: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - _1: u32, - denominator: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - _2: u16, - arrival_threshold: packed struct { - axis1: f32, - axis2: f32, - axis3: f32, - }, - }; -}; - -pub const _CurrentGain = union { - set: _CurrentGain.Set, - get: _CurrentGain.Get, - - pub const Set = packed struct(u320) { - axis1: packed struct { - p: f32, - i: f32, - }, - axis2: packed struct { - p: f32, - i: f32, - }, - axis3: packed struct { - p: f32, - i: f32, - }, - denominator: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - /// SHDrv just send 1 without reasons in the code. - _const_a: u32 = 1, - _const_b: u32 = 1, - /// SHDrv just send 100 without reasons in the code. - _const_deno: u16 = 100, - }; - - pub const Get = packed struct(u312) { - /// Byte 7 is not used - _: u8, - axis1: packed struct { - p: f32, - i: f32, - }, - axis2: packed struct { - p: f32, - i: f32, - }, - axis3: packed struct { - p: f32, - i: f32, - }, - /// Bytes 32..39 are not used. - _1: u64, - denominator: packed struct { - axis1: u16, - axis2: u16, - axis3: u16, - }, - }; -}; - /// Driver configuration field. These fields are used directly in messages /// with driver; their ordering matches firmware field kind ordering. /// Names reflect nested structure within `Config` struct, and types represent diff --git a/src/_Config.zig b/src/_Config.zig new file mode 100644 index 0000000..903f2ae --- /dev/null +++ b/src/_Config.zig @@ -0,0 +1,96 @@ +//! This module represents driver's configuration. +const std = @import("std"); +const Config = @This(); + +/// Driver ID +id: u16, +/// CC-Link Station ID +station: u16, +/// CC-Link baud rate +baud_rate: CcLinkSpeed, +flags: Flags, +line: struct { + /// Total number of axes in line. + axes: u32, + /// Axis length used in the whole line. + axis_length: f32, + /// Slider configuration in the whole line. + slider: struct { + mass: f32, + length: f32, + }, + /// Magnet pitch (m) + magnet_pitch: f32, +}, +voltage_warmup: f32, +retry_count: u8, +hall_cutoff_freq: f32, +overcurrent_timeout: f32, +pos_offset: f32, +right_sensor_distance: f32, +axes: [3]Axis, + +pub const Axis = struct { + rs: f32, + ls: f32, + kf: f32, + kbm: f32, + max_current: f32, + continuous_current: f32, + gain: struct { + current: CurrentGain, + speed: SpeedGain, + position: PositionGain, + }, +}; + +pub const Flags = struct { + home_exists: bool, + has_neighbor: packed struct(u2) { + backward: bool, + forward: bool, + }, + use_axis: packed struct(u2) { + axis2: bool, + axis3: bool, + }, + calibration_spare: packed struct(u2) { + backward: bool, + forward: bool, + }, + collision_avoidance: bool, + calibration_use: bool, + xts: bool, + /// Flip all sensors in a driver + flip: bool, + /// Swap all sensors in a driver + swap: bool, +}; + +pub const CcLinkSpeed = enum(u8) { + @"156 kbps", + @"625 kbps", + @"2.5 Mbps", + @"5 Mbps", + @"10 Mbps", + _, +}; + +pub const CurrentGain = struct { + p: f32, + i: f32, + denominator: u16, +}; + +pub const SpeedGain = struct { + p: f32, + i: f32, + denominator: u16, + denominator_pi: u16, +}; + +pub const PositionGain = struct { + p: f32, + denominator: f32, + arrival_threshold: f32, +}; diff --git a/src/_Message.zig b/src/_Message.zig new file mode 100644 index 0000000..c970bc8 --- /dev/null +++ b/src/_Message.zig @@ -0,0 +1,904 @@ +//! Message format to Premo Robotics driver. Initialize the message by calling +//! `init()` for writing a message to the firmware or `parse()` for reading a +//! message from the firmware. Caller must call `deinit()` to free allocated +//! payload memory. + +const std = @import("std"); +const builtin = @import("builtin"); +const Config = @import("_Config.zig"); +const Message = @This(); + +const STX = 0x02; +const ETX = 0x03; + +header: Header, +/// Payload is defined as slice of u8 since the payload is not consistent for +/// every message. It is preferred over union as union type does not guarantee +/// memory layout. +payload: []u8, +etx: u8, +/// Unused when sending message to firmware. +bcc: u8, + +/// Write the message into the writer. The message is writen in Big Endian due +/// to firmware implementation are using Big Endianness. +pub fn write(self: Message, writer: *std.Io.Writer) std.Io.Writer.Error!void { + const endian: std.builtin.Endian = .big; + try writer.writeStruct(self.header, endian); + try writer.writeSliceEndian(u8, self.payload, endian); + try writer.writeInt(u8, self.etx, endian); + try writer.writeInt(u8, getBcc(writer.buffered()[1..]), endian); +} + +/// Create message structure given its payload values and kind +pub fn init( + gpa: std.mem.Allocator, + comptime kind: Kind, + comptime message_type: Type, + sequence: u8, + p: PayloadType(kind, message_type), +) Message { + return .{ + .header = .{ + .stx = STX, + .kind = kind, + .len = @sizeOf(@TypeOf(p)) + 9, + .sequence = sequence, + }, + .payload = switch (@typeInfo(@TypeOf(p))) { + .@"struct" => try gpa.dupe(u8, std.mem.asBytes(p)), + .array => array: { + if (p.len > 0) { + @compileError("Array message must be zero length"); + } + break :array &p; + }, + else => { + @compileError("Unexpected value"); + }, + }, + .etx = ETX, + // BCC will be evaluated once the message is going to be sent. + .bcc = undefined, + }; +} + +/// Parse incoming response to Message type. Payload are written in Big Endian. +/// Caller must call `deinit()` to free allocated payload message. +pub fn parse( + gpa: std.mem.Allocator, + buf: []const u8, +) (ParseError || std.mem.Allocator.Error)!Message { + const header: Header = try .parse(buf[0..@sizeOf(Header)]); + + const len = std.mem.readInt(u32, buf[2..6], .big); + if (len != buf.len) return ParseError.MismatchLength; + // TODO: What shall be done with the rest of the header? + + const etx = buf[buf.len - 2]; + if (etx != ETX) return ParseError.MissingEtx; + const bcc = getBcc(buf[1 .. buf.len - 1]); + if (bcc != buf[buf.len - 1]) { + return ParseError.InvalidBcc; + } + + const payload = buf[7 .. buf.len - 2]; + + return .{ + .header = header, + .payload = try gpa.dupe(u8, payload), + .etx = etx, + .bcc = bcc, + }; +} + +/// This function shall be used only for response message +pub fn setConfig(self: Message, config: *Config) void { + const response: Response = switch (self.header.kind) { + .get_driver_config => get_driver_config: { + var payload: Response.SystemConfig = std.mem.bytesToValue( + Response.SystemConfig, + self.payload, + ); + std.mem.byteSwapAllFields(Response.SystemConfig, &payload); + break :get_driver_config .{ .get_driver_config = payload }; + }, + .get_driver_state => unreachable, + .get_gain_current => unreachable, + .get_gain_speed => unreachable, + .get_gain_position => unreachable, + .set_servo_on => unreachable, + .set_driver_config => unreachable, + .set_gain_current => unreachable, + .set_gain_speed => unreachable, + .set_gain_position => unreachable, + _ => unreachable, + }; + switch (response) { + .get_driver_config => |message| message.setConfig(config), + .get_driver_state => unreachable, // TODO + .get_gain_current => unreachable, // TODO + .get_gain_speed => unreachable, // TODO + .get_gain_position => unreachable, // TODO + .set_servo_on, + .set_driver_config, + .set_gain_current, + .set_gain_speed, + .set_gain_position, + => unreachable, + } +} + +pub const ParseError = error{ + /// Length message does not match with the whole message + MismatchLength, + /// Payload type does not match with message kind + InvalidPayload, + /// Bcc value does not match + InvalidBcc, + /// Stx byte not found on correct position + MissingStx, + /// Etx byte not found on correct position + MissingEtx, +}; + +pub const Header = extern struct { + stx: u8, + kind: Kind, // 8 bits size + len: u32 align(1), + sequence: u8, // Sequence ID will be wrapped back to zero + + /// Parse header from the firmware response. This function is useful for + /// parsing header first before parsing the whole message to get the actual + /// length of the response. Endian are swapped + pub fn parse(buf: []const u8) ParseError!Header { + std.debug.assert(buf.len == 7); + var header: Header = std.mem.bytesToValue(Header, buf); + std.mem.byteSwapAllFields(Header, &header); + // Ensuring STX is found + if (header.stx != STX) return ParseError.MissingStx; + // Validate kind + const kind_ti = @typeInfo(Kind).@"enum"; + if (@intFromEnum(header.kind) > + kind_ti.fields[kind_ti.fields.len - 1].value) + { + return error.InvalidPayload; + } + return header; + } +}; + +pub const Request = union(Kind) { + get_driver_config: [0]u8, + get_driver_state: [0]u8, + get_gain_current: [0]u8, + get_gain_speed: [0]u8, + get_gain_position: [0]u8, + set_servo_on: bool, + set_driver_config: SystemConfig, + set_gain_current: CurrentGain, + set_gain_speed: SpeedGain, + set_gain_position: PositionGain, + + pub const SystemConfig = extern struct { + rs: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + ls: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + kf: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + kbm: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + max_curr: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + magnet_pitch: f32 align(1), + slider_mass: f32 align(1), + slider_length: f32 align(1), + axis_length: f32 align(1), + home_exist: bool, + has_neighbor: extern struct { + backward: bool, + forward: bool, + }, + use_axis: extern struct { + axis2: bool, + axis3: bool, + }, + /// Driver ID on the line + id: u16 align(1), + /// Whether there is space on the edge of driver during calibration + calibration_spare: extern struct { + backward: bool, + forward: bool, + }, + collision_avoidance: bool, + _unused: u16 align(1), + continuous_current: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + line_axes: u32, + voltage_warmup: f32 align(1), + retry_count: u8, + station: u16 align(1), + cc_link_speed: enum(u8) { + @"156 kbps", + @"625 kbps", + @"2.5 Mbps", + @"5 Mbps", + @"10 Mbps", + }, + hall_cutoff_freq: f32 align(1), + overcurrent_timeout: f32 align(1), + pos_offset: f32 align(1), + calibration_use: bool, + xts: bool, + right_sensor_distance: f32 align(1), + flip: bool, + swap: bool, + }; + + pub const SpeedGain = extern struct { + axis1: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis2: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis3: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + denominator: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + denominator_pi: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + /// SHDrv just send 1 without reasons in the code. + _const_a: u32 align(1) = 1, + _const_b: u32 align(1) = 1, + /// SHDrv just send 100 without reasons in the code. + _const_c: u16 align(1) = 100, + _const_d: u16 align(1) = 100, + }; + + pub const PositionGain = extern struct { + p: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + deno_wpc: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + arrival_threshold: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + /// SHDrv just send 1 without reasons in the code. + _const_a: u32 align(1) = 1, + /// SHDrv just send 100 without reasons in the code. + _const_b: u16 align(1) = 100, + }; + + pub const CurrentGain = extern struct { + axis1: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis2: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis3: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + denominator: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + /// SHDrv just send 1 without reasons in the code. + _const_a: u32 align(1) = 1, + _const_b: u32 align(1) = 1, + /// SHDrv just send 100 without reasons in the code. + _const_deno: u16 align(1) = 100, + }; +}; + +pub const Response = union(Kind) { + get_driver_config: SystemConfig, + get_driver_state: SystemState, + get_gain_current: CurrentGain, + get_gain_speed: SpeedGain, + get_gain_position: PositionGain, + set_servo_on: Ack, + set_driver_config: Ack, + set_gain_current: Ack, + set_gain_speed: Ack, + set_gain_position: Ack, + + /// Decode response payload from the specified payload buffer + fn decode(comptime T: type, payload: []const u8) ParseError!T { + // TODO: validate the buffer is only payload, removing any header message + // and trailing payload buffer + switch (@typeInfo(T)) { + .void => return {}, + .bool => { + if (payload.len < 1) return error.InvalidPayload; + return payload[0] != 0; + }, + .@"enum" => |e| { + if (payload.len < @divExact(@bitSizeOf(e.tag_type), 8)) return error.InvalidPayload; + return @enumFromInt(payload[0]); + }, + .@"struct" => { + const size = @divExact(@bitSizeOf(T), 8); + if (payload.len < size) return error.InvalidPayload; + const Backing = std.meta.Int(.unsigned, @bitSizeOf(T)); + var le: [size]u8 = undefined; + @memcpy(&le, payload[0..size]); + return byteSwapextern(T, @bitCast(std.mem.readInt(Backing, &le, .little))); + }, + else => @compileError("decodePayload: unsupported type " ++ @typeName(T)), + } + } + + pub const SystemConfig = extern struct { + /// Bytes 7..31 are not used + _: [25]u8, + rs: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + + fn axis(self: @This(), i: usize) f32 { + switch (i) { + 0 => return self.axis1, + 1 => return self.axis2, + 2 => return self.axis3, + else => unreachable, + } + } + }, + ls: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + + fn axis(self: @This(), i: usize) f32 { + switch (i) { + 0 => return self.axis1, + 1 => return self.axis2, + 2 => return self.axis3, + else => unreachable, + } + } + }, + kf: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + + fn axis(self: @This(), i: usize) f32 { + switch (i) { + 0 => return self.axis1, + 1 => return self.axis2, + 2 => return self.axis3, + else => unreachable, + } + } + }, + kbm: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + + fn axis(self: @This(), i: usize) f32 { + switch (i) { + 0 => return self.axis1, + 1 => return self.axis2, + 2 => return self.axis3, + else => unreachable, + } + } + }, + max_curr: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + + fn axis(self: @This(), i: usize) f32 { + switch (i) { + 0 => return self.axis1, + 1 => return self.axis2, + 2 => return self.axis3, + else => unreachable, + } + } + }, + magnet_pitch: f32 align(1), + slider_mass: f32 align(1), + slider_length: f32 align(1), + axis_length: f32 align(1), + home_exist: bool, + has_neighbor: extern struct { + backward: bool, + forward: bool, + }, + use_axis: extern struct { + axis2: bool, + axis3: bool, + }, + /// Driver ID on the line + id: u16 align(1), + cc_link_speed: Config.CcLinkSpeed, + /// Whether there is space on the edge of driver during calibration + calibration_spare: extern struct { + backward: bool, + forward: bool, + }, + collision_avoidance: bool, + /// Bytes 119..127 are not used + _1: [9]u8, + continuous_current: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + + fn axis(self: @This(), i: usize) f32 { + switch (i) { + 0 => return self.axis1, + 1 => return self.axis2, + 2 => return self.axis3, + else => unreachable, + } + } + }, + line_axes: u32 align(1), + voltage_warmup: f32 align(1), + retry_count: u8, + station: u16 align(1), + overcurrent_timeout: f32 align(1), + hall_cutoff_freq: f32 align(1), + pos_offset: f32 align(1), + calibration_use: bool, + xts: bool, + right_sensor_distance: f32 align(1), + flip: bool, + swap: bool, + + fn setConfig(self: SystemConfig, config: *Config) void { + config.id = self.id; + config.station = self.station; + config.baud_rate = self.cc_link_speed; + config.flags = .{ + .home_exists = self.home_exist, + .has_neighbor = .{ + .backward = self.has_neighbor.backward, + .forward = self.has_neighbor.forward, + }, + .use_axis = .{ + .axis2 = self.use_axis.axis2, + .axis3 = self.use_axis.axis3, + }, + .calibration_spare = .{ + .backward = self.calibration_spare.backward, + .forward = self.calibration_spare.forward, + }, + .collision_avoidance = self.collision_avoidance, + .calibration_use = self.calibration_use, + .xts = self.xts, + .flip = self.flip, + .swap = self.swap, + }; + config.line = .{ + .axes = self.line_axes, + .axis_length = self.axis_length, + .slider = .{ + .mass = self.slider_mass, + .length = self.slider_length, + }, + .magnet_pitch = self.magnet_pitch, + }; + config.voltage_warmup = self.voltage_warmup; + config.retry_count = self.retry_count; + config.hall_cutoff_freq = self.hall_cutoff_freq; + config.overcurrent_timeout = self.overcurrent_timeout; + config.pos_offset = self.pos_offset; + config.right_sensor_distance = self.right_sensor_distance; + for (&config.axes, 0..) |*axis, i| { + axis.rs = self.rs.axis(i); + axis.ls = self.ls.axis(i); + axis.kf = self.kf.axis(i); + axis.kbm = self.kbm.axis(i); + axis.max_current = self.max_curr.axis(i); + axis.continuous_current = self.continuous_current.axis(i); + } + } + }; + + pub const SystemState = extern struct { + pub const SliderState = enum(u8) { + none = 0, + warm_up = 1, + warm_up_comp = 2, + warm_up_fault = 3, + curr_bias = 4, + curr_bias_comp = 5, + fwd_ramp = 8, + fwd_ramp_comp = 9, + fwd_ramp_fault = 10, + bwd_ramp = 11, + bwd_ramp_comp = 12, + bwd_ramp_fault = 13, + curr_step = 20, + curr_step_comp = 21, + curr_step_fault = 22, + speed_step = 23, + speed_step_comp = 24, + speed_step_fault = 25, + pos_step = 26, + pos_step_comp = 27, + pos_step_fault = 28, + pos_prof = 29, + pos_prof_comp = 30, + pos_prof_fault = 31, + fwd_calib = 32, + fwd_calib_comp = 33, + bwd_calib = 34, + bwd_calib_comp = 35, + speed_prof = 40, + speed_prof_comp = 41, + speed_prof_fault = 42, + fwd_slave = 43, + fwd_slave_comp = 44, + bwd_slave = 45, + bwd_slave_comp = 46, + over_charge = 50, + synch_com_error = 51, + _, + }; + pub const Entrance = enum(u8) { + none = 0, + left = 1, + right = 2, + _, + }; + _: u8, + is_servo_on: bool, + vdc: f32 align(1), + thermo: f32 align(1), + slide_no: extern struct { + axis1: u8, + axis2: u8, + axis3: u8, + }, + fwd_lsen_off_pos: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + base_pos: extern struct { + axis1: f32 align(1), + _basepos1: [4]u8, + axis2: f32 align(1), + _basepos2: [4]u8, + axis3: f32 align(1), + _basepos3: [6]u8, + }, + theta_offset: f32 align(1), + slide_state: extern struct { + axis1: SliderState, + axis2: SliderState, + axis3: SliderState, + }, + is_calibrate: bool, + bias: extern struct { + axis1: extern struct { + a: f32 align(1), + b: f32 align(1), + }, + axis2: extern struct { + a: f32 align(1), + b: f32 align(1), + }, + axis3: extern struct { + a: f32 align(1), + b: f32 align(1), + }, + }, + sys_version: f32 align(1), + is_cclink_on: bool, + mecha_pos: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + /// Byte 107: not read. + _1: u8, + sensor: extern struct { + home: u8, + id0: u8, + id1: u8, + id2: u8, + }, + entrance: extern struct { + axis1: Entrance, + axis2: Entrance, + axis3: Entrance, + }, + pitch_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + /// Bytes 121..122: not read. + _2: [2]u8, + enable: extern struct { + axis1: bool, + axis2: bool, + axis3: bool, + }, + /// Bytes 126..129: not read (was CaliHome, commented out). + _3: [4]u8, + restart_section_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + bwd_rsen_off_pos: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + rsen_off_section_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + fwd_syncr_info_req_pos: f32 align(1), + bwd_syncl_info_req_pos: f32 align(1), + /// Bytes 162..165: not read. + _4: [4]u8, + over_charge_state: extern struct { + axis1: SliderState, + axis2: SliderState, + axis3: SliderState, + }, + fwd_rsen_on_pos: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + fwd_rsen_on_section_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + bwd_lsen_on_pos: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + bwd_lsen_on_section_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + primary_axis: extern struct { + axis1: bool, + axis2: bool, + axis3: bool, + }, + }; + + pub const CurrentGain = extern struct { + /// Byte 7 is not used + _: u8, + axis1: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis2: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis3: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + /// Bytes 32..39 are not used. + _1: [8]u8, + denominator: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + }; + + pub const SpeedGain = extern struct { + _: u8, + axis1: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis2: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + axis3: extern struct { + p: f32 align(1), + i: f32 align(1), + }, + _1: [8]u8, + denominator: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + _2: u16 align(1), + denominator_pi: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + }; + + pub const PositionGain = extern struct { + _: u8, + p: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + _1: [4]u8, + denominator: extern struct { + axis1: u16 align(1), + axis2: u16 align(1), + axis3: u16 align(1), + }, + _2: u16 align(1), + arrival_threshold: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + }; + + pub const Ack = enum(u8) { + success = 0x00, + _, + }; +}; + +pub const Kind = enum(u8) { + get_driver_config = 0x02, + get_driver_state, + get_gain_current, + get_gain_speed, + get_gain_position, + set_servo_on, + set_driver_config, + set_gain_current, + set_gain_speed, + set_gain_position, + _, +}; + +pub const Payload = union(Type) { + request: Request, + response: Response, +}; + +const Type = enum(u2) { + request, + response, +}; + +fn PayloadType( + comptime kind: Kind, + comptime message_type: Type, +) type { + // Get either `Request` or `Response` type + const T = @FieldType(Payload, @tagName(message_type)); + // Return the payload type + return @FieldType(T, @tagName(kind)); +} + +// pub fn getPayload( +// self: *const Message, +// comptime kind: Kind, +// ) PayloadType(kind) { +// const _payload: Payload = @bitCast(self._payload); +// return switch (kind) { +// inline else => @field(_payload, @tagName(kind)), +// }; +// } + +/// Calculate bcc based on the buffer. The buffer must be the message excluding +/// the etx (header) and bcc itself. +fn getBcc(buf: []const u8) u8 { + // std.debug.assert(buf[0] != STX or @byteSwap(buf[0]) != STX); + var bcc: u8 = 0; + for (buf) |b| { + bcc ^= b; + } + return bcc; +} + +fn decodePayload(comptime T: type, payload: []const u8) ParseError!T { + switch (@typeInfo(T)) { + .void => return {}, + .bool => { + if (payload.len < 1) return error.InvalidPayload; + return payload[0] != 0; + }, + .@"enum" => |e| { + if (payload.len < @divExact(@bitSizeOf(e.tag_type), 8)) return error.InvalidPayload; + return @enumFromInt(payload[0]); + }, + .@"struct" => { + const size = @divExact(@bitSizeOf(T), 8); + if (payload.len < size) return error.InvalidPayload; + const Backing = std.meta.Int(.unsigned, @bitSizeOf(T)); + var le: [size]u8 = undefined; + @memcpy(&le, payload[0..size]); + return byteSwapextern(T, @bitCast(std.mem.readInt(Backing, &le, .little))); + }, + else => @compileError("decodePayload: unsupported type " ++ @typeName(T)), + } +} + +/// Convert every multi-byte field of a extern struct between big- and +/// little-endian. Recurses into nested extern structs instead of swapping +/// them as one integer (which would reverse axis1/axis2/axis3), and skips +/// sub-byte fields such as the `_*_padding: u7` slots. +fn byteSwapextern(comptime T: type, v: T) T { + var out = v; + switch (@typeInfo(T)) { + .@"struct" => |s| inline for (s.fields) |f| { + @field(out, f.name) = byteSwapextern(f.type, @field(v, f.name)); + }, + .int => |i| if (i.bits > 8 and i.bits % 8 == 0) { + out = @byteSwap(v); + }, + .float => |fl| { + const U = std.meta.Int(.unsigned, fl.bits); + out = @bitCast(@byteSwap(@as(U, @bitCast(v)))); + }, + .@"enum" => |e| if (@bitSizeOf(e.tag_type) > 8) { + out = @enumFromInt(@byteSwap(@intFromEnum(v))); + }, + .bool => {}, + else => @compileError("byteSwapextern: unsupported type " ++ @typeName(T)), + } + return out; +} diff --git a/src/drivercom.zig b/src/drivercom.zig index 61c4aef..9dbcf25 100644 --- a/src/drivercom.zig +++ b/src/drivercom.zig @@ -5,6 +5,8 @@ pub const version = std.SemanticVersion.parse(build.version) catch unreachable; pub const Config = @import("Config.zig"); +pub const _Config = @import("_Config.zig"); +pub const _Message = @import("_Message.zig"); pub const OldConfig = @import("OldConfig.zig"); pub const Log = @import("Log.zig"); pub const Message = @import("message.zig").Message; diff --git a/src/message.zig b/src/message.zig index 439ded1..9d6fd17 100644 --- a/src/message.zig +++ b/src/message.zig @@ -3,110 +3,6 @@ const std = @import("std"); const Config = @import("Config.zig"); const Log = @import("Log.zig"); -pub const _Message = extern struct { - stx: u8, - kind: Kind, // 8 bits size - len: u32, - sequence: u8, // Sequence ID will be wrapped back to zero - payload: Payload, - etx: u8, - bcc: u8, - - pub const Payload = extern union { - none: void, // Excluded from the Kind - get_config: Config.System.Get, - get_system_state: Config.State.Get, - get_gain_current: Config._CurrentGain.Get, - get_gain_speed: Config._SpeedGain.Get, - get_gain_position: Config._PositionGain.Get, - set_servo_on: bool, - set_config: Config.System.Set, - set_gain_current: Config._CurrentGain.Set, - set_gain_speed: Config._SpeedGain.Set, - set_gain_position: Config._PositionGain.Set, - }; - - pub const Kind = b: { - const ti = @typeInfo(Payload).@"union"; - const tag_type = u8; - var field_names: [ti.fields.len - 1][]const u8 = undefined; - var field_values: [ti.fields.len - 1]tag_type = undefined; - var val: tag_type = 1; - // Do not include none payload as the Kind field - for (ti.fields[1..], 0..) |field, i| { - if (std.mem.eql(u8, "get_config", field.name)) { - val = 0x02; - } - // TODO: Skip any jumping payload values in future - field_names[i] = field.name; - field_values[i] = val; - val += 1; - } - break :b @Enum(tag_type, .nonexhaustive, &field_names, &field_values); - }; - - pub fn PayloadType(comptime kind: Kind) type { - const ti = @typeInfo(Payload).@"union"; - @setEvalBranchQuota(4000); - inline for (ti.fields) |field| { - if (std.mem.eql(u8, "get", field.name[0..3])) { - return void; - } else if (std.mem.eql(u8, field.name, @tagName(kind))) { - return field.type; - } - } - } - - pub fn init( - comptime kind: Kind, - sequence: u16, - p: PayloadType(kind), - ) _Message { - return switch (kind) { - inline else => b: { - var msg: _Message = .{ - .stx = 0x02, - .kind = kind, - .len = @sizeOf(PayloadType(kind)) + 9, - .sequence = sequence, - .payload = undefined, - .etx = 0x03, - .bcc = undefined, - }; - var _payload: Payload = .{ .none = .{} }; - @field(_payload, @tagName(kind)) = p; - msg.payload = @bitCast(_payload); - std.mem.byteSwapAllFields( - PayloadType(kind), - &@field(msg.payload, @tagName(kind)), - ); - msg.bcc = msg.getBcc(); - break :b msg; - }, - }; - } - - pub fn getPayload( - self: *const _Message, - comptime kind: Kind, - ) PayloadType(kind) { - const _payload: Payload = @bitCast(self._payload); - return switch (kind) { - inline else => @field(_payload, @tagName(kind)), - }; - } - - pub fn getBcc(self: *const _Message) u8 { - const bytes: []const u8 = std.mem.asBytes(self); - var bcc: u8 = 0; - // Excluding the STX and BCC value - for (bytes[1 .. bytes.len - 1]) |b| { - bcc ^= b; - } - return bcc; - } -}; - pub const Message = packed struct { kind: Kind, sequence: u16, From d2f192f8aa1ad7a8be30a3f69cb59f5f6355b6fb Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Thu, 1 Oct 2026 14:39:03 +0900 Subject: [PATCH 09/20] refactor: Remove unnecessary functions --- src/_Message.zig | 85 ------------------------------------------------ 1 file changed, 85 deletions(-) diff --git a/src/_Message.zig b/src/_Message.zig index c970bc8..4e03765 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -345,32 +345,6 @@ pub const Response = union(Kind) { set_gain_speed: Ack, set_gain_position: Ack, - /// Decode response payload from the specified payload buffer - fn decode(comptime T: type, payload: []const u8) ParseError!T { - // TODO: validate the buffer is only payload, removing any header message - // and trailing payload buffer - switch (@typeInfo(T)) { - .void => return {}, - .bool => { - if (payload.len < 1) return error.InvalidPayload; - return payload[0] != 0; - }, - .@"enum" => |e| { - if (payload.len < @divExact(@bitSizeOf(e.tag_type), 8)) return error.InvalidPayload; - return @enumFromInt(payload[0]); - }, - .@"struct" => { - const size = @divExact(@bitSizeOf(T), 8); - if (payload.len < size) return error.InvalidPayload; - const Backing = std.meta.Int(.unsigned, @bitSizeOf(T)); - var le: [size]u8 = undefined; - @memcpy(&le, payload[0..size]); - return byteSwapextern(T, @bitCast(std.mem.readInt(Backing, &le, .little))); - }, - else => @compileError("decodePayload: unsupported type " ++ @typeName(T)), - } - } - pub const SystemConfig = extern struct { /// Bytes 7..31 are not used _: [25]u8, @@ -833,16 +807,6 @@ fn PayloadType( return @FieldType(T, @tagName(kind)); } -// pub fn getPayload( -// self: *const Message, -// comptime kind: Kind, -// ) PayloadType(kind) { -// const _payload: Payload = @bitCast(self._payload); -// return switch (kind) { -// inline else => @field(_payload, @tagName(kind)), -// }; -// } - /// Calculate bcc based on the buffer. The buffer must be the message excluding /// the etx (header) and bcc itself. fn getBcc(buf: []const u8) u8 { @@ -853,52 +817,3 @@ fn getBcc(buf: []const u8) u8 { } return bcc; } - -fn decodePayload(comptime T: type, payload: []const u8) ParseError!T { - switch (@typeInfo(T)) { - .void => return {}, - .bool => { - if (payload.len < 1) return error.InvalidPayload; - return payload[0] != 0; - }, - .@"enum" => |e| { - if (payload.len < @divExact(@bitSizeOf(e.tag_type), 8)) return error.InvalidPayload; - return @enumFromInt(payload[0]); - }, - .@"struct" => { - const size = @divExact(@bitSizeOf(T), 8); - if (payload.len < size) return error.InvalidPayload; - const Backing = std.meta.Int(.unsigned, @bitSizeOf(T)); - var le: [size]u8 = undefined; - @memcpy(&le, payload[0..size]); - return byteSwapextern(T, @bitCast(std.mem.readInt(Backing, &le, .little))); - }, - else => @compileError("decodePayload: unsupported type " ++ @typeName(T)), - } -} - -/// Convert every multi-byte field of a extern struct between big- and -/// little-endian. Recurses into nested extern structs instead of swapping -/// them as one integer (which would reverse axis1/axis2/axis3), and skips -/// sub-byte fields such as the `_*_padding: u7` slots. -fn byteSwapextern(comptime T: type, v: T) T { - var out = v; - switch (@typeInfo(T)) { - .@"struct" => |s| inline for (s.fields) |f| { - @field(out, f.name) = byteSwapextern(f.type, @field(v, f.name)); - }, - .int => |i| if (i.bits > 8 and i.bits % 8 == 0) { - out = @byteSwap(v); - }, - .float => |fl| { - const U = std.meta.Int(.unsigned, fl.bits); - out = @bitCast(@byteSwap(@as(U, @bitCast(v)))); - }, - .@"enum" => |e| if (@bitSizeOf(e.tag_type) > 8) { - out = @enumFromInt(@byteSwap(@intFromEnum(v))); - }, - .bool => {}, - else => @compileError("byteSwapextern: unsupported type " ++ @typeName(T)), - } - return out; -} From 4c89cf49d3b115729661e2341f3b1610c6a3115a Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Thu, 1 Oct 2026 16:57:21 +0900 Subject: [PATCH 10/20] feat: Get all configuration (includes gain) --- src/_Config.zig | 4 +- src/_Message.zig | 239 ++++++++++++++++++++++++++++++++++++----------- 2 files changed, 185 insertions(+), 58 deletions(-) diff --git a/src/_Config.zig b/src/_Config.zig index 903f2ae..d9d171f 100644 --- a/src/_Config.zig +++ b/src/_Config.zig @@ -42,6 +42,7 @@ pub const Axis = struct { speed: SpeedGain, position: PositionGain, }, + arrival_threshold: f32, }; pub const Flags = struct { @@ -91,6 +92,5 @@ pub const SpeedGain = struct { pub const PositionGain = struct { p: f32, - denominator: f32, - arrival_threshold: f32, + denominator: u16, }; diff --git a/src/_Message.zig b/src/_Message.zig index 4e03765..dfd54dd 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -63,6 +63,13 @@ pub fn init( }; } +pub fn deinit(self: *Message, gpa: std.mem.Allocator) void { + gpa.free(self.payload); + self.bcc = undefined; + self.etx = undefined; + self.header = undefined; +} + /// Parse incoming response to Message type. Payload are written in Big Endian. /// Caller must call `deinit()` to free allocated payload message. pub fn parse( @@ -104,9 +111,30 @@ pub fn setConfig(self: Message, config: *Config) void { break :get_driver_config .{ .get_driver_config = payload }; }, .get_driver_state => unreachable, - .get_gain_current => unreachable, - .get_gain_speed => unreachable, - .get_gain_position => unreachable, + .get_gain_current => get_gain_current: { + var payload: Response.CurrentGain = std.mem.bytesToValue( + Response.CurrentGain, + self.payload, + ); + std.mem.byteSwapAllFields(Response.CurrentGain, &payload); + break :get_gain_current .{ .get_gain_current = payload }; + }, + .get_gain_speed => get_gain_speed: { + var payload: Response.SpeedGain = std.mem.bytesToValue( + Response.SpeedGain, + self.payload, + ); + std.mem.byteSwapAllFields(Response.SpeedGain, &payload); + break :get_gain_speed .{ .get_gain_speed = payload }; + }, + .get_gain_position => get_gain_position: { + var payload: Response.PositionGain = std.mem.bytesToValue( + Response.PositionGain, + self.payload, + ); + std.mem.byteSwapAllFields(Response.PositionGain, &payload); + break :get_gain_position .{ .get_gain_position = payload }; + }, .set_servo_on => unreachable, .set_driver_config => unreachable, .set_gain_current => unreachable, @@ -115,11 +143,12 @@ pub fn setConfig(self: Message, config: *Config) void { _ => unreachable, }; switch (response) { - .get_driver_config => |message| message.setConfig(config), + inline .get_driver_config, + .get_gain_current, + .get_gain_speed, + .get_gain_position, + => |message| message.setConfig(config), .get_driver_state => unreachable, // TODO - .get_gain_current => unreachable, // TODO - .get_gain_speed => unreachable, // TODO - .get_gain_position => unreachable, // TODO .set_servo_on, .set_driver_config, .set_gain_current, @@ -346,6 +375,54 @@ pub const Response = union(Kind) { set_gain_position: Ack, pub const SystemConfig = extern struct { + fn setConfig(self: SystemConfig, config: *Config) void { + config.id = self.id; + config.station = self.station; + config.baud_rate = self.cc_link_speed; + config.flags = .{ + .home_exists = self.home_exist, + .has_neighbor = .{ + .backward = self.has_neighbor.backward, + .forward = self.has_neighbor.forward, + }, + .use_axis = .{ + .axis2 = self.use_axis.axis2, + .axis3 = self.use_axis.axis3, + }, + .calibration_spare = .{ + .backward = self.calibration_spare.backward, + .forward = self.calibration_spare.forward, + }, + .collision_avoidance = self.collision_avoidance, + .calibration_use = self.calibration_use, + .xts = self.xts, + .flip = self.flip, + .swap = self.swap, + }; + config.line = .{ + .axes = self.line_axes, + .axis_length = self.axis_length, + .slider = .{ + .mass = self.slider_mass, + .length = self.slider_length, + }, + .magnet_pitch = self.magnet_pitch, + }; + config.voltage_warmup = self.voltage_warmup; + config.retry_count = self.retry_count; + config.hall_cutoff_freq = self.hall_cutoff_freq; + config.overcurrent_timeout = self.overcurrent_timeout; + config.pos_offset = self.pos_offset; + config.right_sensor_distance = self.right_sensor_distance; + for (&config.axes, 0..) |*axis, i| { + axis.rs = self.rs.axis(i); + axis.ls = self.ls.axis(i); + axis.kf = self.kf.axis(i); + axis.kbm = self.kbm.axis(i); + axis.max_current = self.max_curr.axis(i); + axis.continuous_current = self.continuous_current.axis(i); + } + } /// Bytes 7..31 are not used _: [25]u8, rs: extern struct { @@ -468,55 +545,6 @@ pub const Response = union(Kind) { right_sensor_distance: f32 align(1), flip: bool, swap: bool, - - fn setConfig(self: SystemConfig, config: *Config) void { - config.id = self.id; - config.station = self.station; - config.baud_rate = self.cc_link_speed; - config.flags = .{ - .home_exists = self.home_exist, - .has_neighbor = .{ - .backward = self.has_neighbor.backward, - .forward = self.has_neighbor.forward, - }, - .use_axis = .{ - .axis2 = self.use_axis.axis2, - .axis3 = self.use_axis.axis3, - }, - .calibration_spare = .{ - .backward = self.calibration_spare.backward, - .forward = self.calibration_spare.forward, - }, - .collision_avoidance = self.collision_avoidance, - .calibration_use = self.calibration_use, - .xts = self.xts, - .flip = self.flip, - .swap = self.swap, - }; - config.line = .{ - .axes = self.line_axes, - .axis_length = self.axis_length, - .slider = .{ - .mass = self.slider_mass, - .length = self.slider_length, - }, - .magnet_pitch = self.magnet_pitch, - }; - config.voltage_warmup = self.voltage_warmup; - config.retry_count = self.retry_count; - config.hall_cutoff_freq = self.hall_cutoff_freq; - config.overcurrent_timeout = self.overcurrent_timeout; - config.pos_offset = self.pos_offset; - config.right_sensor_distance = self.right_sensor_distance; - for (&config.axes, 0..) |*axis, i| { - axis.rs = self.rs.axis(i); - axis.ls = self.ls.axis(i); - axis.kf = self.kf.axis(i); - axis.kbm = self.kbm.axis(i); - axis.max_current = self.max_curr.axis(i); - axis.continuous_current = self.continuous_current.axis(i); - } - } }; pub const SystemState = extern struct { @@ -695,6 +723,35 @@ pub const Response = union(Kind) { }; pub const CurrentGain = extern struct { + fn setConfig(self: CurrentGain, config: *Config) void { + for (&config.axes, 0..) |*ax, i| { + const gain = self.axis(@intCast(i)); + ax.gain.current = .{ + .p = gain.p, + .i = gain.i, + .denominator = gain.denominator, + }; + } + } + + /// Get the gain based on axis index. + fn axis(self: CurrentGain, i: u2) Gain { + switch (i) { + inline 3 => unreachable, + inline else => |idx| { + const axis_name = std.fmt.comptimePrint("axis{}", .{idx + 1}); + const pi = @field(self, axis_name); + return .{ + .p = pi.p, + .i = pi.i, + .denominator = @field(self.denominator, axis_name), + }; + }, + } + } + + const Gain = struct { p: f32, i: f32, denominator: u16 }; + /// Byte 7 is not used _: u8, axis1: extern struct { @@ -719,6 +776,42 @@ pub const Response = union(Kind) { }; pub const SpeedGain = extern struct { + fn setConfig(self: SpeedGain, config: *Config) void { + for (&config.axes, 0..) |*ax, i| { + const gain = self.axis(@intCast(i)); + ax.gain.speed = .{ + .p = gain.p, + .i = gain.i, + .denominator = gain.denominator, + .denominator_pi = gain.denominator_pi, + }; + } + } + + /// Get the gain based on axis index. + fn axis(self: SpeedGain, i: u2) Gain { + switch (i) { + inline 3 => unreachable, + inline else => |idx| { + const axis_name = std.fmt.comptimePrint("axis{}", .{idx + 1}); + const pi = @field(self, axis_name); + return .{ + .p = pi.p, + .i = pi.i, + .denominator = @field(self.denominator, axis_name), + .denominator_pi = @field(self.denominator_pi, axis_name), + }; + }, + } + } + + const Gain = struct { + p: f32, + i: f32, + denominator: u16, + denominator_pi: u16, + }; + _: u8, axis1: extern struct { p: f32 align(1), @@ -747,6 +840,40 @@ pub const Response = union(Kind) { }; pub const PositionGain = extern struct { + fn setConfig(self: PositionGain, config: *Config) void { + for (&config.axes, 0..) |*ax, i| { + const gain = self.axis(@intCast(i)); + ax.gain.position = .{ + .p = gain.p, + .denominator = gain.denominator, + }; + ax.arrival_threshold = gain.arrival_threshold; + } + } + + /// Get the gain based on axis index. + fn axis(self: PositionGain, i: u2) Gain { + switch (i) { + inline 3 => unreachable, + inline else => |idx| { + const axis_name = std.fmt.comptimePrint("axis{}", .{idx + 1}); + return .{ + .p = @field(self.p, axis_name), + .denominator = @field(self.denominator, axis_name), + .arrival_threshold = @field( + self.arrival_threshold, + axis_name, + ), + }; + }, + } + } + + const Gain = struct { + p: f32, + denominator: u16, + arrival_threshold: f32, + }; _: u8, p: extern struct { axis1: f32 align(1), From 733bd76ee6d600b18128711ed074bcc649e4f398 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Fri, 2 Oct 2026 16:21:39 +0900 Subject: [PATCH 11/20] feat: Get driver state --- src/State.zig | 122 +++++++++++++++++++++++++++ src/_Message.zig | 206 ++++++++++++++++++++++------------------------ src/drivercom.zig | 1 + 3 files changed, 223 insertions(+), 106 deletions(-) create mode 100644 src/State.zig diff --git a/src/State.zig b/src/State.zig new file mode 100644 index 0000000..03909f1 --- /dev/null +++ b/src/State.zig @@ -0,0 +1,122 @@ +servo_enabled: bool, +cc_link_enabled: bool, +calibrated: bool, +vdc: f32, +thermo: f32, +theta_offset: f32, +sys_version: f32, +sensor: struct { + home: u8, + id0: u8, + id1: u8, + id2: u8, +}, +synchronization_request_position: struct { + forward: f32, + backward: f32, +}, +axes: [3]Axis, + +pub const Axis = struct { + enabled: bool, + primary: bool, + slider: struct { + id: u8, + state: Fsm, + }, + entrance: Entrance, + overcharge_state: Fsm, + pitch_count: i16, + restart_section_count: i16, + base_position: f32, + mechanical_position: f32, + bias: struct { + a: f32, + b: f32, + }, + sensor: struct { + position: struct { + on: struct { + /// Position of right sensor when slider moves forward and + /// turned on the sensor + forward: f32, + /// Position of left sensor when slider moves backward and + /// turned on the sensor + backward: f32, + }, + off: struct { + /// Position of left sensor when slider moves forward and + /// turned off the sensor + forward: f32, + /// Position of right sensor when slider moves backward and + /// turned off the sensor + backward: f32, + }, + }, + section_count: struct { + restart: i16, + on: struct { + /// Section count of right sensor when slider moves forward and + /// turned on the sensor + forward: i16, + /// Section count of left sensor when slider moves backward and + /// turned on the sensor + backward: i16, + }, + off: struct { + // There is not section count off when moving forward + /// Section count of right sensor when slider moves backward and + /// turned off the sensor + backward: i16, + }, + }, + }, +}; + +pub const Fsm = enum(u8) { + none = 0, + warm_up = 1, + warm_up_comp = 2, + warm_up_fault = 3, + curr_bias = 4, + curr_bias_comp = 5, + fwd_ramp = 8, + fwd_ramp_comp = 9, + fwd_ramp_fault = 10, + bwd_ramp = 11, + bwd_ramp_comp = 12, + bwd_ramp_fault = 13, + curr_step = 20, + curr_step_comp = 21, + curr_step_fault = 22, + speed_step = 23, + speed_step_comp = 24, + speed_step_fault = 25, + pos_step = 26, + pos_step_comp = 27, + pos_step_fault = 28, + pos_prof = 29, + pos_prof_comp = 30, + pos_prof_fault = 31, + fwd_calib = 32, + fwd_calib_comp = 33, + bwd_calib = 34, + bwd_calib_comp = 35, + speed_prof = 40, + speed_prof_comp = 41, + speed_prof_fault = 42, + fwd_slave = 43, + fwd_slave_comp = 44, + bwd_slave = 45, + bwd_slave_comp = 46, + over_charge = 50, + synch_com_error = 51, + _, +}; + +pub const Entrance = enum(u8) { + none = 0, + left = 1, + right = 2, + _, +}; diff --git a/src/_Message.zig b/src/_Message.zig index dfd54dd..f3cd964 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -6,6 +6,7 @@ const std = @import("std"); const builtin = @import("builtin"); const Config = @import("_Config.zig"); +const State = @import("State.zig"); const Message = @This(); const STX = 0x02; @@ -99,62 +100,34 @@ pub fn parse( }; } -/// This function shall be used only for response message +/// Set configuration value from the response message. pub fn setConfig(self: Message, config: *Config) void { - const response: Response = switch (self.header.kind) { - .get_driver_config => get_driver_config: { - var payload: Response.SystemConfig = std.mem.bytesToValue( - Response.SystemConfig, - self.payload, - ); - std.mem.byteSwapAllFields(Response.SystemConfig, &payload); - break :get_driver_config .{ .get_driver_config = payload }; - }, - .get_driver_state => unreachable, - .get_gain_current => get_gain_current: { - var payload: Response.CurrentGain = std.mem.bytesToValue( - Response.CurrentGain, - self.payload, - ); - std.mem.byteSwapAllFields(Response.CurrentGain, &payload); - break :get_gain_current .{ .get_gain_current = payload }; - }, - .get_gain_speed => get_gain_speed: { - var payload: Response.SpeedGain = std.mem.bytesToValue( - Response.SpeedGain, - self.payload, - ); - std.mem.byteSwapAllFields(Response.SpeedGain, &payload); - break :get_gain_speed .{ .get_gain_speed = payload }; - }, - .get_gain_position => get_gain_position: { - var payload: Response.PositionGain = std.mem.bytesToValue( - Response.PositionGain, - self.payload, - ); - std.mem.byteSwapAllFields(Response.PositionGain, &payload); - break :get_gain_position .{ .get_gain_position = payload }; - }, - .set_servo_on => unreachable, - .set_driver_config => unreachable, - .set_gain_current => unreachable, - .set_gain_speed => unreachable, - .set_gain_position => unreachable, - _ => unreachable, - }; - switch (response) { + switch (self.header.kind) { inline .get_driver_config, .get_gain_current, .get_gain_speed, .get_gain_position, - => |message| message.setConfig(config), - .get_driver_state => unreachable, // TODO - .set_servo_on, - .set_driver_config, - .set_gain_current, - .set_gain_speed, - .set_gain_position, - => unreachable, + => |tag| { + const T = PayloadType(tag, .response); + var payload: T = std.mem.bytesToValue(T, self.payload); + // Change the endianness of the payload + std.mem.byteSwapAllFields(T, &payload); + payload.setConfig(config); + }, + else => unreachable, + } +} + +pub fn setState(self: Message, state: *State) void { + switch (self.header.kind) { + inline .get_driver_state => |tag| { + const T = PayloadType(tag, .response); + var payload: T = std.mem.bytesToValue(T, self.payload); + // Change the endianness of the payload + std.mem.byteSwapAllFields(T, &payload); + payload.setState(state); + }, + else => unreachable, } } @@ -548,52 +521,73 @@ pub const Response = union(Kind) { }; pub const SystemState = extern struct { - pub const SliderState = enum(u8) { - none = 0, - warm_up = 1, - warm_up_comp = 2, - warm_up_fault = 3, - curr_bias = 4, - curr_bias_comp = 5, - fwd_ramp = 8, - fwd_ramp_comp = 9, - fwd_ramp_fault = 10, - bwd_ramp = 11, - bwd_ramp_comp = 12, - bwd_ramp_fault = 13, - curr_step = 20, - curr_step_comp = 21, - curr_step_fault = 22, - speed_step = 23, - speed_step_comp = 24, - speed_step_fault = 25, - pos_step = 26, - pos_step_comp = 27, - pos_step_fault = 28, - pos_prof = 29, - pos_prof_comp = 30, - pos_prof_fault = 31, - fwd_calib = 32, - fwd_calib_comp = 33, - bwd_calib = 34, - bwd_calib_comp = 35, - speed_prof = 40, - speed_prof_comp = 41, - speed_prof_fault = 42, - fwd_slave = 43, - fwd_slave_comp = 44, - bwd_slave = 45, - bwd_slave_comp = 46, - over_charge = 50, - synch_com_error = 51, - _, - }; - pub const Entrance = enum(u8) { - none = 0, - left = 1, - right = 2, - _, - }; + fn setState(self: SystemState, state: *State) void { + state.servo_enabled = self.is_servo_on; + state.cc_link_enabled = self.is_cclink_on; + state.calibrated = self.is_calibrate; + state.vdc = self.vdc; + state.thermo = self.thermo; + state.theta_offset = self.theta_offset; + state.sys_version = self.sys_version; + state.synchronization_request_position = .{ + .forward = self.fwd_syncr_info_req_pos, + .backward = self.bwd_syncl_info_req_pos, + }; + state.sensor = .{ + .home = self.sensor.home, + .id0 = self.sensor.id0, + .id1 = self.sensor.id1, + .id2 = self.sensor.id2, + }; + inline for (&state.axes, 0..) |*axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i + 1}); + axis.enabled = @field(self.enable, axis_name); + axis.primary = @field(self.primary_axis, axis_name); + axis.entrance = @field(self.entrance, axis_name); + axis.overcharge_state = @field(self.over_charge_state, axis_name); + axis.pitch_count = @field(self.pitch_cnt, axis_name); + axis.restart_section_count = @field(self.restart_section_cnt, axis_name); + axis.base_position = @field(self.base_pos, axis_name); + axis.mechanical_position = @field(self.mecha_pos, axis_name); + axis.slider = .{ + .id = @field(self.slide_no, axis_name), + .state = @field(self.slide_state, axis_name), + }; + axis.sensor = .{ + .position = .{ + .on = .{ + .backward = @field(self.bwd_lsen_on_pos, axis_name), + .forward = @field(self.fwd_rsen_on_pos, axis_name), + }, + .off = .{ + .backward = @field(self.bwd_rsen_off_pos, axis_name), + .forward = @field(self.fwd_lsen_off_pos, axis_name), + }, + }, + .section_count = .{ + .restart = @field(self.restart_section_cnt, axis_name), + .on = .{ + .forward = @field( + self.fwd_rsen_on_section_cnt, + axis_name, + ), + .backward = @field( + self.bwd_lsen_on_section_cnt, + axis_name, + ), + }, + .off = .{ + .backward = @field( + self.rsen_off_section_cnt, + axis_name, + ), + }, + }, + }; + const bias = @field(self.bias, axis_name); + axis.bias = .{ .a = bias.a, .b = bias.b }; + } + } _: u8, is_servo_on: bool, vdc: f32 align(1), @@ -618,9 +612,9 @@ pub const Response = union(Kind) { }, theta_offset: f32 align(1), slide_state: extern struct { - axis1: SliderState, - axis2: SliderState, - axis3: SliderState, + axis1: State.Fsm, + axis2: State.Fsm, + axis3: State.Fsm, }, is_calibrate: bool, bias: extern struct { @@ -653,9 +647,9 @@ pub const Response = union(Kind) { id2: u8, }, entrance: extern struct { - axis1: Entrance, - axis2: Entrance, - axis3: Entrance, + axis1: State.Entrance, + axis2: State.Entrance, + axis3: State.Entrance, }, pitch_cnt: extern struct { axis1: i16 align(1), @@ -691,9 +685,9 @@ pub const Response = union(Kind) { /// Bytes 162..165: not read. _4: [4]u8, over_charge_state: extern struct { - axis1: SliderState, - axis2: SliderState, - axis3: SliderState, + axis1: State.Fsm, + axis2: State.Fsm, + axis3: State.Fsm, }, fwd_rsen_on_pos: extern struct { axis1: f32 align(1), diff --git a/src/drivercom.zig b/src/drivercom.zig index 9dbcf25..70fbca9 100644 --- a/src/drivercom.zig +++ b/src/drivercom.zig @@ -6,6 +6,7 @@ pub const version = pub const Config = @import("Config.zig"); pub const _Config = @import("_Config.zig"); +pub const State = @import("State.zig"); pub const _Message = @import("_Message.zig"); pub const OldConfig = @import("OldConfig.zig"); pub const Log = @import("Log.zig"); From a7da8b6ac9e7b68e4b6c2e4e27368a5d0a3d5e8f Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 6 Oct 2026 11:48:37 +0900 Subject: [PATCH 12/20] feat: Set whole configuration values and servo state This commit allows drivercom to change whole configuration values of the driver. However, the servo state driver must be "off", otherwise returning `InvalidServoState` --- src/_Message.zig | 124 +++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 109 insertions(+), 15 deletions(-) diff --git a/src/_Message.zig b/src/_Message.zig index f3cd964..fba0304 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -15,7 +15,8 @@ const ETX = 0x03; header: Header, /// Payload is defined as slice of u8 since the payload is not consistent for /// every message. It is preferred over union as union type does not guarantee -/// memory layout. +/// memory layout. Payload must be in big endian before calling `write()` and in +/// big endian after calling `parse()`. payload: []u8, etx: u8, /// Unused when sending message to firmware. @@ -26,7 +27,7 @@ bcc: u8, pub fn write(self: Message, writer: *std.Io.Writer) std.Io.Writer.Error!void { const endian: std.builtin.Endian = .big; try writer.writeStruct(self.header, endian); - try writer.writeSliceEndian(u8, self.payload, endian); + try writer.writeAll(self.payload); try writer.writeInt(u8, self.etx, endian); try writer.writeInt(u8, getBcc(writer.buffered()[1..]), endian); } @@ -38,7 +39,7 @@ pub fn init( comptime message_type: Type, sequence: u8, p: PayloadType(kind, message_type), -) Message { +) std.mem.Allocator.Error!Message { return .{ .header = .{ .stx = STX, @@ -47,13 +48,23 @@ pub fn init( .sequence = sequence, }, .payload = switch (@typeInfo(@TypeOf(p))) { - .@"struct" => try gpa.dupe(u8, std.mem.asBytes(p)), + .@"struct" => payload: { + var payload = p; + std.mem.byteSwapAllFields( + PayloadType(kind, message_type), + &payload, + ); + break :payload try gpa.dupe(u8, std.mem.asBytes(&payload)); + }, .array => array: { if (p.len > 0) { @compileError("Array message must be zero length"); } break :array &p; }, + .bool => bool: { + break :bool try gpa.dupe(u8, std.mem.asBytes(&p)); + }, else => { @compileError("Unexpected value"); }, @@ -176,13 +187,61 @@ pub const Request = union(Kind) { get_gain_current: [0]u8, get_gain_speed: [0]u8, get_gain_position: [0]u8, - set_servo_on: bool, + set_servo: bool, set_driver_config: SystemConfig, set_gain_current: CurrentGain, set_gain_speed: SpeedGain, set_gain_position: PositionGain, pub const SystemConfig = extern struct { + pub fn fromConfig(config: Config) SystemConfig { + var result: SystemConfig = undefined; + inline for (config.axes, 1..) |axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i}); + @field(result.rs, axis_name) = axis.rs; + @field(result.ls, axis_name) = axis.ls; + @field(result.kf, axis_name) = axis.kf; + @field(result.kbm, axis_name) = axis.kbm; + @field(result.max_curr, axis_name) = axis.max_current; + @field( + result.continuous_current, + axis_name, + ) = axis.continuous_current; + } + result.magnet_pitch = config.line.magnet_pitch; + result.slider_mass = config.line.slider.mass; + result.slider_length = config.line.slider.length; + result.axis_length = config.line.axis_length; + result.home_exist = config.flags.home_exists; + result.has_neighbor = .{ + .backward = config.flags.has_neighbor.backward, + .forward = config.flags.has_neighbor.forward, + }; + result.use_axis = .{ + .axis2 = config.flags.use_axis.axis2, + .axis3 = config.flags.use_axis.axis3, + }; + result.id = config.id; + result.calibration_spare = .{ + .backward = config.flags.calibration_spare.backward, + .forward = config.flags.calibration_spare.forward, + }; + result.collision_avoidance = config.flags.collision_avoidance; + result.line_axes = config.line.axes; + result.voltage_warmup = config.voltage_warmup; + result.retry_count = config.retry_count; + result.station = config.station; + result.cc_link_speed = config.baud_rate; + result.hall_cutoff_freq = config.hall_cutoff_freq; + result.overcurrent_timeout = config.overcurrent_timeout; + result.pos_offset = config.pos_offset; + result.calibration_use = config.flags.calibration_use; + result.xts = config.flags.xts; + result.right_sensor_distance = config.right_sensor_distance; + result.flip = config.flags.flip; + result.swap = config.flags.swap; + return result; + } rs: extern struct { axis1: f32 align(1), axis2: f32 align(1), @@ -229,7 +288,7 @@ pub const Request = union(Kind) { forward: bool, }, collision_avoidance: bool, - _unused: u16 align(1), + _unused: u16 align(1) = undefined, continuous_current: extern struct { axis1: f32 align(1), axis2: f32 align(1), @@ -239,13 +298,7 @@ pub const Request = union(Kind) { voltage_warmup: f32 align(1), retry_count: u8, station: u16 align(1), - cc_link_speed: enum(u8) { - @"156 kbps", - @"625 kbps", - @"2.5 Mbps", - @"5 Mbps", - @"10 Mbps", - }, + cc_link_speed: Config.CcLinkSpeed, hall_cutoff_freq: f32 align(1), overcurrent_timeout: f32 align(1), pos_offset: f32 align(1), @@ -257,6 +310,22 @@ pub const Request = union(Kind) { }; pub const SpeedGain = extern struct { + pub fn fromConfig(config: Config) SpeedGain { + var result: SpeedGain = undefined; + inline for (config.axes, 1..) |axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i}); + @field(result, axis_name) = .{ + .p = axis.gain.speed.p, + .i = axis.gain.speed.i, + }; + @field(result.denominator, axis_name) = + axis.gain.speed.denominator; + @field(result.denominator_pi, axis_name) = + axis.gain.speed.denominator_pi; + } + return result; + } + axis1: extern struct { p: f32 align(1), i: f32 align(1), @@ -288,6 +357,18 @@ pub const Request = union(Kind) { }; pub const PositionGain = extern struct { + pub fn fromConfig(config: Config) PositionGain { + var result: PositionGain = undefined; + inline for (config.axes, 1..) |axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i}); + @field(result.p, axis_name) = axis.gain.position.p; + @field(result.deno_wpc, axis_name) = + axis.gain.position.denominator; + @field(result.arrival_threshold, axis_name) = + axis.arrival_threshold; + } + return result; + } p: extern struct { axis1: f32 align(1), axis2: f32 align(1), @@ -310,6 +391,19 @@ pub const Request = union(Kind) { }; pub const CurrentGain = extern struct { + pub fn fromConfig(config: Config) CurrentGain { + var result: CurrentGain = undefined; + inline for (config.axes, 1..) |axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i}); + @field(result, axis_name) = .{ + .p = axis.gain.current.p, + .i = axis.gain.current.i, + }; + @field(result.denominator, axis_name) = + axis.gain.current.denominator; + } + return result; + } axis1: extern struct { p: f32 align(1), i: f32 align(1), @@ -341,7 +435,7 @@ pub const Response = union(Kind) { get_gain_current: CurrentGain, get_gain_speed: SpeedGain, get_gain_position: PositionGain, - set_servo_on: Ack, + set_servo: Ack, set_driver_config: Ack, set_gain_current: Ack, set_gain_speed: Ack, @@ -900,7 +994,7 @@ pub const Kind = enum(u8) { get_gain_current, get_gain_speed, get_gain_position, - set_servo_on, + set_servo, set_driver_config, set_gain_current, set_gain_speed, From 86442a6267448580cc8a362aeb4bb734ee58558d Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 10:14:26 +0900 Subject: [PATCH 13/20] feat: Config holds mutable and immutable values It is not a good approach to separate between the `State` and `Config` into two different type due to some of `State` messages are mutable. Thus, every fields in config are mutable except `state` --- src/State.zig | 122 ------------------------------------- src/_Config.zig | 158 +++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 157 insertions(+), 123 deletions(-) delete mode 100644 src/State.zig diff --git a/src/State.zig b/src/State.zig deleted file mode 100644 index 03909f1..0000000 --- a/src/State.zig +++ /dev/null @@ -1,122 +0,0 @@ -servo_enabled: bool, -cc_link_enabled: bool, -calibrated: bool, -vdc: f32, -thermo: f32, -theta_offset: f32, -sys_version: f32, -sensor: struct { - home: u8, - id0: u8, - id1: u8, - id2: u8, -}, -synchronization_request_position: struct { - forward: f32, - backward: f32, -}, -axes: [3]Axis, - -pub const Axis = struct { - enabled: bool, - primary: bool, - slider: struct { - id: u8, - state: Fsm, - }, - entrance: Entrance, - overcharge_state: Fsm, - pitch_count: i16, - restart_section_count: i16, - base_position: f32, - mechanical_position: f32, - bias: struct { - a: f32, - b: f32, - }, - sensor: struct { - position: struct { - on: struct { - /// Position of right sensor when slider moves forward and - /// turned on the sensor - forward: f32, - /// Position of left sensor when slider moves backward and - /// turned on the sensor - backward: f32, - }, - off: struct { - /// Position of left sensor when slider moves forward and - /// turned off the sensor - forward: f32, - /// Position of right sensor when slider moves backward and - /// turned off the sensor - backward: f32, - }, - }, - section_count: struct { - restart: i16, - on: struct { - /// Section count of right sensor when slider moves forward and - /// turned on the sensor - forward: i16, - /// Section count of left sensor when slider moves backward and - /// turned on the sensor - backward: i16, - }, - off: struct { - // There is not section count off when moving forward - /// Section count of right sensor when slider moves backward and - /// turned off the sensor - backward: i16, - }, - }, - }, -}; - -pub const Fsm = enum(u8) { - none = 0, - warm_up = 1, - warm_up_comp = 2, - warm_up_fault = 3, - curr_bias = 4, - curr_bias_comp = 5, - fwd_ramp = 8, - fwd_ramp_comp = 9, - fwd_ramp_fault = 10, - bwd_ramp = 11, - bwd_ramp_comp = 12, - bwd_ramp_fault = 13, - curr_step = 20, - curr_step_comp = 21, - curr_step_fault = 22, - speed_step = 23, - speed_step_comp = 24, - speed_step_fault = 25, - pos_step = 26, - pos_step_comp = 27, - pos_step_fault = 28, - pos_prof = 29, - pos_prof_comp = 30, - pos_prof_fault = 31, - fwd_calib = 32, - fwd_calib_comp = 33, - bwd_calib = 34, - bwd_calib_comp = 35, - speed_prof = 40, - speed_prof_comp = 41, - speed_prof_fault = 42, - fwd_slave = 43, - fwd_slave_comp = 44, - bwd_slave = 45, - bwd_slave_comp = 46, - over_charge = 50, - synch_com_error = 51, - _, -}; - -pub const Entrance = enum(u8) { - none = 0, - left = 1, - right = 2, - _, -}; diff --git a/src/_Config.zig b/src/_Config.zig index d9d171f..17e688a 100644 --- a/src/_Config.zig +++ b/src/_Config.zig @@ -7,7 +7,7 @@ id: u16, /// CC-Link Station ID station: u16, /// CC-Link baud rate -baud_rate: CcLinkSpeed, +cc_link_speed: CcLinkSpeed, flags: Flags, line: struct { /// Total number of axes in line. @@ -28,8 +28,77 @@ hall_cutoff_freq: f32, overcurrent_timeout: f32, pos_offset: f32, right_sensor_distance: f32, +theta_offset: f32, // Can be changed from `set_calibration_info` command +state: State, // Immutable fields, written from driver +section_count: SectionCount, axes: [3]Axis, +/// Immutable states of driver +pub const State = struct { + servo_enabled: bool, + cc_link_enabled: bool, + calibrated: bool, + vdc: f32, + thermo: f32, + sys_version: f32, + sensor: struct { + home: u8, + id0: u8, + id1: u8, + id2: u8, + }, + synchronization_request_position: struct { + forward: f32, + backward: f32, + }, + section_count: struct { + /// Count at which the secondary axis is switched off, depends on the + /// slider movement direction. + deactivate_secondary: struct { + left: struct { + forward: struct { axis2: i16, axis3: i16 }, + backward: struct { axis1: i16, axis2: i16 }, + }, + right: struct { + forward: struct { axis2: i16, axis3: i16 }, + backward: struct { axis1: i16, axis2: i16 }, + }, + }, + /// Count at which the axis is activated as the primary, depends on + /// the slider movement direction. + activate_primary: struct { + left: struct { + forward: struct { axis1: i16, axis2: i16, axis3: i16 }, + backward: struct { axis1: i16, axis2: i16, axis3: i16 }, + }, + right: struct { + forward: struct { axis1: i16, axis2: i16, axis3: i16 }, + backward: struct { axis1: i16, axis2: i16, axis3: i16 }, + }, + }, + }, + axes: [3]State.Axis, + + pub const Axis = struct { + enabled: bool, + primary: bool, + slider: struct { id: u8, state: Fsm }, + entrance: Entrance, + overcharge_state: Fsm, + pitch_count: i16, + mechanical_position: f32, + // a and b needs confirmation. + bias: struct { a: f32, b: f32 }, + }; +}; + +pub const SectionCount = struct { + /// Count at which the next axis in the travel direction becomes the + /// primary and the current axis becomes the secondary. + skip: i16, + max: struct { axis1: i16, axis2: i16, axis3: i16 }, +}; + pub const Axis = struct { rs: f32, ls: f32, @@ -43,6 +112,45 @@ pub const Axis = struct { position: PositionGain, }, arrival_threshold: f32, + base_position: f32, + sensor: struct { + position: struct { + on: struct { + /// Position of right sensor when slider moves forward and + /// turned on the sensor + forward: f32, + /// Position of left sensor when slider moves backward and + /// turned on the sensor + backward: f32, + }, + off: struct { + /// Position of left sensor when slider moves forward and + /// turned off the sensor + forward: f32, + /// Position of right sensor when slider moves backward and + /// turned off the sensor + backward: f32, + }, + }, + section_count: struct { + on: struct { + /// Section count of right sensor when slider moves forward and + /// turned on the sensor + forward: i16, + /// Section count of left sensor when slider moves backward and + /// turned on the sensor + backward: i16, + }, + off: struct { + /// Section count of left sensor when slider moves forward and + /// turned off the sensor + forward: i16, + /// Section count of right sensor when slider moves backward and + /// turned off the sensor + backward: i16, + }, + }, + }, }; pub const Flags = struct { @@ -94,3 +202,51 @@ pub const PositionGain = struct { p: f32, denominator: u16, }; + +pub const Fsm = enum(u8) { + none = 0, + warm_up = 1, + warm_up_comp = 2, + warm_up_fault = 3, + curr_bias = 4, + curr_bias_comp = 5, + fwd_ramp = 8, + fwd_ramp_comp = 9, + fwd_ramp_fault = 10, + bwd_ramp = 11, + bwd_ramp_comp = 12, + bwd_ramp_fault = 13, + curr_step = 20, + curr_step_comp = 21, + curr_step_fault = 22, + speed_step = 23, + speed_step_comp = 24, + speed_step_fault = 25, + pos_step = 26, + pos_step_comp = 27, + pos_step_fault = 28, + pos_prof = 29, + pos_prof_comp = 30, + pos_prof_fault = 31, + fwd_calib = 32, + fwd_calib_comp = 33, + bwd_calib = 34, + bwd_calib_comp = 35, + speed_prof = 40, + speed_prof_comp = 41, + speed_prof_fault = 42, + fwd_slave = 43, + fwd_slave_comp = 44, + bwd_slave = 45, + bwd_slave_comp = 46, + over_charge = 50, + synch_com_error = 51, + _, +}; + +pub const Entrance = enum(u8) { + none = 0, + left = 1, + right = 2, + _, +}; From a3069cbfcec88d17f4dbddd8985f51eaef05c994 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 11:52:54 +0900 Subject: [PATCH 14/20] feat: Set angle offset and section count --- src/_Config.zig | 2 +- src/_Message.zig | 251 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 252 insertions(+), 1 deletion(-) diff --git a/src/_Config.zig b/src/_Config.zig index 17e688a..bae8289 100644 --- a/src/_Config.zig +++ b/src/_Config.zig @@ -28,7 +28,7 @@ hall_cutoff_freq: f32, overcurrent_timeout: f32, pos_offset: f32, right_sensor_distance: f32, -theta_offset: f32, // Can be changed from `set_calibration_info` command +angle_offset: f32, // Can be changed from `set_calibration_info` command state: State, // Immutable fields, written from driver section_count: SectionCount, axes: [3]Axis, diff --git a/src/_Message.zig b/src/_Message.zig index fba0304..6865b4c 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -187,11 +187,14 @@ pub const Request = union(Kind) { get_gain_current: [0]u8, get_gain_speed: [0]u8, get_gain_position: [0]u8, + get_section_count: [0]u8, set_servo: bool, set_driver_config: SystemConfig, set_gain_current: CurrentGain, set_gain_speed: SpeedGain, set_gain_position: PositionGain, + set_angle_offset: AngleOffset, + set_section_count: SectionCount, pub const SystemConfig = extern struct { pub fn fromConfig(config: Config) SystemConfig { @@ -427,6 +430,131 @@ pub const Request = union(Kind) { /// SHDrv just send 100 without reasons in the code. _const_deno: u16 align(1) = 100, }; + + pub const AngleOffset = extern struct { + pub fn fromConfig(config: Config) AngleOffset { + var result: AngleOffset = undefined; + result.angle_offset = config.angle_offset; + inline for (config.axes, 1..) |axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i}); + const position = axis.sensor.position; + const section_count = axis.sensor.section_count; + @field(result.sensor_off, axis_name) = .{ + .fwd_lsen_off_pos = position.off.forward, + .fwd_lsen_off_section_cnt = section_count.off.forward, + .bwd_rsen_off_pos = position.off.backward, + .bwd_rsen_off_section_cnt = section_count.off.backward, + }; + @field(result.base_pos, axis_name) = axis.base_position; + @field(result.sensor_on, axis_name) = .{ + .fwd_rsen_on_pos = position.on.forward, + .fwd_rsen_on_section_cnt = section_count.on.forward, + .bwd_lsen_on_pos = position.on.backward, + .bwd_lsen_on_section_cnt = section_count.on.backward, + }; + } + return result; + } + angle_offset: f32 align(1), + sensor_off: extern struct { + axis1: extern struct { + fwd_lsen_off_pos: f32 align(1), + fwd_lsen_off_section_cnt: i16 align(1), + bwd_rsen_off_pos: f32 align(1), + bwd_rsen_off_section_cnt: i16 align(1), + }, + axis2: extern struct { + fwd_lsen_off_pos: f32 align(1), + fwd_lsen_off_section_cnt: i16 align(1), + bwd_rsen_off_pos: f32 align(1), + bwd_rsen_off_section_cnt: i16 align(1), + }, + axis3: extern struct { + fwd_lsen_off_pos: f32 align(1), + fwd_lsen_off_section_cnt: i16 align(1), + bwd_rsen_off_pos: f32 align(1), + bwd_rsen_off_section_cnt: i16 align(1), + }, + }, + base_pos: extern struct { + axis1: f32 align(1), + axis2: f32 align(1), + axis3: f32 align(1), + }, + sensor_on: extern struct { + axis1: extern struct { + fwd_rsen_on_pos: f32 align(1), + fwd_rsen_on_section_cnt: i16 align(1), + bwd_lsen_on_pos: f32 align(1), + bwd_lsen_on_section_cnt: i16 align(1), + }, + axis2: extern struct { + fwd_rsen_on_pos: f32 align(1), + fwd_rsen_on_section_cnt: i16 align(1), + bwd_lsen_on_pos: f32 align(1), + bwd_lsen_on_section_cnt: i16 align(1), + }, + axis3: extern struct { + fwd_rsen_on_pos: f32 align(1), + fwd_rsen_on_section_cnt: i16 align(1), + bwd_lsen_on_pos: f32 align(1), + bwd_lsen_on_section_cnt: i16 align(1), + }, + }, + }; + + pub const SectionCount = extern struct { + pub fn fromConfig(config: Config) SectionCount { + const disable = config.state.section_count.deactivate_secondary; + const max = config.section_count.max; + return .{ + .fwd_left_secondary_disable = .{ + .axis2 = disable.left.forward.axis2, + .axis3 = disable.left.forward.axis3, + }, + .fwd_right_secondary_disable = .{ + .axis2 = disable.right.forward.axis2, + .axis3 = disable.right.forward.axis3, + }, + .bwd_left_secondary_disable = .{ + .axis1 = disable.left.backward.axis1, + .axis2 = disable.left.backward.axis2, + }, + .bwd_right_secondary_disable = .{ + .axis1 = disable.right.backward.axis1, + .axis2 = disable.right.backward.axis2, + }, + .max_section_cnt = .{ + .axis1 = max.axis1, + .axis2 = max.axis2, + .axis3 = max.axis3, + }, + .skip_cnt = config.section_count.skip, + }; + } + fwd_left_secondary_disable: extern struct { + axis2: i16 align(1), + axis3: i16 align(1), + }, + fwd_right_secondary_disable: extern struct { + axis2: i16 align(1), + axis3: i16 align(1), + }, + bwd_left_secondary_disable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + }, + bwd_right_secondary_disable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + }, + max_section_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + skip_cnt: i16 align(1), + }; }; pub const Response = union(Kind) { @@ -435,11 +563,14 @@ pub const Response = union(Kind) { get_gain_current: CurrentGain, get_gain_speed: SpeedGain, get_gain_position: PositionGain, + get_section_count: SectionCount, set_servo: Ack, set_driver_config: Ack, set_gain_current: Ack, set_gain_speed: Ack, set_gain_position: Ack, + set_angle_offset: Ack, + set_section_count: Ack, pub const SystemConfig = extern struct { fn setConfig(self: SystemConfig, config: *Config) void { @@ -982,6 +1113,123 @@ pub const Response = union(Kind) { }, }; + pub const SectionCount = extern struct { + fn setConfig(self: SectionCount, config: *Config) void { + const max = self.max_section_cnt; + config.section_count = .{ + .skip = self.skip_cnt, + .max = .{ + .axis1 = max.axis1, + .axis2 = max.axis2, + .axis3 = max.axis3, + }, + }; + const fwd_left_disable = self.fwd_left_secondary_disable; + const fwd_right_disable = self.fwd_right_secondary_disable; + const bwd_left_disable = self.bwd_left_secondary_disable; + const bwd_right_disable = self.bwd_right_secondary_disable; + const fwd_left_enable = self.fwd_left_inactive_primary_enable; + const fwd_right_enable = self.fwd_right_inactive_primary_enable; + const bwd_left_enable = self.bwd_left_inactive_primary_enable; + const bwd_right_enable = self.bwd_right_inactive_primary_enable; + config.state.section_count = .{ + .deactivate_secondary = .{ + .left = .{ + .forward = .{ + .axis2 = fwd_left_disable.axis2, + .axis3 = fwd_left_disable.axis3, + }, + .backward = .{ + .axis1 = bwd_left_disable.axis1, + .axis2 = bwd_left_disable.axis2, + }, + }, + .right = .{ + .forward = .{ + .axis2 = fwd_right_disable.axis2, + .axis3 = fwd_right_disable.axis3, + }, + .backward = .{ + .axis1 = bwd_right_disable.axis1, + .axis2 = bwd_right_disable.axis2, + }, + }, + }, + .activate_primary = .{ + .left = .{ + .forward = .{ + .axis1 = fwd_left_enable.axis1, + .axis2 = fwd_left_enable.axis2, + .axis3 = fwd_left_enable.axis3, + }, + .backward = .{ + .axis1 = bwd_left_enable.axis1, + .axis2 = bwd_left_enable.axis2, + .axis3 = bwd_left_enable.axis3, + }, + }, + .right = .{ + .forward = .{ + .axis1 = fwd_right_enable.axis1, + .axis2 = fwd_right_enable.axis2, + .axis3 = fwd_right_enable.axis3, + }, + .backward = .{ + .axis1 = bwd_right_enable.axis1, + .axis2 = bwd_right_enable.axis2, + .axis3 = bwd_right_enable.axis3, + }, + }, + }, + }; + } + + /// Byte 7 is not used + _: u8, + fwd_left_secondary_disable: extern struct { + axis2: i16 align(1), + axis3: i16 align(1), + }, + fwd_right_secondary_disable: extern struct { + axis2: i16 align(1), + axis3: i16 align(1), + }, + bwd_left_secondary_disable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + }, + bwd_right_secondary_disable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + }, + fwd_left_inactive_primary_enable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + fwd_right_inactive_primary_enable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + bwd_left_inactive_primary_enable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + bwd_right_inactive_primary_enable: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + max_section_cnt: extern struct { + axis1: i16 align(1), + axis2: i16 align(1), + axis3: i16 align(1), + }, + skip_cnt: i16 align(1), + }; + pub const Ack = enum(u8) { success = 0x00, _, @@ -999,6 +1247,9 @@ pub const Kind = enum(u8) { set_gain_current, set_gain_speed, set_gain_position, + set_angle_offset = 0x11, + set_section_count = 0x15, + get_section_count, _, }; From 74d0ff14c62f627b5b5cda19b6ed2caa3ed420db Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 13:57:47 +0900 Subject: [PATCH 15/20] refactor: Write driver state inside config --- src/_Message.zig | 144 +++++++++++++++++++++------------------------- src/drivercom.zig | 1 - 2 files changed, 64 insertions(+), 81 deletions(-) diff --git a/src/_Message.zig b/src/_Message.zig index 6865b4c..3ac1607 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -6,7 +6,6 @@ const std = @import("std"); const builtin = @import("builtin"); const Config = @import("_Config.zig"); -const State = @import("State.zig"); const Message = @This(); const STX = 0x02; @@ -118,6 +117,8 @@ pub fn setConfig(self: Message, config: *Config) void { .get_gain_current, .get_gain_speed, .get_gain_position, + .get_driver_state, + .get_section_count, => |tag| { const T = PayloadType(tag, .response); var payload: T = std.mem.bytesToValue(T, self.payload); @@ -129,19 +130,6 @@ pub fn setConfig(self: Message, config: *Config) void { } } -pub fn setState(self: Message, state: *State) void { - switch (self.header.kind) { - inline .get_driver_state => |tag| { - const T = PayloadType(tag, .response); - var payload: T = std.mem.bytesToValue(T, self.payload); - // Change the endianness of the payload - std.mem.byteSwapAllFields(T, &payload); - payload.setState(state); - }, - else => unreachable, - } -} - pub const ParseError = error{ /// Length message does not match with the whole message MismatchLength, @@ -187,7 +175,6 @@ pub const Request = union(Kind) { get_gain_current: [0]u8, get_gain_speed: [0]u8, get_gain_position: [0]u8, - get_section_count: [0]u8, set_servo: bool, set_driver_config: SystemConfig, set_gain_current: CurrentGain, @@ -195,6 +182,7 @@ pub const Request = union(Kind) { set_gain_position: PositionGain, set_angle_offset: AngleOffset, set_section_count: SectionCount, + get_section_count: [0]u8, pub const SystemConfig = extern struct { pub fn fromConfig(config: Config) SystemConfig { @@ -234,7 +222,7 @@ pub const Request = union(Kind) { result.voltage_warmup = config.voltage_warmup; result.retry_count = config.retry_count; result.station = config.station; - result.cc_link_speed = config.baud_rate; + result.cc_link_speed = config.cc_link_speed; result.hall_cutoff_freq = config.hall_cutoff_freq; result.overcurrent_timeout = config.overcurrent_timeout; result.pos_offset = config.pos_offset; @@ -563,7 +551,6 @@ pub const Response = union(Kind) { get_gain_current: CurrentGain, get_gain_speed: SpeedGain, get_gain_position: PositionGain, - get_section_count: SectionCount, set_servo: Ack, set_driver_config: Ack, set_gain_current: Ack, @@ -571,12 +558,13 @@ pub const Response = union(Kind) { set_gain_position: Ack, set_angle_offset: Ack, set_section_count: Ack, + get_section_count: SectionCount, pub const SystemConfig = extern struct { fn setConfig(self: SystemConfig, config: *Config) void { config.id = self.id; config.station = self.station; - config.baud_rate = self.cc_link_speed; + config.cc_link_speed = self.cc_link_speed; config.flags = .{ .home_exists = self.home_exist, .has_neighbor = .{ @@ -746,71 +734,65 @@ pub const Response = union(Kind) { }; pub const SystemState = extern struct { - fn setState(self: SystemState, state: *State) void { + /// Fills the driver state, the angle offset, and each axis's base + /// position and sensor values. `config.state.section_count` is left + /// as is; `get_section_count` fills it. + fn setConfig(self: SystemState, config: *Config) void { + config.angle_offset = self.theta_offset; + const state = &config.state; state.servo_enabled = self.is_servo_on; state.cc_link_enabled = self.is_cclink_on; state.calibrated = self.is_calibrate; state.vdc = self.vdc; state.thermo = self.thermo; - state.theta_offset = self.theta_offset; state.sys_version = self.sys_version; - state.synchronization_request_position = .{ - .forward = self.fwd_syncr_info_req_pos, - .backward = self.bwd_syncl_info_req_pos, - }; state.sensor = .{ .home = self.sensor.home, .id0 = self.sensor.id0, .id1 = self.sensor.id1, .id2 = self.sensor.id2, }; - inline for (&state.axes, 0..) |*axis, i| { - const axis_name = std.fmt.comptimePrint("axis{}", .{i + 1}); - axis.enabled = @field(self.enable, axis_name); - axis.primary = @field(self.primary_axis, axis_name); - axis.entrance = @field(self.entrance, axis_name); - axis.overcharge_state = @field(self.over_charge_state, axis_name); - axis.pitch_count = @field(self.pitch_cnt, axis_name); - axis.restart_section_count = @field(self.restart_section_cnt, axis_name); - axis.base_position = @field(self.base_pos, axis_name); - axis.mechanical_position = @field(self.mecha_pos, axis_name); - axis.slider = .{ - .id = @field(self.slide_no, axis_name), - .state = @field(self.slide_state, axis_name), - }; - axis.sensor = .{ - .position = .{ - .on = .{ - .backward = @field(self.bwd_lsen_on_pos, axis_name), - .forward = @field(self.fwd_rsen_on_pos, axis_name), - }, - .off = .{ - .backward = @field(self.bwd_rsen_off_pos, axis_name), - .forward = @field(self.fwd_lsen_off_pos, axis_name), - }, - }, - .section_count = .{ - .restart = @field(self.restart_section_cnt, axis_name), - .on = .{ - .forward = @field( - self.fwd_rsen_on_section_cnt, - axis_name, - ), - .backward = @field( - self.bwd_lsen_on_section_cnt, - axis_name, - ), - }, - .off = .{ - .backward = @field( - self.rsen_off_section_cnt, - axis_name, - ), - }, + state.synchronization_request_position = .{ + .forward = self.fwd_syncr_info_req_pos, + .backward = self.bwd_syncl_info_req_pos, + }; + inline for (&config.axes, 1..) |*axis, i| { + const axis_name = std.fmt.comptimePrint("axis{}", .{i}); + const bias = @field(self.bias, axis_name); + state.axes[i - 1] = .{ + .enabled = @field(self.enable, axis_name), + .primary = @field(self.primary_axis, axis_name), + .slider = .{ + .id = @field(self.slide_no, axis_name), + .state = @field(self.slide_state, axis_name), }, + .entrance = @field(self.entrance, axis_name), + .overcharge_state = @field( + self.over_charge_state, + axis_name, + ), + .pitch_count = @field(self.pitch_cnt, axis_name), + .mechanical_position = @field(self.mecha_pos, axis_name), + .bias = .{ .a = bias.a, .b = bias.b }, }; - const bias = @field(self.bias, axis_name); - axis.bias = .{ .a = bias.a, .b = bias.b }; + + axis.base_position = @field(self.base_pos, axis_name); + const position = &axis.sensor.position; + position.on.forward = @field(self.fwd_rsen_on_pos, axis_name); + position.on.backward = @field(self.bwd_lsen_on_pos, axis_name); + position.off.forward = + @field(self.fwd_lsen_off_pos, axis_name); + position.off.backward = + @field(self.bwd_rsen_off_pos, axis_name); + const section_count = &axis.sensor.section_count; + section_count.on.forward = + @field(self.fwd_rsen_on_section_cnt, axis_name); + section_count.on.backward = + @field(self.bwd_lsen_on_section_cnt, axis_name); + section_count.off.forward = + @field(self.fwd_lsen_off_section_cnt, axis_name); + section_count.off.backward = + @field(self.bwd_rsen_off_section_cnt, axis_name); } } _: u8, @@ -837,9 +819,9 @@ pub const Response = union(Kind) { }, theta_offset: f32 align(1), slide_state: extern struct { - axis1: State.Fsm, - axis2: State.Fsm, - axis3: State.Fsm, + axis1: Config.Fsm, + axis2: Config.Fsm, + axis3: Config.Fsm, }, is_calibrate: bool, bias: extern struct { @@ -872,9 +854,9 @@ pub const Response = union(Kind) { id2: u8, }, entrance: extern struct { - axis1: State.Entrance, - axis2: State.Entrance, - axis3: State.Entrance, + axis1: Config.Entrance, + axis2: Config.Entrance, + axis3: Config.Entrance, }, pitch_cnt: extern struct { axis1: i16 align(1), @@ -890,7 +872,9 @@ pub const Response = union(Kind) { }, /// Bytes 126..129: not read (was CaliHome, commented out). _3: [4]u8, - restart_section_cnt: extern struct { + /// This field represent two things in SHDrv: RestartSectionCnt and the + /// following. + fwd_lsen_off_section_cnt: extern struct { axis1: i16 align(1), axis2: i16 align(1), axis3: i16 align(1), @@ -900,7 +884,7 @@ pub const Response = union(Kind) { axis2: f32 align(1), axis3: f32 align(1), }, - rsen_off_section_cnt: extern struct { + bwd_rsen_off_section_cnt: extern struct { axis1: i16 align(1), axis2: i16 align(1), axis3: i16 align(1), @@ -910,9 +894,9 @@ pub const Response = union(Kind) { /// Bytes 162..165: not read. _4: [4]u8, over_charge_state: extern struct { - axis1: State.Fsm, - axis2: State.Fsm, - axis3: State.Fsm, + axis1: Config.Fsm, + axis2: Config.Fsm, + axis3: Config.Fsm, }, fwd_rsen_on_pos: extern struct { axis1: f32 align(1), diff --git a/src/drivercom.zig b/src/drivercom.zig index 70fbca9..9dbcf25 100644 --- a/src/drivercom.zig +++ b/src/drivercom.zig @@ -6,7 +6,6 @@ pub const version = pub const Config = @import("Config.zig"); pub const _Config = @import("_Config.zig"); -pub const State = @import("State.zig"); pub const _Message = @import("_Message.zig"); pub const OldConfig = @import("OldConfig.zig"); pub const Log = @import("Log.zig"); From db5c87571555443c39d9db41a359e7b5998b5585 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 14:26:24 +0900 Subject: [PATCH 16/20] refactor: Change the state order to last --- src/_Config.zig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/_Config.zig b/src/_Config.zig index bae8289..d77d44d 100644 --- a/src/_Config.zig +++ b/src/_Config.zig @@ -29,9 +29,9 @@ overcurrent_timeout: f32, pos_offset: f32, right_sensor_distance: f32, angle_offset: f32, // Can be changed from `set_calibration_info` command -state: State, // Immutable fields, written from driver section_count: SectionCount, axes: [3]Axis, +state: State, // Immutable fields, written from driver /// Immutable states of driver pub const State = struct { From 68c352ec4f4e153fdc1ca25c1669bf225a03f59a Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 15:40:07 +0900 Subject: [PATCH 17/20] refactor: change `set_servo` to `toggle_servo` This is a deliberate change to make the usage in drivercom-cli done in comptime. Now, every `set_xx` command now set something in config and `get_xx` get something from config. --- src/_Message.zig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/_Message.zig b/src/_Message.zig index 3ac1607..9eb1cad 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -175,7 +175,7 @@ pub const Request = union(Kind) { get_gain_current: [0]u8, get_gain_speed: [0]u8, get_gain_position: [0]u8, - set_servo: bool, + toggle_servo: bool, set_driver_config: SystemConfig, set_gain_current: CurrentGain, set_gain_speed: SpeedGain, @@ -551,7 +551,7 @@ pub const Response = union(Kind) { get_gain_current: CurrentGain, get_gain_speed: SpeedGain, get_gain_position: PositionGain, - set_servo: Ack, + toggle_servo: Ack, set_driver_config: Ack, set_gain_current: Ack, set_gain_speed: Ack, @@ -1226,7 +1226,7 @@ pub const Kind = enum(u8) { get_gain_current, get_gain_speed, get_gain_position, - set_servo, + toggle_servo, set_driver_config, set_gain_current, set_gain_speed, From a0ea3de4c191b41184a81ac2a60e728f0c480d89 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 15:41:22 +0900 Subject: [PATCH 18/20] refactor: Make `PayloadType` public --- src/_Message.zig | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/_Message.zig b/src/_Message.zig index 9eb1cad..aa5bfc5 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -130,6 +130,16 @@ pub fn setConfig(self: Message, config: *Config) void { } } +pub fn PayloadType( + comptime kind: Kind, + comptime message_type: Type, +) type { + // Get either `Request` or `Response` type + const T = @FieldType(Payload, @tagName(message_type)); + // Return the payload type + return @FieldType(T, @tagName(kind)); +} + pub const ParseError = error{ /// Length message does not match with the whole message MismatchLength, @@ -1237,7 +1247,7 @@ pub const Kind = enum(u8) { _, }; -pub const Payload = union(Type) { +const Payload = union(Type) { request: Request, response: Response, }; @@ -1247,16 +1257,6 @@ const Type = enum(u2) { response, }; -fn PayloadType( - comptime kind: Kind, - comptime message_type: Type, -) type { - // Get either `Request` or `Response` type - const T = @FieldType(Payload, @tagName(message_type)); - // Return the payload type - return @FieldType(T, @tagName(kind)); -} - /// Calculate bcc based on the buffer. The buffer must be the message excluding /// the etx (header) and bcc itself. fn getBcc(buf: []const u8) u8 { From 44a6585166d49b08a5d23d3a9d563a697d5e7840 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Wed, 7 Oct 2026 16:40:04 +0900 Subject: [PATCH 19/20] refactor: Add comment to `Kind` as a rule --- src/_Message.zig | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/_Message.zig b/src/_Message.zig index aa5bfc5..79b28ab 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -571,7 +571,7 @@ pub const Response = union(Kind) { get_section_count: SectionCount, pub const SystemConfig = extern struct { - fn setConfig(self: SystemConfig, config: *Config) void { + pub fn setConfig(self: SystemConfig, config: *Config) void { config.id = self.id; config.station = self.station; config.cc_link_speed = self.cc_link_speed; @@ -744,10 +744,7 @@ pub const Response = union(Kind) { }; pub const SystemState = extern struct { - /// Fills the driver state, the angle offset, and each axis's base - /// position and sensor values. `config.state.section_count` is left - /// as is; `get_section_count` fills it. - fn setConfig(self: SystemState, config: *Config) void { + pub fn setConfig(self: SystemState, config: *Config) void { config.angle_offset = self.theta_offset; const state = &config.state; state.servo_enabled = self.is_servo_on; @@ -936,7 +933,7 @@ pub const Response = union(Kind) { }; pub const CurrentGain = extern struct { - fn setConfig(self: CurrentGain, config: *Config) void { + pub fn setConfig(self: CurrentGain, config: *Config) void { for (&config.axes, 0..) |*ax, i| { const gain = self.axis(@intCast(i)); ax.gain.current = .{ @@ -989,7 +986,7 @@ pub const Response = union(Kind) { }; pub const SpeedGain = extern struct { - fn setConfig(self: SpeedGain, config: *Config) void { + pub fn setConfig(self: SpeedGain, config: *Config) void { for (&config.axes, 0..) |*ax, i| { const gain = self.axis(@intCast(i)); ax.gain.speed = .{ @@ -1053,7 +1050,7 @@ pub const Response = union(Kind) { }; pub const PositionGain = extern struct { - fn setConfig(self: PositionGain, config: *Config) void { + pub fn setConfig(self: PositionGain, config: *Config) void { for (&config.axes, 0..) |*ax, i| { const gain = self.axis(@intCast(i)); ax.gain.position = .{ @@ -1108,7 +1105,7 @@ pub const Response = union(Kind) { }; pub const SectionCount = extern struct { - fn setConfig(self: SectionCount, config: *Config) void { + pub fn setConfig(self: SectionCount, config: *Config) void { const max = self.max_section_cnt; config.section_count = .{ .skip = self.skip_cnt, @@ -1230,6 +1227,8 @@ pub const Response = union(Kind) { }; }; +/// `get_` prefix gets the information required by the `Config`. `set_` prefix +/// set the mutable configuration to the firmware. pub const Kind = enum(u8) { get_driver_config = 0x02, get_driver_state, @@ -1267,3 +1266,17 @@ fn getBcc(buf: []const u8) u8 { } return bcc; } + +test "get_ responses have setConfig and set_ requests have fromConfig" { + inline for (@typeInfo(Kind).@"enum".fields) |field| { + const kind: Kind = @enumFromInt(field.value); + if (comptime std.mem.startsWith(u8, field.name, "get_")) { + const T = PayloadType(kind, .response); + try std.testing.expect(@hasDecl(T, "setConfig")); + } + if (comptime std.mem.startsWith(u8, field.name, "set_")) { + const T = PayloadType(kind, .request); + try std.testing.expect(@hasDecl(T, "fromConfig")); + } + } +} From 76a731350b3c2de8aba67dd2b403856dfe8e6893 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Thu, 8 Oct 2026 11:04:16 +0900 Subject: [PATCH 20/20] refactor: Add `ping` message --- src/_Message.zig | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/_Message.zig b/src/_Message.zig index 79b28ab..d70fe14 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -180,6 +180,7 @@ pub const Header = extern struct { }; pub const Request = union(Kind) { + ping: [0]u8, get_driver_config: [0]u8, get_driver_state: [0]u8, get_gain_current: [0]u8, @@ -556,6 +557,7 @@ pub const Request = union(Kind) { }; pub const Response = union(Kind) { + ping: Ack, get_driver_config: SystemConfig, get_driver_state: SystemState, get_gain_current: CurrentGain, @@ -1230,7 +1232,8 @@ pub const Response = union(Kind) { /// `get_` prefix gets the information required by the `Config`. `set_` prefix /// set the mutable configuration to the firmware. pub const Kind = enum(u8) { - get_driver_config = 0x02, + ping = 0x01, + get_driver_config, get_driver_state, get_gain_current, get_gain_speed,