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
|
// 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 com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;
import edu.wpi.first.wpilibj.GenericHID;
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.Arm;
import frc.robot.subsystems.Chain;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hooks;
import frc.robot.subsystems.Intake;
import frc.robot.subsystems.Shooter;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
private final Chain m_chain = new Chain();
private final Arm m_arm = new Arm();
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_controller = Constants.controller;
public RobotContainer()
{
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.0)));
m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
m_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees(
deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)))));
m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)),
deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5)))));
m_chain.setDefaultCommand(m_chain.run(() ->
m_chain.atPercentage(
Constants.controller2.getRawAxis(0)
* (-0.5) + 0.5
)));
m_drive.setDefaultCommand(m_drive.run(() -> {
double[] inputs = correctInputs(
m_controller.getRawAxis(2),
m_controller.getRawAxis(1));
m_drive.tank(
inputs[0] * Constants.DriveMaxSpeed,
inputs[1] * 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_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue(
Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)),
Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9)))
.until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue(
// Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20))
// .andThen(() ->
// m_chain.run(() -> m_chain.atPercentage(0.9)).until(() ->
// m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40))))
// ));
// Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen());
// 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 Commands.print("No autonomous command configured");
}
}
|
