import random def to_q8_8(val): """Convert float to Q8.8 integer representation.""" return int(val * 256) def to_q0_8(val): """Convert float to Q0.8 integer representation.""" return int(val * 256) def saturate(val, min_val, max_val): return max(min_val, min(max_val, val)) class PIDController: def __init__(self): self.integral = 0 self.prev_error = 0 self.output_min = -32768 self.output_max = 32767 def reset(self): self.integral = 0 self.prev_error = 0 def compute(self, setpoint, position, kp, ki, kd): # All inputs are integers representing fixed-point values # setpoint, position: Q8.8 (16-bit signed) # kp, ki, kd: Q0.8 (8-bit unsigned) error = setpoint - position # Proportional term # error is Q8.8, kp is Q0.8 -> mult is Q8.16 -> shift right 8 to get Q8.8 p_term = (error * kp) >> 8 # Integral term # Accumulate error scaled by Ki # integral storage should probably have more bits to avoid precision loss, # but for this simple model we'll assume it matches the output width/range for anti-windup # Let's accumulate (error * ki) >> 8 # Integrator term calculation # To match typical hardware implementation: # new_integral = old_integral + (error * ki) >> 8 # Then clamp the integral itself to the output range (anti-windup) # Note: In some implementations, the error itself is accumulated and then scaled. # But (error * ki) >> 8 is more standard for simple fixed point. i_increment = (error * ki) >> 8 self.integral += i_increment # Anti-windup self.integral = saturate(self.integral, self.output_min, self.output_max) # Derivative term # (current_error - prev_error) * kd d_term = ((error - self.prev_error) * kd) >> 8 self.prev_error = error # Sum terms output = p_term + self.integral + d_term # Saturate output output = saturate(output, self.output_min, self.output_max) return output def generate_test_vectors(num_tests=50): pid = PIDController() vectors = [] # 1. Reset Test pid.reset() vectors.append({ 'rst': 1, 'enable': 0, 'setpoint': 0, 'position': 0, 'kp': 0, 'ki': 0, 'kd': 0, 'expected_out': 0 }) # 2. Basic P-Only Test # Kp=1.0 (256), Error=10.0 (2560) -> Output=10.0 (2560) pid.reset() vectors.append({ 'rst': 1, 'enable': 1, # Reset first 'setpoint': 0, 'position': 0, 'kp': 0, 'ki': 0, 'kd': 0, 'expected_out': 0 }) kp, ki, kd = 128, 0, 0 # Kp=0.5 setpoint, position = 25600, 20480 # 100.0, 80.0 -> Error=20.0 (5120) # P = 5120 * 128 >> 8 = 2560 (10.0) vectors.append({ 'rst': 0, 'enable': 1, 'setpoint': setpoint, 'position': position, 'kp': kp, 'ki': ki, 'kd': kd, 'expected_out': 2560 }) # Update internal state for next cycle prediction pid.compute(setpoint, position, kp, ki, kd) # 3. Random Random Tests with continuity # We want to test the integrator, so we should run a sequence # Let's generate a few sequences for _ in range(5): # 5 sequences pid.reset() # Start with a reset in the vector list to sync vectors.append({ 'rst': 1, 'enable': 1, 'setpoint': 0, 'position': 0, 'kp': 0, 'ki': 0, 'kd': 0, 'expected_out': 0 }) # Random gains for this sequence kp = random.randint(0, 255) ki = random.randint(0, 50) # Keep Ki small to see accumulation slowly kd = random.randint(0, 255) setpoint = random.randint(-10000, 10000) position = random.randint(-10000, 10000) # Run for 10 cycles for _ in range(10): # Change position slightly to simulate movement position += random.randint(-500, 500) # Calculate expected output out = pid.compute(setpoint, position, kp, ki, kd) vectors.append({ 'rst': 0, 'enable': 1, 'setpoint': setpoint, 'position': position, 'kp': kp, 'ki': ki, 'kd': kd, 'expected_out': out }) # 4. Saturation Test pid.reset() vectors.append({'rst': 1, 'enable': 1, 'setpoint': 0, 'position': 0, 'kp': 0, 'ki': 0, 'kd': 0, 'expected_out': 0}) kp = 255 setpoint = 30000 position = -30000 # Large error: 60000 # P term: 60000 * 255 >> 8 = 59765 -> Should saturate to 32767 out = pid.compute(setpoint, position, kp, 0, 0) vectors.append({ 'rst': 0, 'enable': 1, 'setpoint': setpoint, 'position': position, 'kp': kp, 'ki': 0, 'kd': 0, 'expected_out': out # Should be 32767 }) return vectors def write_verilog_arrays(vectors): filename = "golden_vectors.vh" with open(filename, "w") as f: f.write("// Generated by generate_golden.py\n") f.write(f"localparam NUM_TESTS = {len(vectors)};\n") f.write("reg [15:0] test_setpoint [0:NUM_TESTS-1];\n") f.write("reg [15:0] test_position [0:NUM_TESTS-1];\n") f.write("reg [7:0] test_kp [0:NUM_TESTS-1];\n") f.write("reg [7:0] test_ki [0:NUM_TESTS-1];\n") f.write("reg [7:0] test_kd [0:NUM_TESTS-1];\n") f.write("reg test_rst [0:NUM_TESTS-1];\n") f.write("reg test_enable [0:NUM_TESTS-1];\n") f.write("reg [15:0] expected_out [0:NUM_TESTS-1];\n") f.write("\n") f.write("initial begin\n") for i, v in enumerate(vectors): f.write(f" test_rst[{i}] = {v['rst']};\n") f.write(f" test_enable[{i}] = {v['enable']};\n") # Handle signed 16-bit values for Verilog hex sp = v['setpoint'] & 0xFFFF pos = v['position'] & 0xFFFF out = v['expected_out'] & 0xFFFF f.write(f" test_setpoint[{i}] = 16'h{sp:04X};\n") f.write(f" test_position[{i}] = 16'h{pos:04X};\n") f.write(f" test_kp[{i}] = 8'h{v['kp']:02X};\n") f.write(f" test_ki[{i}] = 8'h{v['ki']:02X};\n") f.write(f" test_kd[{i}] = 8'h{v['kd']:02X};\n") f.write(f" expected_out[{i}] = 16'h{out:04X};\n") f.write("end\n") print(f"Generated {len(vectors)} test vectors to {filename}") if __name__ == "__main__": vectors = generate_test_vectors() write_verilog_arrays(vectors)