Looking for comments suggestions. Other stuff. I think I got it pretty close.
Too handle the signum function one has to pass in a Matrix supplier. I’ll update that in just a second.
Taking an arbitrary supplier like that means it no longer supports just affine systems, so AffineSystem is a misnomer. SysId ended up making separate classes for each simulation, like this:
Good point. Perhaps DCMotorSimAffine? Another work other than Affine.
I just now realised all of the help was in the SysId source code.
I’ll make the appropriate changes.
Thanks again.
Alright. I’ve added some simple control and cleaned some bugs up. Again comments, opinions. Thought, prayers, hopes and dream 
So in terms of using LQR to control this. Since it’s not really linear would I have to make version ofthe Kalman filter, LinearQuadraticRegulator and the LinearSystemLoop to account for this, are there existing replacements, or could I use it with just minor type changes with the existing items.
Use a feedforward to cancel out the affine and nonlinear parts, then just spam LQR on the linear model.
1 Like
Sounds good. The only other thought I had was with the encoder resolution on the motor. Like the NEO and NEO550 have a rotation resolution of 1 rot/ 42 ticks.
Is there a way to factor that into a the model or should it be handled more at the control level. Like don’t try to got to a value that doesn’t exist in terms of position sort of thing.
The feedforward constants absorb the gearing.
So I’ve been playing around with a model using KS, KG, and arm angle for about 3 weeks now for simple motors, elevators, and arms. I’ve noticed that when idling (no input voltage) after moving that the velocity in the simulator toggles between some small positive RPM and the same Negative. Like 15.7 RPM and -15.7 RPM. I’m assuming this is because of the way that the model iterates with the kS value. I’ve been trying to come up with a way to eliminate this (or whether or not eliminating it even makes sense from a physical standpoint). I’m going to drop the current state of the code as an attachment
NonLinearSimpleMotorSim.java (7.8 KB)
.
There’s no reason to include kS in a simulation; it’s there to counteract static friction, and you can instead just not simulate any friction to begin with.
Should I compensate for it in feedforward though in a simulation
Like use the kS, but just pretend the motor is oblivious
Why bother including it to not use it?
I guess what I’m getting at is when I’m simulating don’t use kS and when running a real robot use kS
Yes, that’s what you should do. You can always have a separate set of constants for simulation and just have kS set to zero for those.
Thank you. I guess I just wanted a really accurate simulator. And dove down a bunch of rabbit holes. I have versions of these with kG and arm angle, I’ll keep those as you can’t shut off gravity.
1 Like
Just a thought I had. Would it make sense to cap the simulated voltage to max - kS and min + kS as a way of using it in the simulator.
It wouldn’t gain you anything; at best you introduce exactly the same thing that your feedforward cancels out, and the simulation is unchanged.
Separate note on Pid tuning. Should the gains be tuned in a regular Pid controller first before it’s made profiled or does it not matter
So I’ve tried the following code 2 ways. Its purpose is to turn a motor to a setpoint. When I set it to 90 degrees I overshoot by about 5 degrees with my currently collected data.
public void turnToPosition(State measurement, State goal) {
double voltageFB = this.positionPIDController.calculate(measurement.position, goal);
State setpointState = this.positionPIDController.getSetpoint();
double voltageFF = this.simpleMotorFeedforward.calculate(measurement.velocity, setpointState .velocity, 0.020);
this.motorVoltageControl.setInputVoltage(voltageFF + voltageFB);
}
When I try this
public void turnToPosition(State measurement, State goal) {
double voltageFB = this.positionPIDController.calculate(measurement.position, goal);
State setpointState = this.positionPIDController.getSetpoint();
double voltageFF = this.simpleMotorFeedforward.calculate(measurement.velocity, goal.velocity, 0.020);
this.motorVoltageControl.setInputVoltage(voltageFF + voltageFB);
}
It tracks perfectly. The difference being I use the profiledPIDController’s setpoint instead of the goal in the feed forward.