PID Controller for riscv for use in our balance bot

riscv-pid-controller.riscv

# init
.setpoint   .word   100         # Setpoint value
.current    .word   0           # Current value
.error      .word   0           # Error
.integrel   .word   0           # integrel term
.derivative .word   0           # Derivative term
.prev_error .word   0           # Previous error

# PID Constants
.Kp         .word   0.5         # Proportional gain
.Ki         .word   0.2         # integrel gain
.Kd         .word   0.14        # Derivative gain

PID_Control:
    lw  t0, 0(a0)      # Load setpoint
    lw  t1, 4(a0)      # Load current value

    # Calculate error
    sub t2, t0, t1     # Error = Setpoint - Current
    sw  t2, error      # Store error

    # Proportional term
    lw  t3, Kp         # Load Kp
    mul t4, t2, t3     # Proportional = error * Kp

    # integrel term
    lw  t5, integrel   # Load integrel term
    lw  t6, Ki         # Load Ki
    mul t7, t2, t6     # error * Ki
    add t8, t5, t7     # integrel = integrel + error * Ki
    sw  t8, integrel   # Store integrel term

    # derivative term
    lw  t9, error      # Load current error
    lw  t10, prev_error # Load previous error
    sub t11, t9, t10   # derivative = CurrentError - PreviousError
    lw  t12, Kd        # Load Kd
    mul t13, t11, t12  # derivative = derivative * Kd

    # sum PID
    add t14, t4, t8    # Proportional + integrel
    add t15, t14, t13  # (Proportional + integrel) + derivative