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

Commit

Permalink
Merge branch 'fresno-comp' into revert-to-old-shooting-code
Browse files Browse the repository at this point in the history
  • Loading branch information
@maxspier
maxspier authored Mar 26, 2024
2 parents db91168 + fc121b5 commit 12c87f0
Show file tree
Hide file tree
Showing 11 changed files with 81 additions and 65 deletions.
14 changes: 7 additions & 7 deletions src/main/java/com/team766/odometry/Odometry.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,21 +175,21 @@ private void updateCurrentPositions() {
.plus(new Transform2d(wheelPositions[i], new Rotation2d()))
.getTranslation(),
currPositions[i].getRotation());
SmartDashboard.putNumber(
"early curr rotation", currPositions[i].getRotation().getDegrees());
SmartDashboard.putString("prev rotation direct", prevPositions[i].toString());
// SmartDashboard.putNumber(
// "early curr rotation", currPositions[i].getRotation().getDegrees());
// SmartDashboard.putString("prev rotation direct", prevPositions[i].toString());
currPositions[i] =
new Pose2d(
currPositions[i].getTranslation(),
gyroPosition.plus(Rotation2d.fromDegrees(absolutePosition)));

rotationChange = currPositions[i].getRotation().minus(prevPositions[i].getRotation());
SmartDashboard.putNumber("curr rotation", currPositions[i].getRotation().getDegrees());
SmartDashboard.putNumber("prev rotation", prevPositions[i].getRotation().getDegrees());
SmartDashboard.putNumber("rotation change", rotationChange.getDegrees());
// SmartDashboard.putNumber("curr rotation", currPositions[i].getRotation().getDegrees());
// SmartDashboard.putNumber("prev rotation", prevPositions[i].getRotation().getDegrees());
// SmartDashboard.putNumber("rotation change", rotationChange.getDegrees());

double yaw = Math.toRadians(gyro.getAngle());
SmartDashboard.putNumber("odom yaw", yaw);
// SmartDashboard.putNumber("odom yaw", yaw);
double roll = Math.toRadians(gyro.getRoll());
double pitch = Math.toRadians(gyro.getPitch());

