Merge pull request #528 from Coolearthsky/visualServoing

Visual servoing

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

@@ -33,6 +33,7 @@
 import org.team100.lib.async.Async;
 import org.team100.lib.async.AsyncFactory;
 import org.team100.lib.commands.AllianceCommand;
+import org.team100.lib.commands.drivetrain.DriveWithProfile2;
 import org.team100.lib.commands.drivetrain.DriveWithProfileRotation;
 import org.team100.lib.commands.drivetrain.FancyTrajectory;
 import org.team100.lib.commands.drivetrain.ResetPose;
@@ -96,6 +97,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -267,7 +269,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
 
         whileTrue(driverControl::driveToNote,
                 new ParallelDeadlineGroup(new DriveWithProfileRotation(
-                        () -> Optional.of(new Translation2d()),
+                        noteListener::getClosestTranslation2d,
                         m_drive,
                         halfFullStateController,
                         swerveKinodynamics), intake.run(intake::intakeSmart)));
@@ -400,7 +402,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
                         swerveKinodynamics,
                         gyro,
                         driverControl::desiredRotation,
-                        new double[] { 5.0, 0.5 }));
+                        new double[] { 5.0, 1 }));
 
         driveManually.register("SNAPS_MIN_TIME", true,
                 new ManualWithMinTimeHeading(

lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfile2.java

@@ -40,6 +40,7 @@ public class DriveWithProfile2 extends Command implements Glassy {
     private final double dt = 0.02;
     private double sx = 1;
     private double sy = 1;
+    private boolean wheelsAtCorrectPos;
 
     private State100 xSetpoint;
     private State100 ySetpoint;
@@ -60,21 +61,23 @@ public DriveWithProfile2(
         m_limits = limits;
         xProfile = new TrapezoidProfile100(
                 m_limits.getMaxDriveVelocityM_S(),
-                m_limits.getMaxDriveAccelerationM_S2(),
+                m_limits.getMaxDriveAccelerationM_S2() /2,
                 kTranslationalToleranceM);
         yProfile = new TrapezoidProfile100(
                 m_limits.getMaxDriveVelocityM_S(),
-                m_limits.getMaxDriveAccelerationM_S2(),
+                m_limits.getMaxDriveAccelerationM_S2()/2,
                 kTranslationalToleranceM);
         thetaProfile = new TrapezoidProfile100(
                 m_limits.getMaxAngleSpeedRad_S(),
-                m_limits.getMaxAngleAccelRad_S2(),
+                m_limits.getMaxAngleAccelRad_S2()/2,
                 kRotationToleranceRad);
         addRequirements(m_swerve);
     }
 
     @Override
     public void initialize() {
+        wheelsAtCorrectPos = false;
+
         xSetpoint = m_swerve.getState().x();
         ySetpoint = m_swerve.getState().y();
         thetaSetpoint = m_swerve.getState().theta();
@@ -93,10 +96,10 @@ public void initialize() {
         double slowETA = Math.max(tx, ty);
 
         sx = TrapezoidProfile100.solveForSlowerETA(
-                m_limits.getMaxDriveVelocityM_S(), m_limits.getMaxDriveAccelerationM_S2(), kTranslationalToleranceM, dt,
+                m_limits.getMaxDriveVelocityM_S(), m_limits.getMaxDriveAccelerationM_S2()/2, kTranslationalToleranceM, dt,
                 xSetpoint, m_xGoalRaw, slowETA, kTranslationalToleranceM_S);
         sy = TrapezoidProfile100.solveForSlowerETA(
-                m_limits.getMaxDriveVelocityM_S(), m_limits.getMaxDriveAccelerationM_S2(), kTranslationalToleranceM, dt,
+                m_limits.getMaxDriveVelocityM_S(), m_limits.getMaxDriveAccelerationM_S2()/2, kTranslationalToleranceM, dt,
                 ySetpoint, m_yGoalRaw, slowETA, kTranslationalToleranceM_S);
 
         xProfile = xProfile.scale(sx);
@@ -105,6 +108,7 @@ public void initialize() {
 
     @Override
     public void execute() {
+
         Rotation2d currentRotation = m_swerve.getState().pose().getRotation();
         // take the short path
         double measurement = currentRotation.getRadians();
@@ -117,6 +121,24 @@ public void execute() {
         Rotation2d bearing = new Rotation2d(
                 Math100.getMinDistance(measurement, fieldRelativeGoal.getRotation().getRadians()));
 
+        if (!wheelsAtCorrectPos) {
+            if (atRest()) {
+                m_thetaGoalRaw = new State100(bearing.getRadians(), 0);
+                m_xGoalRaw = new State100(fieldRelativeGoal.getX(), 0, 0);
+                State100 xs = xProfile.calculate(TimedRobot100.LOOP_PERIOD_S, xSetpoint, m_xGoalRaw);
+
+                m_yGoalRaw = new State100(fieldRelativeGoal.getY(), 0, 0);
+                State100 ys = yProfile.calculate(TimedRobot100.LOOP_PERIOD_S, ySetpoint, m_yGoalRaw);
+
+                State100 thetas = thetaProfile.calculate(TimedRobot100.LOOP_PERIOD_S, thetaSetpoint, m_thetaGoalRaw);
+                SwerveState goalState = new SwerveState(xs, ys, thetas);
+                FieldRelativeVelocity goal = m_controller.calculate(m_swerve.getState(), goalState);
+                wheelsAtCorrectPos = m_swerve.steerAtRest(goal);
+                return;
+            } else {
+                wheelsAtCorrectPos = true;
+            }
+        }
         // make sure the setpoint uses the modulus close to the measurement.
         thetaSetpoint = new State100(
                 Math100.getMinDistance(measurement, thetaSetpoint.x()),
@@ -138,18 +160,19 @@ public void execute() {
     @Override
     public boolean isFinished() {
         if (!m_fieldRelativeGoal.get().isPresent()) return true;
-        State100 x = m_swerve.getState().x();
-        double xError = m_xGoalRaw.x() - x.x();
-        State100 y = m_swerve.getState().y();
-        double yError = m_yGoalRaw.x() - y.x();
-        State100 theta = m_swerve.getState().theta();
-        double thetaError = m_thetaGoalRaw.x() - theta.x();
+        double xError = m_xGoalRaw.x() - m_swerve.getState().x().x();
+        double yError = m_yGoalRaw.x() - m_swerve.getState().y().x();
+        double thetaError = m_thetaGoalRaw.x() - m_swerve.getState().theta().x();
         return Math.abs(xError) < kTranslationalToleranceM
                 && Math.abs(yError) < kTranslationalToleranceM
                 && Math.abs(thetaError) < kRotationToleranceRad
-                && Math.abs(x.v()) < kTranslationalToleranceM_S
-                && Math.abs(y.v()) < kTranslationalToleranceM_S
-                && Math.abs(theta.v()) < kRotationToleranceRad_S;
+                && atRest();
+    }
+
+    public boolean atRest() {
+        return Math.abs(m_swerve.getState().x().v()) < kTranslationalToleranceM_S
+        && Math.abs(m_swerve.getState().y().v()) < kTranslationalToleranceM_S
+        && Math.abs(m_swerve.getState().theta().v()) < kRotationToleranceRad_S;
     }
 
     @Override

lib/src/main/java/org/team100/lib/localization/VisionDataProvider24.java

@@ -123,13 +123,14 @@ public void update() {
             NetworkTableValue v = ve.value;
             String name = ve.getTopic().getName();
             String[] fields = name.split("/");
-            if (fields.length != 3)
+            if (fields.length != 4) {
                 return;
+            }
             if (fields[2].equals("fps")) {
                 // FPS is not used by the robot
             } else if (fields[2].equals("latency")) {
                 // latency is not used by the robot
-            } else if (fields[2].equals("blips")) {
+            } else if (fields[3].equals("blips")) {
                 // decode the way StructArrayEntryImpl does
                 byte[] b = v.getRaw();
                 if (b.length == 0)

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

@@ -94,7 +94,7 @@ public class SwerveKinodynamics implements Glassy {
         m_radius = Math.hypot(track / 2, m_wheelbase / 2);
         m_kinematics = get(m_fronttrack, m_backtrack, m_wheelbase, frontoffset);
         // fulcrum is the distance from the center to the nearest edge.
-        double fulcrum = Math.min(m_fronttrack / 2, m_wheelbase / 2);
+        double fulcrum = Math.min(Math.min(m_fronttrack,m_backtrack) / 2, m_wheelbase / 2);
         m_MaxCapsizeAccelM_S2 = 9.8 * (fulcrum / m_vcg);
 
         setMaxDriveVelocityM_S(maxDriveVelocity);
@@ -147,7 +147,7 @@ public class SwerveKinodynamics implements Glassy {
         m_radius = Math.hypot((fronttrack + backtrack) / 4, m_wheelbase / 2);
         m_kinematics = get(m_fronttrack, m_backtrack, m_wheelbase, m_frontoffset);
         // fulcrum is the distance from the center to the nearest edge.
-        double fulcrum = Math.min(m_fronttrack / 2, m_wheelbase / 2);
+        double fulcrum = Math.min(Math.min(m_fronttrack,m_backtrack) / 2, m_wheelbase / 2);
         m_MaxCapsizeAccelM_S2 = 9.8 * (fulcrum / m_vcg);
 
         setMaxDriveVelocityM_S(maxDriveVelocity);

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

@@ -61,8 +61,9 @@ public static SwerveKinodynamics get() {
                         20, // max decel m/s/s
                         40, // steering rate rad/s
                         40 * Math.PI, // steering accel rad/s/s
-                        0.449, // track m
-                        0.464, // wheelbase m
+                        0.49, // front track m
+                        0.44, // back track m
+                        0.462, // wheelbase m
                         .232, // front offset m
                         0.3); // vcg m
             case BLANK:

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

@@ -135,7 +135,7 @@ public static SwerveModuleCollection get(
                                 DriveRatio.FAST, AnalogTurningEncoder.class,
                                 33,
                                 0,
-                                0.46478,
+                                0.489393,
                                 kinodynamics,
                                 EncoderDrive.DIRECT, MotorPhase.REVERSE));
             case BETA_BOT:

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

@@ -121,7 +121,7 @@ public static WCPSwerveModule100 getFalconDrive(
         //
         // note the outboard steering setting!
         //
-        final boolean useOutboardSteering = false;
+        final boolean useOutboardSteering = true;
         LinearVelocityServo driveServo = driveFalconServo(
                 parent.child("Drive"),
                 supplyLimitAmps,

lib/src/main/java/org/team100/lib/sensors/ReduxGyro.java

@@ -17,17 +17,19 @@ public class ReduxGyro implements Gyro {
     private final DoubleLogger m_log_pitch;
     private final DoubleLogger m_log_roll;
     private final DoubleLogger m_log_yaw_deg;
+    private final DoubleLogger m_log_yaw_rate_deg;
     private final DoubleLogger m_log_pitch_deg;
     private final DoubleLogger m_log_roll_deg;
     public ReduxGyro(LoggerFactory parent) {
         LoggerFactory child = parent.child(this);
         m_gyro = new Canandgyro(60);
-        m_gyro.setYaw(0);
+        m_gyro.setYaw(0);   
         m_log_heading = child.doubleLogger(Level.TRACE, "Heading NWU (rad)");
         m_log_heading_rate = child.doubleLogger(Level.TRACE, "Heading Rate NWU (rad_s)");
         m_log_pitch = child.doubleLogger(Level.TRACE, "Pitch NWU (rad)");
         m_log_roll = child.doubleLogger(Level.TRACE, "Roll NWU (rad)");
         m_log_yaw_deg = child.doubleLogger(Level.DEBUG, "Yaw NED (deg)");
+        m_log_yaw_rate_deg = child.doubleLogger(Level.DEBUG, "Yaw Rate NED (deg)");
         m_log_pitch_deg = child.doubleLogger(Level.TRACE, "Pitch (deg)");
         m_log_roll_deg = child.doubleLogger(Level.TRACE, "Roll (deg)");
 
@@ -63,14 +65,14 @@ public Rotation2d getRollNWU() {
     }
 
     private double getYawNEDDeg() {
-        double yawDeg = 360 * m_gyro.getYaw();
+        double yawDeg = -360 * m_gyro.getYaw();
         m_log_yaw_deg.log(() -> yawDeg);
         return yawDeg;
     }
 
     private double getYawVelocityNEDDeg() {
-        double yawDeg = 360 * m_gyro.getAngularVelocityYaw();
-        m_log_yaw_deg.log(() -> yawDeg);
+        double yawDeg = -360 * m_gyro.getAngularVelocityYaw();
+        m_log_yaw_rate_deg.log(() -> yawDeg);
         return yawDeg;
     }
 
@@ -90,8 +92,5 @@ private double getRollDeg() {
     }
 
     @Override
-    public void periodic() {
-        // TODO Auto-generated method stub
-        throw new UnsupportedOperationException("Unimplemented method 'periodic'");
-    }
+    public void periodic() {}
 }

raspberry_pi/app/camera/real_camera.py

@@ -230,7 +230,7 @@ def __size_from_model(model: Model) -> Size:
 
             case Model.GS:
                 return Size(
-                    sensor_width=1456, sensor_height=1088, width=1456, height=1088
+                    sensor_width=1456, sensor_height=1088, width=1408, height=1088
                 )
 
             case _:
@@ -325,7 +325,7 @@ def __dist_from_model(model: Model) -> Mat:
             case Model.V2:
                 return np.array([[-0.003, 0.04, 0, 0]])
             case Model.GS:
-                return np.array([[0, 0, 0, 0]])
+                return np.array([[-0.2, -0.4, 0, 0]])
             case _:
                 return np.array([[0, 0, 0, 0]])
 

raspberry_pi/app/main.py

@@ -15,11 +15,8 @@
 from app.camera.interpreter_factory import InterpreterFactory
 from app.camera.camera_loop import CameraLoop
 from app.config.identity import Identity
-from app.dashboard.real_display import RealDisplay
 from app.framework.looper import Looper
 from app.network.real_network import RealNetwork
-from app.localization.note_detector import NoteDetector
-from app.localization.tag_detector import TagDetector
 from app.sensors.gyro_factory import GyroFactory
 from app.sensors.gyro_loop import GyroLoop
 

raspberry_pi/app/network/real_network.py

@@ -53,7 +53,7 @@ def __init__(self, identity: Identity) -> None:
 
         # roboRio address. windows machines can impersonate this for simulation.
         # also localhost for testing
-        self._inst.setServer(["10.1.0.2", "127.0.0.1"])
+        self._inst.setServer("10.1.0.2")
 
     @override
     def get_double_sender(self, name: str) -> RealDoubleSender: