Skip to content
This repository has been archived by the owner on Jan 8, 2025. It is now read-only.

Commit

Permalink
feat: add Valkyrie map and general clean-up
Browse files Browse the repository at this point in the history 
This adds drive to the ValkyrieMap with the latest magnet offsets, etc.

It also includes some other clean-up in various files.
  • Loading branch information
@bot190
bot190 committed Jan 3, 2024
1 parent 82473d0 commit aa69ad4
Show file tree
Hide file tree
Showing 7 changed files with 116 additions and 62 deletions.
4 changes: 2 additions & 2 deletions build.gradle
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4"
id "com.chopshop166.plugin" version "0.9"
}

Expand Down Expand Up @@ -60,7 +60,7 @@ repositories {
}

chopshop {
version = "2024.1.1-alpha2"
version = "2024.1.1-alpha4"
}

// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ public Command testDriveDriver() {
public Command stowArmCloseIntake() {
return sequence(
armExtend.moveTo(ArmPresets.ARM_STOWED),
intake.coneGrab(),
intake.closeIntake(),
armRotate.moveTo(ArmPresets.ARM_STOWED));
}

Expand Down
32 changes: 5 additions & 27 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.auto.ConeStation;
import frc.robot.auto.CubePickupLocation;
import frc.robot.maps.FrostBiteMap;
import frc.robot.maps.RobotMap;
import frc.robot.maps.Valkyrie;
import frc.robot.subsystems.ArmExtend;
import frc.robot.subsystems.ArmRotate;
// $Imports$
Expand All @@ -43,7 +43,7 @@ public class Robot extends CommandRobot {
SendableChooser<CubePickupLocation> cubePosChooser = new SendableChooser<>();
SendableChooser<Integer> cubeScorePosChooser = new SendableChooser<>();

private RobotMap map = new FrostBiteMap();
private RobotMap map = new Valkyrie();
private ButtonXboxController driveController = new ButtonXboxController(0);
private ButtonXboxController copilotController = new ButtonXboxController(1);

Expand Down Expand Up @@ -81,14 +81,6 @@ public class Robot extends CommandRobot {
@Autonomous(defaultAuto = true, name = "(MAIN) Score leave")
public Command scoreThenLeave = auto.leaveCommunity();

// @Autonomous(name = "Piecemeal Auto")
// public Command buildCommand = new ProxyCommand(() -> {
// ConeStation conePos = conePosChooser.getSelected();
// CubePickupLocation cubePos = cubePosChooser.getSelected();
// int cubeScorePos = cubeScorePosChooser.getSelected();
// return auto.combinedAuto(conePos, cubePos, cubeScorePos);
// });

public Command driveScoreMidNode = sequence(
armRotate.moveTo(ArmPresets.MEDIUM_SCORE),
drive.driveToNearest(),
Expand Down Expand Up @@ -142,20 +134,19 @@ public Command rumbleAndIntakeSpinningOff() {

public Command stowArm = sequence(
led.setOrange(),
intake.coneGrab(),
intake.closeIntake(),
armExtend.retract(0.4),
armRotate.moveTo(ArmPresets.ARM_STOWED),
led.colorAlliance());

public Command pickUpGamePiece = sequence(
new ConditionalCommand(
sequence(
armRotate.moveTo(ArmPresets.CONE_PICKUP), armExtend.moveTo(ArmPresets.CONE_PICKUP),
intake.toggle()),
armRotate.moveTo(ArmPresets.CONE_PICKUP), armExtend.moveTo(ArmPresets.CONE_PICKUP)),
sequence(
armRotate.moveTo(ArmPresets.CUBE_PICKUP), armExtend.moveTo(
ArmPresets.CUBE_PICKUP),
intake.coneRelease(), intake.toggle()),
intake.openIntake()),
() -> {
return gamePieceSub.get() == "Cone";
}));
Expand Down Expand Up @@ -222,19 +213,6 @@ public void configureButtonBindings() {
driveController.x().whileTrue(drive.rotateToAngle(Rotation2d.fromDegrees(180), () -> 0, () -> 0));
driveController.a().whileTrue(drive.rotateToAngle(Rotation2d.fromDegrees(0), () -> 0, () -> 0));

driveController.povDown().onTrue(drive.moveForDirectional(0, 1, 5));
driveController.povRight().onTrue(drive.moveForDirectional(1, 0, 5));
driveController.povUp().onTrue(drive.moveForDirectional(0, -1, 5));
driveController.povLeft().onTrue(drive.moveForDirectional(-1, 0, 5));
/*
* public Command testDriveDriver() {
* return sequence(
* moveForDirectional(0, 1, 5),
* moveForDirectional(1, 0, 5),
* moveForDirectional(0, -1, 5),
* moveForDirectional(-1, 0, 5));
* }
*/
// Arm

// COPILOT CONTROLLER
Expand Down
102 changes: 102 additions & 0 deletions src/main/java/frc/robot/maps/Valkyrie.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,20 +4,35 @@
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;

import com.chopshop166.chopshoplib.drive.SDSSwerveModule;
import com.chopshop166.chopshoplib.drive.SDSSwerveModule.Configuration;
import com.chopshop166.chopshoplib.maps.RobotMapFor;
import com.chopshop166.chopshoplib.motors.CSSparkMax;
import com.chopshop166.chopshoplib.sensors.MockEncoder;
import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro;
import com.chopshop166.chopshoplib.states.PIDValues;
import com.ctre.phoenix.sensors.AbsoluteSensorRange;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import frc.robot.maps.subsystems.ArmExtendMap;
import frc.robot.maps.subsystems.ArmRotateMap;
import frc.robot.maps.subsystems.LedMap;
import frc.robot.maps.subsystems.SwerveDriveMap;
import frc.robot.util.DrivePID;

@RobotMapFor("Valkyrie")
public class Valkyrie extends RobotMap {
Expand Down Expand Up @@ -52,4 +67,91 @@ public void setupLogging() {
Logger.recordMetadata("RobotMap", this.getClass().getSimpleName());
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
}

@Override
public SwerveDriveMap getDriveMap() {

final double MODULE_OFFSET_XY = Units.inchesToMeters(12.375);
final PigeonGyro pigeonGyro = new PigeonGyro(new PigeonIMU(0));

final CSSparkMax frontLeftSteer = new CSSparkMax(4, MotorType.kBrushless);
final CSSparkMax frontRightSteer = new CSSparkMax(8, MotorType.kBrushless);
final CSSparkMax rearLeftSteer = new CSSparkMax(2, MotorType.kBrushless);
final CSSparkMax rearRightSteer = new CSSparkMax(6, MotorType.kBrushless);

frontLeftSteer.getMotorController().setPeriodicFramePeriod(PeriodicFrame.kStatus3, 1000);
frontRightSteer.getMotorController().setPeriodicFramePeriod(PeriodicFrame.kStatus3, 1000);
rearLeftSteer.getMotorController().setPeriodicFramePeriod(PeriodicFrame.kStatus3, 1000);
rearRightSteer.getMotorController().setPeriodicFramePeriod(PeriodicFrame.kStatus3, 1000);

frontLeftSteer.getMotorController().setInverted(false);
frontRightSteer.getMotorController().setInverted(false);
rearLeftSteer.getMotorController().setInverted(false);
rearRightSteer.getMotorController().setInverted(false);

// Configuration for MK4 with L2 speeds
Configuration MK4_V2 = new Configuration(SDSSwerveModule.MK4_V2.gearRatio,
SDSSwerveModule.MK4_V2.wheelDiameter, new PIDValues(0.004, 0.00, 0.0002));

// All Distances are in Meters
// Front Left Module
final CANCoder encoderFL = new CANCoder(2);
encoderFL.configMagnetOffset(-35.771);
encoderFL.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
final SDSSwerveModule frontLeft = new SDSSwerveModule(new Translation2d(MODULE_OFFSET_XY, MODULE_OFFSET_XY),
encoderFL, frontLeftSteer, new CSSparkMax(3, MotorType.kBrushless),
MK4_V2);

// Front Right Module
final CANCoder encoderFR = new CANCoder(4);
encoderFR.configMagnetOffset(103.607);
encoderFR.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
final SDSSwerveModule frontRight = new SDSSwerveModule(new Translation2d(MODULE_OFFSET_XY, -MODULE_OFFSET_XY),
encoderFR, frontRightSteer, new CSSparkMax(7,
MotorType.kBrushless),
MK4_V2);

// Rear Left Module
final CANCoder encoderRL = new CANCoder(1);
encoderRL.configMagnetOffset(-110.566);
encoderRL.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
final SDSSwerveModule rearLeft = new SDSSwerveModule(new Translation2d(-MODULE_OFFSET_XY, MODULE_OFFSET_XY),
encoderRL, rearLeftSteer, new CSSparkMax(1,
MotorType.kBrushless),
MK4_V2);

// Rear Right Module
final CANCoder encoderRR = new CANCoder(3);
encoderRR.configMagnetOffset(-209.707);
encoderRR.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
final SDSSwerveModule rearRight = new SDSSwerveModule(new Translation2d(-MODULE_OFFSET_XY, -MODULE_OFFSET_XY),
encoderRR, rearRightSteer, new CSSparkMax(5,
MotorType.kBrushless),
MK4_V2);

final double maxDriveSpeedMetersPerSecond = Units.feetToMeters(12);

final double maxRotationRadianPerSecond = Math.PI;

final DrivePID pid = new DrivePID(
1.2, 0.002, 0.0,
0.02, 0.00001, 0,
new Constraints(1.5, 2.5));

final Transform3d cameraPosition = new Transform3d(
// These probably need to be refined
new Translation3d(
Units.inchesToMeters(
2.44),
Units.inchesToMeters(
-7.25),
Units.inchesToMeters(
25)),
new Rotation3d());

return new SwerveDriveMap(frontLeft, frontRight, rearLeft, rearRight,
maxDriveSpeedMetersPerSecond,
maxRotationRadianPerSecond, pigeonGyro, pid, cameraPosition, "eyes");

}
}
5 changes: 0 additions & 5 deletions src/main/java/frc/robot/maps/subsystems/IntakeData.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ public class IntakeData implements LoggableInputs {

public double motorSetPoint;
public Value solenoidSetPoint = Value.kOff;
public Color sensorColor;
public int gamePieceDistance;
public int maxGamePieceDistance;
public int minGamePieceDistance;
Expand Down Expand Up @@ -48,8 +47,6 @@ public void updateData(IntakeData data) {
public void toLog(LogTable table) {
table.put("MotorSetPoint", motorSetPoint);
table.put("SolenoidSetPoint", solenoidSetPoint.toString());
double[] detectedColor = new double[] { sensorColor.red, sensorColor.green, sensorColor.blue };
table.put("DetetectedColor", detectedColor);
table.put("GamePieceDistance", gamePieceDistance);
table.put("MotorCurrentAmps", currentAmps);

Expand All @@ -62,8 +59,6 @@ public void fromLog(LogTable table) {

motorSetPoint = table.get("MotorSetPoint", motorSetPoint);
solenoidSetPoint = Value.valueOf(table.get("SolenoidSetPoint", solenoidSetPoint.toString()));
double[] detectedColor = table.get("DetectedColor", colorDoubleArray);
sensorColor = new Color(detectedColor[0], detectedColor[1], detectedColor[2]);
gamePieceDistance = table.get("GamePieceDistance", gamePieceDistance);
this.currentAmps = table.get("MotorCurrentAmps", currentAmps);
}
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/subsystems/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.ProxyCommand;
import frc.robot.Field;
import frc.robot.Vision;
Expand Down Expand Up @@ -60,7 +59,6 @@ public class Drive extends SmartSubsystemBase {

private static final double blueX = 1.8;
private static final Rotation2d rotation0 = Rotation2d.fromDegrees(0);
private static final Rotation2d rotation180 = Rotation2d.fromDegrees(180);

public enum GridPosition {

Expand Down
31 changes: 6 additions & 25 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,21 +37,20 @@ public void periodic() {
clawPub.set(getData().solenoidSetPoint == Value.kForward);
}

// Grabs game piece Cone
public Command coneGrab() {
// Releases game piece Cone
public Command openIntake() {
return runOnce(() -> {
getData().solenoidSetPoint = Value.kForward;
getData().solenoidSetPoint = Value.kReverse;
});
}

// Releases game piece Cone
public Command coneRelease() {
public Command closeIntake() {
return runOnce(() -> {
getData().solenoidSetPoint = Value.kReverse;
getData().solenoidSetPoint = Value.kForward;
});
}

// Releases game piece Cone
// Opens or closes intake
public Command toggle() {
return runOnce(() -> {
if (getData().solenoidSetPoint == Value.kForward) {
Expand Down Expand Up @@ -91,24 +90,6 @@ public Command cubeRelease() {
});
}

// Closes intake based on game piece detected
public Command sensorControl() {

return cmd().onExecute(() -> {
if (getData().gamePieceDistance <= getData().maxGamePieceDistance &&
getData().gamePieceDistance >= getData().minGamePieceDistance) {
if (ColorMath.equals(getData().sensorColor, Color.kPurple, .2)) {
getData().motorSetPoint = GRAB_SPEED;
} else if (ColorMath.equals(getData().sensorColor, Color.kYellow, .2)) {
getData().solenoidSetPoint = Value.kForward;
}
}
}).runsUntil(() -> getData().gamePieceDistance <= getData().minGamePieceDistance).onEnd(() -> {
safeState();
});

}

@Override
public void reset() {
// Nothing to reset here
Expand Down

0 comments on commit aa69ad4

Please sign in to comment.