Expand Down
18 changes: 9 additions & 9 deletions src/main/java/com/team766/robot/common/mechanisms/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,8 +164,8 @@ private static Vector2D createOrthogonalVector(Vector2D vector) {
*/
public void controlRobotOriented(double x, double y, double turn) {
checkContextOwnership();
SmartDashboard.putString(
"[" + "joystick" + "]" + "x, y", String.format("%.2f, %.2f", x, y));
// SmartDashboard.putString(
// "[" + "joystick" + "]" + "x, y", String.format("%.2f, %.2f", x, y));

// Calculate the necessary turn velocity (m/s) for each motor:
double turnVelocity = config.wheelDistanceFromCenter() * turn;
Expand Down Expand Up @@ -240,7 +240,7 @@ public void controlFieldOrientedWithRotationTarget(double x, double y, Rotation2
checkContextOwnership();
if (target != null) {
rotationPID.setSetpoint(target.getDegrees());
SmartDashboard.putNumber("Rotation Target", target.getDegrees());
// SmartDashboard.putNumber("Rotation Target", target.getDegrees());
}

movingToTarget = true;
Expand Down Expand Up @@ -385,11 +385,11 @@ private static Translation2d getPositionForWheel(
public void run() {
swerveOdometry.run();
// log(currentPosition.toString());
SmartDashboard.putString("pos", getCurrentPosition().toString());
// SmartDashboard.putString("pos", getCurrentPosition().toString());

SmartDashboard.putNumber("Yaw", getHeading());
SmartDashboard.putNumber("Pitch", getPitch());
SmartDashboard.putNumber("Roll", getRoll());
// SmartDashboard.putNumber("Yaw", getHeading());
// SmartDashboard.putNumber("Pitch", getPitch());
// SmartDashboard.putNumber("Roll", getRoll());

if (movingToTarget) {
rotationPID.calculate(getHeading());
Expand All @@ -401,9 +401,9 @@ public void run() {
: rotationPID.getOutput()));
}

SmartDashboard.putBoolean("movingToTarget", movingToTarget);
// SmartDashboard.putBoolean("movingToTarget", movingToTarget);

SmartDashboard.putBoolean("isAtRotationTarget", isAtRotationTarget());
// SmartDashboard.putBoolean("isAtRotationTarget", isAtRotationTarget());

swerveFR.dashboardCurrentUsage();
swerveFL.dashboardCurrentUsage();
Expand Down
55 changes: 28 additions & 27 deletions src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ public SwerveModule(
this.steer = steer;
this.encoder = encoder;
this.offset = computeEncoderOffset();
SmartDashboard.putNumber("[" + modulePlacement + "]" + "Offset", offset);
// SmartDashboard.putNumber("[" + modulePlacement + "]" + "Offset", offset);

// Current limit for motors to avoid breaker problems
drive.setCurrentLimit(driveMotorCurrentLimit);
Expand Down Expand Up @@ -109,9 +109,9 @@ private double computeEncoderOffset() {
*/
public void steer(Vector2D vector) {
boolean reversed = false;
SmartDashboard.putString(
"[" + modulePlacement + "]" + "x, y",
String.format("%.2f, %.2f", vector.getX(), vector.getY()));
// SmartDashboard.putString(
// "[" + modulePlacement + "]" + "x, y",
// String.format("%.2f, %.2f", vector.getX(), vector.getY()));

// Calculates the angle of the vector from -180° to 180°
final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX()));
Expand Down Expand Up @@ -143,18 +143,18 @@ public void steer(Vector2D vector) {
// Sets the degree of the steer wheel
// Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor
// understands
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + "Steer", ENCODER_CONVERSION_FACTOR * angleDegrees);
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + "Steer", ENCODER_CONVERSION_FACTOR * angleDegrees);

steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees);

SmartDashboard.putNumber("[" + modulePlacement + "]" + "TargetAngle", vectorTheta);
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + "RelativeAngle",
steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR - offset);
// SmartDashboard.putNumber("[" + modulePlacement + "]" + "TargetAngle", vectorTheta);
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + "RelativeAngle",
// steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR - offset);
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + "CANCoder",
encoder.getAbsolutePosition().getValueAsDouble() * 360);
"[" + modulePlacement + "]" + "CANCoder",
encoder.getAbsolutePosition().getValueAsDouble() * 360);
// return reversed;
}

Expand All @@ -168,19 +168,20 @@ public void driveAndSteer(Vector2D vector) {

// sets the power to the magnitude of the vector and reverses power if necessary
// TODO: does this need to be clamped to a specific range, eg btn -1 and 1?
SmartDashboard.putNumber("[" + modulePlacement + "]" + "Desired drive", vector.getNorm());
// SmartDashboard.putNumber("[" + modulePlacement + "]" + "Desired drive", vector.getNorm());
double power;
// if (reversed) {
// power = -vector.getNorm() * MOTOR_WHEEL_FACTOR_MPS;
// reversed = false;

// } else {
power = vector.getNorm() * MOTOR_WHEEL_FACTOR_MPS;
// }
SmartDashboard.putNumber("[" + modulePlacement + "]" + "Input motor velocity", power);
drive.set(ControlMode.Velocity, power);

SmartDashboard.putNumber(
"[" + modulePlacement + "]" + "Read Vel", drive.getSensorVelocity());
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + "Read Vel", drive.getSensorVelocity());
}

