State Space Control Swerve Drive

Technical Programming
1

So below I’ve included my code to turn a Swerve Module using State Space Control
The angle wrapping documentation didn’t help me out all that well compared to using the sample code for elevators and flywheels to figure out the flow of the code.

I went with the strategy of handling the angle wrapping to recompute the setpoint and then passing it through the linear system loop procedure (setNextR, correct, predict, getvolts)

This worked as is, but I got goofy behavior on the turnover from -180 to 180 degrees.
I solved the problem by adding linearSystemLoop.getObserver().setXHat(current measurement)

Why wouldn’t just recomputing the angle wrapping and following the normal procedure work?
Why do I have to add the setXhat portion?
Does this seem to be a good solution?

Edit: Also as an aside. I’ve yet to deploy this fully. Testing this on modules first. I think I’m on the right track with tuning my numbers, but I get close and there is a rattling around the setpoint that occurs. I think it might be due to the line with kS. In all of the examples I saw no addition of kS (even in the elevator I was expecting an addition of kG).

Is this something that the model that removes the need for kS? Or was this an oversight in the examples?

        public void turnToPosition(double degrees) {
                double measurementDegrees = getPositionDegrees();

                double measurementRadians = Math.toRadians(measurementDegrees);
                double measurementRadiansPerSecond = Math.toRadians(getVelocityDegreesPerSecond());
                Matrix<N1, N1> measurementPositionVector = Matrix.mat(Nat.N1(), Nat.N1())
                                .fill(measurementRadians);
                double setPointRadians = Math.toRadians(degrees);
                double errorBound = (Math.PI - -Math.PI) / 2.0;
                double difference = MathUtil.inputModulus(setPointRadians - measurementRadians, -errorBound,
                                errorBound);
                setPointRadians = measurementRadians + difference;
                State current = new State(measurementRadians, measurementRadiansPerSecond);
                State goal = new State(setPointRadians, 0.0);
                State achievableSetpoint = exponentialProfile.calculate(0.020, current, goal);
                Matrix<N2, N1> measurementVector = Matrix.mat(Nat.N2(), Nat.N1()).fill(measurementRadians, measurementRadiansPerSecond);
                linearSystemLoop.getObserver().setXhat(measurementVector);
                linearSystemLoop.setNextR(achievableSetpoint.position, achievableSetpoint.velocity);
                linearSystemLoop.correct(measurementPositionVector);
                linearSystemLoop.predict(Constants.dtSeconds);
                double voltage = linearSystemLoop.getU(0);
                voltage += Math.signum(achievableSetpoint.velocity) * kS;
                setInputVoltage(voltage);
        }```
1 Like