|
@@ -1,29 +1,31 @@
|
|
|
`timescale 1ns/1ps
|
|
`timescale 1ns/1ps
|
|
|
|
|
|
|
|
-module axis_travel_monitor (
|
|
|
|
|
|
|
+module axis_travel_monitor #(
|
|
|
|
|
+ // How far beyond the target is considered a serious overrun.
|
|
|
|
|
+ parameter integer OVERRUN_LIMIT = 20
|
|
|
|
|
+)(
|
|
|
input wire clk,
|
|
input wire clk,
|
|
|
input wire reset_n,
|
|
input wire reset_n,
|
|
|
|
|
|
|
|
// Start a new travel segment.
|
|
// Start a new travel segment.
|
|
|
- // On start, current_position becomes the new start_position.
|
|
|
|
|
input wire start,
|
|
input wire start,
|
|
|
|
|
|
|
|
- // Current position, for example encoder pulse count.
|
|
|
|
|
|
|
+ // Current measured position.
|
|
|
input wire signed [31:0] current_position,
|
|
input wire signed [31:0] current_position,
|
|
|
|
|
|
|
|
- // Relative travel command.
|
|
|
|
|
- // +500 = move positive by 500
|
|
|
|
|
- // -500 = move negative by 500
|
|
|
|
|
|
|
+ // Relative requested travel.
|
|
|
|
|
+ // +500 = positive travel
|
|
|
|
|
+ // -500 = negative travel
|
|
|
input wire signed [31:0] target_travel,
|
|
input wire signed [31:0] target_travel,
|
|
|
|
|
|
|
|
- // Allowed target-position error.
|
|
|
|
|
|
|
+ // Allowed target error.
|
|
|
input wire [31:0] tolerance,
|
|
input wire [31:0] tolerance,
|
|
|
|
|
|
|
|
- // Physical limit inputs.
|
|
|
|
|
|
|
+ // Physical limits.
|
|
|
input wire limit_pos,
|
|
input wire limit_pos,
|
|
|
input wire limit_neg,
|
|
input wire limit_neg,
|
|
|
|
|
|
|
|
- // Latched position where this travel started.
|
|
|
|
|
|
|
+ // Position captured when START occurs.
|
|
|
output reg signed [31:0] start_position,
|
|
output reg signed [31:0] start_position,
|
|
|
|
|
|
|
|
// start_position + target_travel
|
|
// start_position + target_travel
|
|
@@ -32,41 +34,45 @@ module axis_travel_monitor (
|
|
|
// current_position - start_position
|
|
// current_position - start_position
|
|
|
output wire signed [31:0] travel,
|
|
output wire signed [31:0] travel,
|
|
|
|
|
|
|
|
- // Travel state
|
|
|
|
|
|
|
+ // Travel state.
|
|
|
output reg busy,
|
|
output reg busy,
|
|
|
|
|
|
|
|
- // Current position is inside target tolerance window.
|
|
|
|
|
|
|
+ // Current position is inside target tolerance.
|
|
|
output wire in_position,
|
|
output wire in_position,
|
|
|
|
|
|
|
|
- // Latched completion flag.
|
|
|
|
|
- // Cleared by next start or reset.
|
|
|
|
|
|
|
+ // Normal completion.
|
|
|
output reg done,
|
|
output reg done,
|
|
|
|
|
|
|
|
- // Latched limit status.
|
|
|
|
|
|
|
+ // Limit status.
|
|
|
output reg limit_hit,
|
|
output reg limit_hit,
|
|
|
output reg limit_pos_hit,
|
|
output reg limit_pos_hit,
|
|
|
- output reg limit_neg_hit
|
|
|
|
|
|
|
+ output reg limit_neg_hit,
|
|
|
|
|
+
|
|
|
|
|
+ // Target was crossed without entering tolerance window.
|
|
|
|
|
+ output reg overshoot,
|
|
|
|
|
+
|
|
|
|
|
+ // Position went too far beyond target.
|
|
|
|
|
+ output reg travel_overrun
|
|
|
);
|
|
);
|
|
|
|
|
|
|
|
- // Keep the direction of the current travel command.
|
|
|
|
|
reg signed [31:0] latched_target_travel;
|
|
reg signed [31:0] latched_target_travel;
|
|
|
|
|
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
- // Actual travelled distance
|
|
|
|
|
|
|
+ // Travel calculation
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
|
|
|
|
|
assign travel =
|
|
assign travel =
|
|
|
current_position - start_position;
|
|
current_position - start_position;
|
|
|
|
|
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
- // Position error
|
|
|
|
|
- //
|
|
|
|
|
- // Use 33 bits so subtraction itself does not overflow when
|
|
|
|
|
- // comparing two signed 32-bit positions.
|
|
|
|
|
|
|
+ // Extend positions to 33 bits before subtraction/comparison.
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
|
|
|
|
|
wire signed [32:0] current_ext;
|
|
wire signed [32:0] current_ext;
|
|
|
wire signed [32:0] target_ext;
|
|
wire signed [32:0] target_ext;
|
|
|
|
|
+ wire signed [32:0] tolerance_ext;
|
|
|
|
|
+ wire signed [32:0] overrun_limit_ext;
|
|
|
|
|
+
|
|
|
wire signed [32:0] position_error;
|
|
wire signed [32:0] position_error;
|
|
|
wire [32:0] abs_position_error;
|
|
wire [32:0] abs_position_error;
|
|
|
|
|
|
|
@@ -76,6 +82,12 @@ module axis_travel_monitor (
|
|
|
assign target_ext =
|
|
assign target_ext =
|
|
|
{target_position[31], target_position};
|
|
{target_position[31], target_position};
|
|
|
|
|
|
|
|
|
|
+ assign tolerance_ext =
|
|
|
|
|
+ {1'b0, tolerance};
|
|
|
|
|
+
|
|
|
|
|
+ assign overrun_limit_ext =
|
|
|
|
|
+ OVERRUN_LIMIT;
|
|
|
|
|
+
|
|
|
assign position_error =
|
|
assign position_error =
|
|
|
current_ext - target_ext;
|
|
current_ext - target_ext;
|
|
|
|
|
|
|
@@ -85,22 +97,64 @@ module axis_travel_monitor (
|
|
|
: position_error;
|
|
: position_error;
|
|
|
|
|
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
- // In-position window
|
|
|
|
|
|
|
+ // Current position is inside allowed target window.
|
|
|
|
|
+ // ------------------------------------------------------------
|
|
|
|
|
+
|
|
|
|
|
+ assign in_position =
|
|
|
|
|
+ (busy || done) &&
|
|
|
|
|
+ (abs_position_error <= tolerance_ext);
|
|
|
|
|
+
|
|
|
|
|
+ // ------------------------------------------------------------
|
|
|
|
|
+ // Overshoot detection
|
|
|
//
|
|
//
|
|
|
- // Example:
|
|
|
|
|
|
|
+ // Positive movement:
|
|
|
//
|
|
//
|
|
|
// target = 1500
|
|
// target = 1500
|
|
|
// tolerance = 5
|
|
// tolerance = 5
|
|
|
//
|
|
//
|
|
|
- // 1495 ... 1505 => in_position = 1
|
|
|
|
|
|
|
+ // >1505 means target window was passed.
|
|
|
|
|
+ //
|
|
|
|
|
+ // Negative movement is the opposite.
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
|
|
|
|
|
- assign in_position =
|
|
|
|
|
- (busy || done) &&
|
|
|
|
|
- (abs_position_error <= {1'b0, tolerance});
|
|
|
|
|
|
|
+ wire positive_overshoot_now;
|
|
|
|
|
+ wire negative_overshoot_now;
|
|
|
|
|
+
|
|
|
|
|
+ assign positive_overshoot_now =
|
|
|
|
|
+ (latched_target_travel > 0) &&
|
|
|
|
|
+ (current_ext > (target_ext + tolerance_ext));
|
|
|
|
|
+
|
|
|
|
|
+ assign negative_overshoot_now =
|
|
|
|
|
+ (latched_target_travel < 0) &&
|
|
|
|
|
+ (current_ext < (target_ext - tolerance_ext));
|
|
|
|
|
+
|
|
|
|
|
+ // ------------------------------------------------------------
|
|
|
|
|
+ // Serious travel-overrun detection.
|
|
|
|
|
+ //
|
|
|
|
|
+ // Default OVERRUN_LIMIT = 20.
|
|
|
|
|
+ //
|
|
|
|
|
+ // target=1500:
|
|
|
|
|
+ // positive overrun at >=1520
|
|
|
|
|
+ //
|
|
|
|
|
+ // target=2500:
|
|
|
|
|
+ // negative overrun at <=2480
|
|
|
|
|
+ // ------------------------------------------------------------
|
|
|
|
|
+
|
|
|
|
|
+ wire positive_overrun_now;
|
|
|
|
|
+ wire negative_overrun_now;
|
|
|
|
|
+
|
|
|
|
|
+ assign positive_overrun_now =
|
|
|
|
|
+ (latched_target_travel > 0) &&
|
|
|
|
|
+ (current_ext >=
|
|
|
|
|
+ (target_ext + overrun_limit_ext));
|
|
|
|
|
+
|
|
|
|
|
+ assign negative_overrun_now =
|
|
|
|
|
+ (latched_target_travel < 0) &&
|
|
|
|
|
+ (current_ext <=
|
|
|
|
|
+ (target_ext - overrun_limit_ext));
|
|
|
|
|
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
- // Main travel state logic
|
|
|
|
|
|
|
+ // Main state logic
|
|
|
// ------------------------------------------------------------
|
|
// ------------------------------------------------------------
|
|
|
|
|
|
|
|
always @(posedge clk or negedge reset_n) begin
|
|
always @(posedge clk or negedge reset_n) begin
|
|
@@ -118,17 +172,19 @@ module axis_travel_monitor (
|
|
|
limit_pos_hit <= 1'b0;
|
|
limit_pos_hit <= 1'b0;
|
|
|
limit_neg_hit <= 1'b0;
|
|
limit_neg_hit <= 1'b0;
|
|
|
|
|
|
|
|
|
|
+ overshoot <= 1'b0;
|
|
|
|
|
+ travel_overrun <= 1'b0;
|
|
|
|
|
+
|
|
|
end else begin
|
|
end else begin
|
|
|
|
|
|
|
|
- // ----------------------------------------------------
|
|
|
|
|
- // New travel command
|
|
|
|
|
- //
|
|
|
|
|
- // A new start always creates a new measurement segment.
|
|
|
|
|
- // ----------------------------------------------------
|
|
|
|
|
|
|
+ // ====================================================
|
|
|
|
|
+ // New travel segment
|
|
|
|
|
+ // ====================================================
|
|
|
|
|
|
|
|
if (start) begin
|
|
if (start) begin
|
|
|
|
|
|
|
|
- start_position <= current_position;
|
|
|
|
|
|
|
+ start_position <=
|
|
|
|
|
+ current_position;
|
|
|
|
|
|
|
|
target_position <=
|
|
target_position <=
|
|
|
current_position + target_travel;
|
|
current_position + target_travel;
|
|
@@ -136,14 +192,16 @@ module axis_travel_monitor (
|
|
|
latched_target_travel <=
|
|
latched_target_travel <=
|
|
|
target_travel;
|
|
target_travel;
|
|
|
|
|
|
|
|
- done <= 1'b0;
|
|
|
|
|
|
|
+ done <= 1'b0;
|
|
|
|
|
|
|
|
- limit_hit <= 1'b0;
|
|
|
|
|
- limit_pos_hit <= 1'b0;
|
|
|
|
|
- limit_neg_hit <= 1'b0;
|
|
|
|
|
|
|
+ limit_hit <= 1'b0;
|
|
|
|
|
+ limit_pos_hit <= 1'b0;
|
|
|
|
|
+ limit_neg_hit <= 1'b0;
|
|
|
|
|
|
|
|
|
|
+ overshoot <= 1'b0;
|
|
|
|
|
+ travel_overrun <= 1'b0;
|
|
|
|
|
|
|
|
- // Zero-distance request is already complete.
|
|
|
|
|
|
|
+ // Zero-distance command is complete immediately.
|
|
|
if (target_travel == 0) begin
|
|
if (target_travel == 0) begin
|
|
|
|
|
|
|
|
busy <= 1'b0;
|
|
busy <= 1'b0;
|
|
@@ -155,52 +213,111 @@ module axis_travel_monitor (
|
|
|
|
|
|
|
|
end
|
|
end
|
|
|
|
|
|
|
|
- end
|
|
|
|
|
-
|
|
|
|
|
- // ----------------------------------------------------
|
|
|
|
|
- // Monitor active travel
|
|
|
|
|
- // ----------------------------------------------------
|
|
|
|
|
|
|
+ end else begin
|
|
|
|
|
+
|
|
|
|
|
+ // =================================================
|
|
|
|
|
+ // Continue monitoring overrun even after the normal
|
|
|
|
|
+ // travel state has stopped.
|
|
|
|
|
+ //
|
|
|
|
|
+ // This allows:
|
|
|
|
|
+ //
|
|
|
|
|
+ // reach target normally
|
|
|
|
|
+ // ↓
|
|
|
|
|
+ // external mechanics keep moving
|
|
|
|
|
+ // ↓
|
|
|
|
|
+ // travel_overrun becomes 1
|
|
|
|
|
+ // =================================================
|
|
|
|
|
|
|
|
- else if (busy) begin
|
|
|
|
|
-
|
|
|
|
|
- // Positive motion:
|
|
|
|
|
- // positive limit is relevant.
|
|
|
|
|
if (
|
|
if (
|
|
|
- (latched_target_travel > 0) &&
|
|
|
|
|
- limit_pos
|
|
|
|
|
|
|
+ (
|
|
|
|
|
+ busy ||
|
|
|
|
|
+ done ||
|
|
|
|
|
+ overshoot ||
|
|
|
|
|
+ limit_hit
|
|
|
|
|
+ ) &&
|
|
|
|
|
+ !travel_overrun
|
|
|
) begin
|
|
) begin
|
|
|
|
|
|
|
|
- busy <= 1'b0;
|
|
|
|
|
- done <= 1'b0;
|
|
|
|
|
|
|
+ if (
|
|
|
|
|
+ positive_overrun_now ||
|
|
|
|
|
+ negative_overrun_now
|
|
|
|
|
+ ) begin
|
|
|
|
|
+
|
|
|
|
|
+ travel_overrun <= 1'b1;
|
|
|
|
|
|
|
|
- limit_hit <= 1'b1;
|
|
|
|
|
- limit_pos_hit <= 1'b1;
|
|
|
|
|
|
|
+ end
|
|
|
|
|
|
|
|
end
|
|
end
|
|
|
|
|
|
|
|
- // Negative motion:
|
|
|
|
|
- // negative limit is relevant.
|
|
|
|
|
- else if (
|
|
|
|
|
- (latched_target_travel < 0) &&
|
|
|
|
|
- limit_neg
|
|
|
|
|
- ) begin
|
|
|
|
|
|
|
+ // =================================================
|
|
|
|
|
+ // Active travel monitoring
|
|
|
|
|
+ // =================================================
|
|
|
|
|
|
|
|
- busy <= 1'b0;
|
|
|
|
|
- done <= 1'b0;
|
|
|
|
|
|
|
+ if (busy) begin
|
|
|
|
|
|
|
|
- limit_hit <= 1'b1;
|
|
|
|
|
- limit_neg_hit <= 1'b1;
|
|
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+ // Positive physical limit has highest priority.
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
|
|
|
- end
|
|
|
|
|
|
|
+ if (
|
|
|
|
|
+ (latched_target_travel > 0) &&
|
|
|
|
|
+ limit_pos
|
|
|
|
|
+ ) begin
|
|
|
|
|
|
|
|
- // Reached requested position.
|
|
|
|
|
- else if (
|
|
|
|
|
- abs_position_error <=
|
|
|
|
|
- {1'b0, tolerance}
|
|
|
|
|
- ) begin
|
|
|
|
|
|
|
+ busy <= 1'b0;
|
|
|
|
|
+ done <= 1'b0;
|
|
|
|
|
|
|
|
- busy <= 1'b0;
|
|
|
|
|
- done <= 1'b1;
|
|
|
|
|
|
|
+ limit_hit <= 1'b1;
|
|
|
|
|
+ limit_pos_hit <= 1'b1;
|
|
|
|
|
+
|
|
|
|
|
+ end
|
|
|
|
|
+
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+ // Negative physical limit.
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+
|
|
|
|
|
+ else if (
|
|
|
|
|
+ (latched_target_travel < 0) &&
|
|
|
|
|
+ limit_neg
|
|
|
|
|
+ ) begin
|
|
|
|
|
+
|
|
|
|
|
+ busy <= 1'b0;
|
|
|
|
|
+ done <= 1'b0;
|
|
|
|
|
+
|
|
|
|
|
+ limit_hit <= 1'b1;
|
|
|
|
|
+ limit_neg_hit <= 1'b1;
|
|
|
|
|
+
|
|
|
|
|
+ end
|
|
|
|
|
+
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+ // Normal arrival inside tolerance window.
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+
|
|
|
|
|
+ else if (
|
|
|
|
|
+ abs_position_error <= tolerance_ext
|
|
|
|
|
+ ) begin
|
|
|
|
|
+
|
|
|
|
|
+ busy <= 1'b0;
|
|
|
|
|
+ done <= 1'b1;
|
|
|
|
|
+
|
|
|
|
|
+ end
|
|
|
|
|
+
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+ // Target was skipped.
|
|
|
|
|
+ //
|
|
|
|
|
+ // This is NOT a normal completion.
|
|
|
|
|
+ // ---------------------------------------------
|
|
|
|
|
+
|
|
|
|
|
+ else if (
|
|
|
|
|
+ positive_overshoot_now ||
|
|
|
|
|
+ negative_overshoot_now
|
|
|
|
|
+ ) begin
|
|
|
|
|
+
|
|
|
|
|
+ busy <= 1'b0;
|
|
|
|
|
+ done <= 1'b0;
|
|
|
|
|
+ overshoot <= 1'b1;
|
|
|
|
|
+
|
|
|
|
|
+ end
|
|
|
|
|
|
|
|
end
|
|
end
|
|
|
|
|
|