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.