Skip to content

Commit 7a31806

Browse files
jasondickertjasondickert
committed
Merge remote-tracking branch 'origin/Test' into main
2 parents b058727 + a8c6c28 commit 7a31806

File tree

5 files changed

+145
-96
lines changed

5 files changed

+145
-96
lines changed

‎src/main/java/frc/robot/Robot.java

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -175,6 +175,7 @@ public void autonomousInit() {
175175
if (m_autonomousCommand != null) {
176176
m_autonomousCommand.schedule();
177177
}
178+
mRobotContainer.enableLights();
178179
}
179180

180181
/** This function is called periodically during autonomous. */

‎src/main/java/frc/robot/RobotContainer.java

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -123,7 +123,7 @@ public RobotContainer() {
123123
configureButtonBindings();
124124
mDrive.setDefaultCommand(new JoystickDrive(mDrive, joysticks));
125125
// mDrive.setDefaultCommand(new TestDrive(mDrive));
126-
mIntake.setDefaultCommand(new RetractIntake(mIntake));
126+
mIntake.setDefaultCommand(new RetractIntake(mIntake)); // UNDO THIS
127127
// mShooter.setDefaultCommand(new StopShooter(mShooter));
128128
// mShooter.setDefaultCommand(new FlywheelController(mShooter, 0));
129129
// mClimber.setDefaultCommand(new RetractClimber(mClimber));
@@ -146,7 +146,8 @@ public Command getAutonomousCommand() {
146146

147147
try {
148148
if(!fiveBallAuto.get()){
149-
autoCommand = new FiveBallAuto(mDrive, mIntake, mShooter);
149+
autoCommand = new NewFiveBallAuto(mDrive, mShooter, mIntake, mLimelight, joysticks);
150+
// autoCommand = new FiveBallAuto(mDrive, mIntake, mShooter);
150151
} else if (!twoBallSouthAuto.get()) {
151152
autoCommand = new TwoBallSouthAuto(mDrive, mIntake, mShooter);
152153
} else if (!twoBallEastAuto.get()) {
@@ -163,7 +164,7 @@ public Command getAutonomousCommand() {
163164
*/
164165
}
165166
// return autoCommand;
166-
return new NewFiveBallAuto(mDrive, mShooter, mIntake);
167+
return autoCommand;
167168
}
168169

169170
public void displaySwitch() {
@@ -359,9 +360,9 @@ public void configureButtonBindings() {
359360
.alongWith(new XBoxButtonCommand(X_BOX_X)));
360361
// xBoxX.whenHeld(new MotionProfileTurn(mDrive, Math.PI/2));
361362

362-
xBoxX.whenReleased(new StopShooter(mShooter)
363-
.alongWith(new TurnOffLEDs(mLimelight))
364-
.alongWith(new IdleCommand()));
363+
// xBoxX.whenReleased(new StopShooter(mShooter)
364+
// .alongWith(new TurnOffLEDs(mLimelight))
365+
// .alongWith(new IdleCommand()));
365366

366367
JoystickButton xBoxY = new JoystickButton(operator, X_BOX_Y);
367368
xBoxY.whenHeld(new PresetFlywheelController(mShooter, "TUR")

‎src/main/java/frc/robot/auto/groups/NewAuto.java

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.auto.groups;
6+
7+
import edu.wpi.first.math.geometry.Pose2d;
8+
import edu.wpi.first.math.geometry.Rotation2d;
9+
import edu.wpi.first.math.geometry.Translation2d;
10+
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
11+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
12+
import edu.wpi.first.wpilibj2.command.WaitCommand;
13+
import frc.lib.HelixJoysticks;
14+
import frc.paths.NewAutoPartFour;
15+
import frc.paths.NewAutoPartOne;
16+
import frc.paths.NewAutoPartThree;
17+
import frc.paths.NewAutoPartTwo;
18+
import frc.robot.Robot;
19+
import frc.robot.drive.Drivetrain;
20+
import frc.robot.drive.commands.ResetOdometry;
21+
import frc.robot.drive.commands.TrajectoryFollower;
22+
import frc.robot.drive.commands.TurnToAngle;
23+
import frc.robot.intake.Intake;
24+
import frc.robot.intake.commands.FastIntake;
25+
import frc.robot.intake.commands.RetractIntake;
26+
import frc.robot.shooter.Shooter;
27+
import frc.robot.shooter.commands.FlywheelController;
28+
import frc.robot.shooter.commands.PullTrigger;
29+
import frc.robot.shooter.commands.ResetEncoder;
30+
import frc.robot.shooter.commands.StopShooter;
31+
import frc.robot.shooter.commands.StopTrigger;
32+
import frc.robot.status.actions.ImageAction;
33+
import frc.robot.status.commands.ActionCommand;
34+
import frc.robot.vision.Limelight;
35+
36+
// NOTE: Consider using this command inline, rather than writing a subclass. For more
37+
// information, see:
38+
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
39+
public class NewFiveBallAuto extends SequentialCommandGroup {
40+
/** Creates a new NewAuto. */
41+
public NewFiveBallAuto(Drivetrain drive, Shooter shooter, Intake intake, Limelight limelight, HelixJoysticks joysticks) {
42+
addCommands(
43+
new ResetOdometry(drive, new Pose2d(new Translation2d(0,0), new Rotation2d(-2.35))),
44+
new ResetEncoder(shooter),
45+
new ParallelDeadlineGroup(
46+
new WaitCommand(2.9),
47+
// new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()),
48+
new TrajectoryFollower(drive, new NewAutoPartOne()),
49+
new FastIntake(intake),
50+
new SequentialCommandGroup(
51+
new WaitCommand(1.25),
52+
new PullTrigger(shooter)
53+
),
54+
new FlywheelController(shooter, 1980, 72)),
55+
new ParallelDeadlineGroup(
56+
new TrajectoryFollower(drive, new NewAutoPartTwo()),
57+
new StopTrigger(shooter),
58+
new StopShooter(shooter)
59+
),
60+
new WaitCommand(0.35),
61+
new ParallelDeadlineGroup(
62+
new WaitCommand(4.5),
63+
new SequentialCommandGroup(
64+
new WaitCommand(1.75),
65+
new FlywheelController(shooter, 1795, 76.0)
66+
),
67+
new SequentialCommandGroup(
68+
new TrajectoryFollower(drive, new NewAutoPartThree()),
69+
new TurnToAngle(drive, limelight, joysticks)
70+
),
71+
new SequentialCommandGroup(
72+
new WaitCommand(3.4),
73+
new PullTrigger(shooter)
74+
),
75+
new SequentialCommandGroup(
76+
new WaitCommand(0.25),
77+
new RetractIntake(intake)
78+
)
79+
),
80+
new ParallelDeadlineGroup(
81+
new WaitCommand(3.0),
82+
new TrajectoryFollower(drive, new NewAutoPartFour()),
83+
new FlywheelController(shooter, 1785, 75.5),
84+
new SequentialCommandGroup(
85+
new StopTrigger(shooter),
86+
new WaitCommand(1.25),
87+
new PullTrigger(shooter)
88+
),
89+
new FastIntake(intake)
90+
),
91+
new ParallelDeadlineGroup(
92+
new WaitCommand(1.0),
93+
new StopTrigger(shooter),
94+
new StopShooter(shooter),
95+
new RetractIntake(intake))
96+
);
97+
}
98+
}

‎src/main/java/frc/robot/auto/groups/NewFiveBallAuto.java

Lines changed: 2 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -1,88 +1,5 @@
1-
// Copyright (c) FIRST and other WPILib contributors.
2-
// Open Source Software; you can modify and/or share it under the terms of
3-
// the WPILib BSD license file in the root directory of this project.
4-
51
package frc.robot.auto.groups;
62

7-
import edu.wpi.first.math.geometry.Pose2d;
8-
import edu.wpi.first.math.geometry.Rotation2d;
9-
import edu.wpi.first.math.geometry.Translation2d;
10-
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
11-
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
12-
import edu.wpi.first.wpilibj2.command.WaitCommand;
13-
import frc.paths.NewAutoPartFour;
14-
import frc.paths.NewAutoPartOne;
15-
import frc.paths.NewAutoPartThree;
16-
import frc.paths.NewAutoPartTwo;
17-
import frc.robot.Robot;
18-
import frc.robot.drive.Drivetrain;
19-
import frc.robot.drive.commands.ResetOdometry;
20-
import frc.robot.drive.commands.TrajectoryFollower;
21-
import frc.robot.intake.Intake;
22-
import frc.robot.intake.commands.FastIntake;
23-
import frc.robot.intake.commands.RetractIntake;
24-
import frc.robot.shooter.Shooter;
25-
import frc.robot.shooter.commands.FlywheelController;
26-
import frc.robot.shooter.commands.PullTrigger;
27-
import frc.robot.shooter.commands.ResetEncoder;
28-
import frc.robot.shooter.commands.StopShooter;
29-
import frc.robot.shooter.commands.StopTrigger;
30-
import frc.robot.status.actions.ImageAction;
31-
import frc.robot.status.commands.ActionCommand;
32-
33-
// NOTE: Consider using this command inline, rather than writing a subclass. For more
34-
// information, see:
35-
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
36-
public class NewFiveBallAuto extends SequentialCommandGroup {
37-
/** Creates a new NewAuto. */
38-
public NewFiveBallAuto(Drivetrain drive, Shooter shooter, Intake intake) {
39-
addCommands(
40-
new ResetOdometry(drive, new Pose2d(new Translation2d(0,0), new Rotation2d(-2.35))),
41-
new ResetEncoder(shooter),
42-
new ParallelDeadlineGroup(
43-
new WaitCommand(2.9),
44-
new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()),
45-
new TrajectoryFollower(drive, new NewAutoPartOne()),
46-
new FastIntake(intake),
47-
new SequentialCommandGroup(
48-
new WaitCommand(1.25),
49-
new PullTrigger(shooter)
50-
),
51-
new FlywheelController(shooter, 1980, 72)),
52-
new ParallelDeadlineGroup(
53-
new TrajectoryFollower(drive, new NewAutoPartTwo()),
54-
new StopTrigger(shooter),
55-
new StopShooter(shooter)
56-
),
57-
new WaitCommand(0.35),
58-
new ParallelDeadlineGroup(
59-
new WaitCommand(3.9),
60-
new TrajectoryFollower(drive, new NewAutoPartThree()),
61-
new SequentialCommandGroup(
62-
new WaitCommand(1.25),
63-
new FlywheelController(shooter, 1795, 76.0)
64-
),
65-
new SequentialCommandGroup(
66-
new WaitCommand(2.9),
67-
new PullTrigger(shooter)
68-
),
69-
new RetractIntake(intake)),
70-
new ParallelDeadlineGroup(
71-
new WaitCommand(3.0),
72-
new TrajectoryFollower(drive, new NewAutoPartFour()),
73-
new FlywheelController(shooter, 1785, 75.5),
74-
new SequentialCommandGroup(
75-
new StopTrigger(shooter),
76-
new WaitCommand(1.25),
77-
new PullTrigger(shooter)
78-
),
79-
new FastIntake(intake)
80-
),
81-
new ParallelDeadlineGroup(
82-
new WaitCommand(1.0),
83-
new StopTrigger(shooter),
84-
new StopShooter(shooter),
85-
new RetractIntake(intake))
86-
);
87-
}
3+
public class NewFiveBallAuto {
4+
885
}

‎src/main/java/frc/robot/drive/commands/MotionProfileTurn.java

Lines changed: 37 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,40 +8,72 @@
88
import edu.wpi.first.math.geometry.Rotation2d;
99
import edu.wpi.first.math.kinematics.ChassisSpeeds;
1010
import edu.wpi.first.math.trajectory.TrapezoidProfile;
11+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1112
import edu.wpi.first.wpilibj2.command.CommandBase;
1213
import frc.robot.drive.Drivetrain;
1314

1415
public class MotionProfileTurn extends CommandBase {
1516
Drivetrain drive;
1617
double offset;
1718
double target;
19+
double velocity;
20+
double dt;
21+
double maxAcceleration, maxVelocity;
1822
ProfiledPIDController controller;
1923

2024
public MotionProfileTurn(Drivetrain drive, double offset) {
2125
this.drive = drive;
2226
this.offset = offset;
2327
addRequirements(drive);
24-
controller = new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(10, 10));
25-
controller.enableContinuousInput(-Math.PI, Math.PI);
2628
}
2729

2830
@Override
2931
public void initialize() {
30-
target = offset + drive.getPose().getRotation().getRadians();
32+
// target = offset + drive.getPose().getRotation().getRadians();
33+
SmartDashboard.putString("Turning", "Enabled");
34+
controller = new ProfiledPIDController(0.35, 0.0, 0.0, new TrapezoidProfile.Constraints(1, 1), 0.02);
35+
// controller.enableContinuousInput(-Math.PI, Math.PI);
36+
velocity = 0;
37+
maxAcceleration = 0.5;
38+
maxVelocity = 1;
39+
dt = 0.02;
3140
}
3241

3342
@Override
3443
public void execute() {
3544
double theta = drive.getPose().getRotation().getRadians();
36-
double feedback = controller.calculate(theta, 0);
37-
double feedforward = controller.getSetpoint().velocity;
45+
double error = shortAngle(0, theta);
46+
double feedback = controller.calculate(0, shortAngle(0, theta));
47+
double feedforward = solveVelocity(maxVelocity, maxAcceleration, error, dt);
48+
SmartDashboard.putNumber("Error", error);
49+
SmartDashboard.putNumber("Velocity", feedforward);
3850
drive.drive(new ChassisSpeeds(0, 0, feedforward), false);
3951
}
4052

53+
public double solveVelocity(double maxVelocity, double maxAcceleration, double error, double dt) {
54+
double accel = 0;
55+
if (Math.sqrt(2*maxAcceleration*Math.abs(error)) > Math.abs(velocity)) {
56+
accel = -maxAcceleration * Math.signum(error);
57+
} else {
58+
accel = maxAcceleration * Math.signum(error);
59+
}
60+
SmartDashboard.putNumber("Accel", accel);
61+
velocity = Math.min(maxVelocity, Math.max(-maxVelocity, velocity + accel * dt));
62+
// velocity += accel * dt;
63+
return velocity;
64+
}
65+
66+
public double shortAngle(double targetAngle, double currentAngle) {
67+
double diff = (targetAngle - currentAngle + Math.PI) % (2* Math.PI) - Math.PI;
68+
diff = diff < -Math.PI ? diff + 2 * Math.PI : diff;
69+
return diff;
70+
}
71+
4172
// Called once the command ends or is interrupted.
4273
@Override
4374
public void end(boolean interrupted) {
4475
drive.brake();
76+
SmartDashboard.putString("Turning", "Disabled");
4577
}
4678

4779
// Returns true when the command should end.

0 commit comments

Comments
 (0)