1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
|
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hooks;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.MainMech;
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.MainMech.MechCmd;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
private final MainMech m_mech = new MainMech();
private final Intake m_intake = new Intake();
private final Shooter m_shooter = new Shooter();
private final Hooks m_hooks = new Hooks();
private final GenericHID m_controller1 = Constants.controller1;
private final GenericHID m_controller2 = Constants.controller2;
SendableChooser<Command> m_chooser = new SendableChooser<>();
public RobotContainer()
{
m_chooser.setDefaultOption("None", Commands.none());
m_chooser.addOption("Shoot",
Commands.parallel(
m_shooter.run(() -> m_shooter.atPercentage(1)),
Commands.none().withTimeout(2000).andThen(m_intake.run(() -> m_intake.atPercentage(1)).withTimeout(3000))));
SmartDashboard.putData(m_chooser);
configureBindings();
}
private double[] correctInputs(double in1, double in2)
{
double p1 = processInput(in1);
double p2 = processInput(in2);
if(Math.signum(p1) != Math.signum(p2))
return new double[] {p1, p2};
double diff = Math.abs(p1 - p2) * Constants.InputSmoothing;
return (p1 > p2) ? new double[] {p1 - diff, p2 + diff}
: new double[] {p1 + diff, p2 - diff};
}
private double processInput(double val)
{
double input = deadzone(-0.06, 0.06, val);
return input * input * input;
}
private double deadzone(double low, double high, double val)
{
if(val > low && val < high) return 0.0;
return ((val > 0.0) ? val - high : val - low);
}
private void configureBindings()
{
m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(-0.2)));
m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
deadzone(-0.1, 0.1, m_controller2.getRawAxis(4)),
deadzone(-0.1, 0.1, m_controller2.getRawAxis(5)))));
m_mech.setDefaultCommands(
m_mech.m_arm.run(() -> m_mech.m_arm.incrementAngle(
deadzone(-0.05, 0.05, m_controller2.getRawAxis(2)) * 0.05)) ,
m_mech.m_chain.run(() -> m_mech.m_chain.atPercentage(
m_controller2.getRawAxis(0)
* (-0.5) + 0.5))
);
m_mech.setDefaultCommand(m_mech.run(() -> m_mech.toState(MechCmd.MANUAL).execute()));
m_drive.setDefaultCommand(m_drive.run(() -> {
double[] inputs = correctInputs(
m_controller1.getRawAxis(2),
m_controller1.getRawAxis(1));
m_drive.tank(
-inputs[1] * Constants.DriveMaxSpeed, // reversed inputs
-inputs[0] * Constants.DriveMaxSpeed);
}));
// Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
// new Pose2d(0, 0, new Rotation2d(0)),
// List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
// new Pose2d(3, 0, new Rotation2d(0)), CameraServer camere = CameraServer.startAutomaticCapture();
// m_drive.trajectoyConfig);
// new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
new Trigger(() -> m_controller2.getRawButton(1)).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
new Trigger(() -> m_controller1.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.shoot()));
new Trigger(() -> m_controller1.getRawAxis(6) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
new Trigger(() -> m_controller1.getRawAxis(7) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
new Trigger(() -> m_controller2.getRawButton(2)).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.2)));
new Trigger(() -> m_controller2.getRawAxis(6) > 0).whileTrue(m_mech.toState(MechCmd.INTAKE));
new Trigger(() -> m_controller2.getRawAxis(7) > 0).whileTrue(m_mech.toState(MechCmd.DEFAULT));
new Trigger(() -> m_controller2.getRawButton(4)).whileTrue(m_mech.toState(MechCmd.AMP));
new Trigger(() -> m_controller2.getRawButton(3)).whileTrue(m_mech.toState(MechCmd.SPEAKER));
// new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
// new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
// new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
// new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kForward));
// new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kReverse));
// new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kForward));
// new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kReverse));
}
public Command getAutonomousCommand()
{
return m_chooser.getSelected();
}
}
|