/**
Expand All @@ -198,17 +199,17 @@ public SwerveModuleState getModuleState() {
}

public void dashboardCurrentUsage() {
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + " steer supply current",
MotorUtil.getCurrentUsage(steer));
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + " steer stator current",
MotorUtil.getStatorCurrentUsage(steer));
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + " drive supply current",
MotorUtil.getCurrentUsage(drive));
SmartDashboard.putNumber(
"[" + modulePlacement + "]" + " drive stator current",
MotorUtil.getStatorCurrentUsage(drive));
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + " steer supply current",
// MotorUtil.getCurrentUsage(steer));
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + " steer stator current",
// MotorUtil.getStatorCurrentUsage(steer));
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + " drive supply current",
// MotorUtil.getCurrentUsage(drive));
// SmartDashboard.putNumber(
// "[" + modulePlacement + "]" + " drive stator current",
// MotorUtil.getStatorCurrentUsage(drive));
}
}
18 changes: 16 additions & 2 deletions src/main/java/com/team766/robot/reva/BoxOpOI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ public class BoxOpOI extends OIFragment {
private final OICondition moveShoulder;
private final OICondition enableClimberControls;
private final OICondition climberOverrideSoftLimits;
// private final OICondition shootoi;

public BoxOpOI(
JoystickReader gamepad,
Expand All @@ -43,6 +44,7 @@ public BoxOpOI(
this.shooter = shooter;
this.climber = climber;

// shootoi = new OICondition(() -> gamepad.getPOV()==270);
intakeOut = new OICondition(() -> gamepad.getButton(InputConstants.XBOX_RB));
intakeIn = new OICondition(() -> gamepad.getButton(InputConstants.XBOX_LB));
shooterShoot = new OICondition(() -> gamepad.getAxis(InputConstants.XBOX_RT) > 0);
Expand Down Expand Up @@ -99,12 +101,12 @@ protected void handleOI(Context context) {
} else if (gamepad.getButtonPressed(InputConstants.XBOX_X)) {
// other shoot pos
context.takeOwnership(shoulder);
shoulder.rotate(ShoulderPosition.TOP);
shoulder.rotate(ShoulderPosition.AMP);
context.releaseOwnership(shoulder);
} else if (gamepad.getButtonPressed(InputConstants.XBOX_Y)) {
// amp shot
context.takeOwnership(shoulder);
shoulder.rotate(ShoulderPosition.AMP);
shoulder.rotate(ShoulderPosition.TOP);
context.releaseOwnership(shoulder);
} else if (gamepad.getPOV() == 0) {
context.takeOwnership(shoulder);
Expand All @@ -120,6 +122,18 @@ protected void handleOI(Context context) {
} */
}

// if(shootoi.isTriggering()){
// if(shootoi.isNewlyTriggering()){
// context.takeOwnership(shooter);
// }

// if(gamepad.getPOV()== 270){
// shooter.shoot(3000);
// }
// } else if (shootoi.isFinishedTriggering()){
// context.releaseOwnership(shooter);
// }

