I’m attempting to simulate a PID controller for a shooter using the velocity of an encoder in ticks/sec. The relevant parts of the PID are here:
def returnPIDInput(self):
velocity = self.m_Motor1.getSelectedSensorVelocity(0)
SmartDashboard.putNumber("Shooter Velocity", velocity)
return velocity
def usePIDOutput(self, output):
SmartDashboard.putNumber("Shooter Output", output)
return self.m_Motor1.set(output)
def start(self):
self.enable()
self.setOutputRange(0, 1.0)
self.setPercentTolerance(5)
self.setSetpoint(100)
My PhysicsEngine is configured to update the quad_position and quad_velocity like this:
class PhysicsEngine(object):
def __init__(self, physics_controller):
self.physics_controller = physics_controller
self.shooter = motion.LinearMotion('Shooter', 3, 917)
def update_sim(self, hal_data, now, tm_diff):
# Simulate the Shooter
s_motor = hal_data'CAN'][robotmap.SHOOT_MOTOR_1]'value']
s_position = self.shooter.compute(s_motor, tm_diff)
hal_data'CAN'][robotmap.SHOOT_MOTOR_1]'quad_position'] = s_position
hal_data'CAN'][robotmap.SHOOT_MOTOR_1]'quad_velocity'] = getRate(hal_data'CAN'][robotmap.SHOOT_MOTOR_1]'quad_position'],
self.s_last_pos,
tm_diff
)
The getRate() function just returns the ticks/10msec:
def getRate(motor, last_pos, tm_diff):
rate = abs(motor - last_pos) / tm_diff / 100
return int(rate)
The net result is indeed a motor that returns a rate of 27.5 when published to a dashboard (1440 clicks/foot / (.5ft wheel * pi)) / 100msec = 27.5 clicks/10msec).
My “problem” is that there’s no actual flywheel motion so the PID instantaneously achieves full speed and then when the PID switches back to 0, instantaneously achieves 0 rate. My thought is that I need to simulate some sort of inertia but this is proving to be difficult. I can do it with a relatively simple class that only allows a decay at a certain rate but this doesn’t work when the motor is not “on”.
Has anyone simulated a flywheel in the physics module or have thoughts on how to simulate inertia (or maybe just what I’m doing wrong…). Full disclosure: I’m not a math major nor developer by trade so feel free to point out the obvious mistakes in logic or math as well.
Thanks in advance.