it dosent;t jiggle to death now i changed the tolerance and the motor…

… knows what do to when it dosen;t know

comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java

@@ -37,6 +37,8 @@
 import org.team100.lib.commands.drivetrain.ResetPose;
 import org.team100.lib.commands.drivetrain.SetRotation;
 import org.team100.lib.commands.drivetrain.for_testing.DriveInACircle;
+import org.team100.lib.commands.drivetrain.for_testing.OscillateDirect;
+import org.team100.lib.commands.drivetrain.for_testing.OscillateForceField;
 import org.team100.lib.commands.drivetrain.for_testing.OscillateProfile;
 import org.team100.lib.commands.drivetrain.manual.DriveManually;
 import org.team100.lib.commands.drivetrain.manual.FieldManualWithNoteRotation;
@@ -287,14 +289,14 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
         // HolonomicProfile hp = new HolonomicProfile(TimedRobot100.LOOP_PERIOD_S, 1, 1,
         // 0.01, 1, 1, 0.01);
         // high cruise but also moderate accel
-        HolonomicProfile hp = new HolonomicProfile(TimedRobot100.LOOP_PERIOD_S, 4, 4, 0.01, 3, 3, 0.01);
+        HolonomicProfile hp = new HolonomicProfile(TimedRobot100.LOOP_PERIOD_S, 1, 1, 0.01, 3, 3, 0.01);
         FullStateDriveController hcontroller = new FullStateDriveController(hlog);
 
         whileTrue(driverControl::fullCycle,
                 // new RepeatCommand(
                 // new SequentialCommandGroup(
-                // new OscillateForceField(m_drive, hcontroller, 1),
-                // new OscillateForceField(m_drive, hcontroller, -1))));
+                // new OscillateForceField(m_drive, halfFullStateController, 1),
+                // new OscillateForceField(m_drive, halfFullStateController, -1))));
 
                 new RepeatCommand(
                         new SequentialCommandGroup(
@@ -306,7 +308,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
         // new OscillatePosition(driveLog, m_drive, maker, controller, 1, viz),
         // new OscillatePosition(driveLog, m_drive, maker, controller, -1, viz))));
 
-        // new OscillateDirect(comLog, m_drive));
+        // whileTrue(driverControl::fullCycle, new OscillateDirect(comLog, m_drive));
         // new Oscillate(comLog, m_drive));
         // new RepeatCommand(
         // new FullCycle(comLog, m_drive, controller, viz)));

lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateForceField.java

@@ -43,11 +43,9 @@ public void initialize() {
         // choose a goal 1m away
         SwerveState start = m_swerve.getState();
         Pose2d startPose = start.pose();
-        // don't rotate
+
         Pose2d endPose = startPose.plus(new Transform2d(m_offsetM, 0, new Rotation2d()));
-        // spin 180 between the endpoints
-        // Pose2d endPose = startPose.plus(new Transform2d(m_offsetM, 0,
-        // GeometryUtil.kRotation180));
+
         m_goal = new SwerveState(endPose);
     }
 

lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateProfile.java

@@ -14,7 +14,7 @@
 
 /** Uses a holonomic profile. */
 public class OscillateProfile extends Command implements Glassy {
-    private static final double TOLERANCE = 0.01;
+    private static final double TOLERANCE = 0.05;
 
     private final SwerveDriveSubsystem m_swerve;
     private final HolonomicProfile m_profile;
@@ -38,6 +38,7 @@ public OscillateProfile(
 
     @Override
     public void initialize() {
+        m_swerve.stop();
         m_controller.reset();
         // choose a goal 1m away
         SwerveState start = m_swerve.getState();

lib/src/main/java/org/team100/lib/controller/drivetrain/FullStateDriveController.java

@@ -15,7 +15,7 @@
 public class FullStateDriveController implements HolonomicFieldRelativeController {
     // TODO: make these parameters not constants
     private static final double kXK1 = 4; // position
-    private static final double kXK2 = 0.25; // velocity
+    private static final double kXK2 = 0.25 * 0; // velocity
     private static final double kThetaK1 = 4; // position
     private static final double kThetaK2 = 0.25; // velocity
     private static final double kXTolerance = 0.01; // 1 cm

lib/src/main/java/org/team100/lib/controller/drivetrain/HolonomicDriveControllerFactory.java

@@ -24,7 +24,7 @@ public static PIDController cartesian() {
         PIDController pid;
         switch (Identity.instance) {
             case COMP_BOT:
-                pid = new PIDController(3, 2, 0);
+                pid = new PIDController(0.5, 0, 0);
                 pid.setIntegratorRange(-0.1, 0.1);
                 pid.setTolerance(0.01); // 1 cm
                 return pid;

lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java

@@ -55,6 +55,8 @@ public class SwerveLocal implements Glassy, SwerveLocalObserver {
     private final ChassisSpeedsLogger m_log_setpoint_delta;
     private final ChassisSpeedsLogger m_log_prev_setpoint;
     private final ChassisSpeedsLogger m_log_setpoint;
+    private final ChassisSpeedsLogger m_log_chassis_speed;
+
 
     private SwerveSetpoint prevSetpoint;
 
@@ -67,6 +69,7 @@ public SwerveLocal(
         m_log_setpoint_delta = child.chassisSpeedsLogger(Level.TRACE, "setpoint delta");
         m_log_prev_setpoint = child.chassisSpeedsLogger(Level.TRACE, "prevSetpoint chassis speed");
         m_log_setpoint = child.chassisSpeedsLogger(Level.DEBUG, "setpoint chassis speed");
+        m_log_chassis_speed = child.chassisSpeedsLogger(Level.TRACE, "chassis speed LOG");
         m_swerveKinodynamics = swerveKinodynamics;
         m_modules = modules;
         m_SwerveSetpointGenerator = new AsymSwerveSetpointGenerator(child, m_swerveKinodynamics);
@@ -234,6 +237,8 @@ public void setChassisSpeedsNormally(ChassisSpeeds speeds, double gyroRateRad_S)
                     prevSetpoint.getModuleStates(), gyroRateRad_S);
         } else {
             states = m_swerveKinodynamics.toSwerveModuleStates(speeds, gyroRateRad_S);
+            // System.out.println(speeds);
+            m_log_chassis_speed.log(() -> speeds);
         }
         setModuleStates(states);
         prevSetpoint = new SwerveSetpoint(speeds, states);

lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100.java

@@ -438,10 +438,11 @@ private SwerveModulePosition100[] constantModulePositions() {
     public SwerveModuleState100[] statesFromVector(SimpleMatrix chassisSpeedsVector) {
         SimpleMatrix moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
         SwerveModuleState100[] moduleStates = new SwerveModuleState100[m_numModules];
+
         for (int i = 0; i < m_numModules; i++) {
             double x = moduleStatesMatrix.get(i * 2, 0);
             double y = moduleStatesMatrix.get(i * 2 + 1, 0);
-            if (Math.abs(x) < 1e-6 && Math.abs(y) < 1e-6) {
+            if (Math.abs(x) < 0.004 && Math.abs(y) < 0.004) {
                 moduleStates[i] = new SwerveModuleState100(0.0, Optional.empty());
             } else {
                 moduleStates[i] = new SwerveModuleState100(Math.hypot(x, y),

lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java

@@ -21,6 +21,7 @@
 public abstract class SwerveModule100 implements Glassy {
     private final LinearVelocityServo m_driveServo;
     private final AngularPositionServo m_turningServo;
+    private Rotation2d previousPosition = new Rotation2d();
 
     protected SwerveModule100(
             LinearVelocityServo driveServo,
@@ -34,8 +35,14 @@ protected SwerveModule100(
      */
     void setDesiredState(SwerveModuleState100 desiredState) {
         OptionalDouble position = m_turningServo.getPosition();
+
+        if(position.isPresent()){
+            previousPosition = new Rotation2d(position.getAsDouble());
+        }
+
         if (position.isEmpty())
-            return;
+            desiredState = new SwerveModuleState100(desiredState.speedMetersPerSecond, Optional.of(previousPosition));
+
         setRawDesiredState(
                 SwerveModuleState100.optimize(
                         desiredState,

lib/src/main/java/org/team100/lib/motion/servo/OnboardAngularPositionServo.java

@@ -14,6 +14,7 @@
 import org.team100.lib.motion.mechanism.RotaryMechanism;
 import org.team100.lib.profile.NullProfile;
 import org.team100.lib.profile.Profile100;
+import org.team100.lib.profile.ProfileWPI;
 import org.team100.lib.state.State100;
 import org.team100.lib.util.Util;
 
@@ -46,6 +47,7 @@ public class OnboardAngularPositionServo implements AngularPositionServo {
 
     /** Profile may be updated at runtime. */
     private Profile100 m_profile = new NullProfile();
+    private ProfileWPI profileTest = new ProfileWPI(40,120);
 
     private State100 m_goal = new State100(0, 0);
     private State100 m_setpointRad = new State100(0, 0);
@@ -151,7 +153,8 @@ public void setPositionWithVelocity(
                 MathUtil.angleModulus(m_setpointRad.x() - measurementPositionRad) + measurementPositionRad,
                 m_setpointRad.v());
 
-        m_setpointRad = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpointRad, m_goal);
+        // m_setpointRad = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpointRad, m_goal);
+        m_setpointRad = profileTest.calculate(0.02, m_setpointRad, m_goal);
 
         final double u_FB;
         if (Experiments.instance.enabled(Experiment.FilterFeedback)) {
@@ -165,6 +168,7 @@ public void setPositionWithVelocity(
                     m_setpointRad.x());
         }
 
+
         final double u_FF = m_setpointRad.v();
         // note u_FF is rad/s, so a big number, u_FB should also be a big number.
 

lib/src/main/java/org/team100/lib/motion/servo/OutboardAngularPositionServo.java

@@ -11,9 +11,12 @@
 import org.team100.lib.logging.LoggerFactory.State100Logger;
 import org.team100.lib.motion.mechanism.RotaryMechanism;
 import org.team100.lib.profile.Profile100;
+import org.team100.lib.profile.ProfileWPI;
 import org.team100.lib.state.State100;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 
 /**
  * Passthrough to outboard closed-loop angular control, using a profile with
@@ -46,6 +49,8 @@ public class OutboardAngularPositionServo implements AngularPositionServo {
     /** Remember that the outboard setpoint "winds up" i.e. it's not in [-pi,pi] */
     private State100 m_setpoint = new State100(0, 0);
 
+    private ProfileWPI profileTest = new ProfileWPI(40,120);
+
     /** Don't forget to set a profile. */
     public OutboardAngularPositionServo(
             LoggerFactory parent,
@@ -102,7 +107,7 @@ public void setPositionWithVelocity(double wrappedGoalRad, double goalVelocity,
 
         // choose a goal which is near the measurement
         // apply error to unwrapped measurement, so goal is [-inf, inf]
-        m_goal = new State100(goalErr + unwrappedMeasurementRad, goalVelocity);
+        m_goal = new State100(0, goalVelocity);
 
         // @sanjan's version from sep 2024 used measurement as setpoint which i think is
         // an error.
@@ -116,7 +121,8 @@ public void setPositionWithVelocity(double wrappedGoalRad, double goalVelocity,
         m_setpoint = new State100(setpointErr + unwrappedMeasurementRad, m_setpoint.v());
 
         // finally compute a new setpoint
-        m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_goal);
+        // m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_goal);
+        m_setpoint = profileTest.calculate(0.02, m_setpoint, m_goal);
 
         m_mechanism.setPosition(m_setpoint.x(), m_setpoint.v(), feedForwardTorqueNm);