// climber
if (enableClimberControls.isTriggering()) {
if (enableClimberControls.isNewlyTriggering()) {
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/com/team766/robot/reva/DriverOI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ public void handleOI(Context context) {
context.takeOwnership(shooter);
context.takeOwnership(shoulder);
context.takeOwnership(intake);

Robot.shooter.stop();
Robot.intake.stop();

Expand All @@ -123,7 +123,6 @@ public void handleOI(Context context) {
// drive.stopDrive();
// drive.setCross();

// context.releaseOwnership(drive);
}

// TODO: update OI with new optimization OI
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/team766/robot/reva/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ public void run(Context context) {
RobotProvider.instance.refreshDriverStationData();

// NOTE: DriverStation.getAlliance() returns Optional<Alliance>
SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());
// SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());

// Add driver controls here - make sure to take/release ownership
// of mechanisms when appropriate.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ private boolean updateTarget() {
relativeTarget.rotateBy(
Rotation2d.fromDegrees((drive.getHeading() + 180))));

SmartDashboard.putString("target pos", absTargetPos.toString());
// SmartDashboard.putString("target pos", absTargetPos.toString());

// context.takeOwnership(drive);

Expand Down
4 changes: 3 additions & 1 deletion src/main/java/com/team766/robot/reva/mechanisms/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@ public class Shooter extends Mechanism {
private static final double CURRENT_LIMIT = 40.0; // needs tuning
private static final double MAX_SPEED = 5600.0; // spec is 6000.0
private static final double MIN_SPEED = 0.0;
private static final double SPEED_TOLERANCE = 600.0; // rpm

// TODO: Get the voltage of the battery and set the speed tolerance propotional to this
private static final double SPEED_TOLERANCE = 800.0; // rpm

private MotorController shooterMotorTop;
private MotorController shooterMotorBottom;
Expand Down
24 changes: 12 additions & 12 deletions src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public enum ShoulderPosition {
// TODO: Find actual values.
BOTTOM(0),
INTAKE_FLOOR(0),
SHOOT_LOW(10),
SHOOT_LOW(15),
SHOOT_MEDIUM(30),
SHOOT_HIGH(80),
AMP(90),
Expand Down Expand Up @@ -132,7 +132,7 @@ public void rotate(double angle) {
com.team766.math.Math.clamp(
angle, ShoulderPosition.BOTTOM.getAngle(), ShoulderPosition.TOP.getAngle());
targetRotations = degreesToRotations(targetAngle);
SmartDashboard.putNumber("[SHOULDER Target Angle]", targetAngle);
// SmartDashboard.putNumber("[SHOULDER Target Angle]", targetAngle);
// actual rotation will happen in run()
}

Expand All @@ -153,13 +153,13 @@ public void run() {
leftMotor.setSensorPosition(convertedPos);
encoderInitializationCount++;
}
SmartDashboard.putNumber("[SHOULDER] Angle", getAngle());
SmartDashboard.putNumber("[SHOULDER] Target Angle", targetAngle);
SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations());
SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations);
SmartDashboard.putNumber("[SHOULDER] Encoder Frequency", absoluteEncoder.getFrequency());
SmartDashboard.putNumber(
"[SHOULDER] Absolute Encoder Position", getAbsoluteEncoderPosition());
// SmartDashboard.putNumber("[SHOULDER] Angle", getAngle());
// SmartDashboard.putNumber("[SHOULDER] Target Angle", targetAngle);
// SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations());
// SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations);
// SmartDashboard.putNumber("[SHOULDER] Encoder Frequency", absoluteEncoder.getFrequency());
// SmartDashboard.putNumber(
// "[SHOULDER] Absolute Encoder Position", getAbsoluteEncoderPosition());
// SmartDashboard.putNumber(
// "[SHOULDER] Left Motor Supply Current", MotorUtil.getCurrentUsage(leftMotor));
// SmartDashboard.putNumber(
Expand All @@ -172,10 +172,10 @@ public void run() {
// MotorUtil.getStatorCurrentUsage(rightMotor));

TalonFX leftTalon = (TalonFX) leftMotor;
SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get());
// SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get());
double ff = ffGain.valueOr(0.0) * Math.cos(Math.toRadians(getAngle()));
SmartDashboard.putNumber("[SHOULDER] FF", ff);
SmartDashboard.putNumber("[SHOULDER VELOCITY]", Math.abs(leftMotor.getSensorVelocity()));
// SmartDashboard.putNumber("[SHOULDER] FF", ff);
// SmartDashboard.putNumber("[SHOULDER VELOCITY]", Math.abs(leftMotor.getSensorVelocity()));
PositionDutyCycle positionRequest = new PositionDutyCycle(targetRotations);
positionRequest.FeedForward = ff;
leftTalon.setControl(positionRequest);
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/com/team766/robot/reva/procedures/RotateAndShootNow.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,13 @@ public void run(Context context) {
Robot.shoulder.rotate(armAngle);
Robot.drive.controlFieldOrientedWithRotationTarget(0, 0, heading);
// Robot.shooter.shoot(power);
context.releaseOwnership(Robot.shoulder);

context.waitForConditionOrTimeout(Robot.shoulder::isFinished, 0.5);
context.waitForConditionOrTimeout(Robot.drive::isAtRotationTarget, 3.0);
Robot.drive.stopDrive();

context.releaseOwnership(Robot.shoulder);
// context.releaseOwnership(Robot.shoulder);
context.releaseOwnership(Robot.drive);
context.releaseOwnership(Robot.shooter);
new ShootVelocityAndIntake().run(context);
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,7 @@ public void run(Context context) {
context.takeOwnership(Robot.shooter);

Robot.shooter.shoot(speed);
context.waitForConditionOrTimeout(Robot.shooter::isCloseToExpectedSpeed, 4.0);
context.waitForSeconds(0.5);
context.waitForConditionOrTimeout(Robot.shooter::isCloseToExpectedSpeed, 1.5);

context.takeOwnership(Robot.intake);
new IntakeIn().run(context);
Expand Down

0 comments on commit 12c87f0

Please sign in to comment.