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
|
// 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 java.util.List;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.subsystems.Drivetrain;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
// private final Chain m_chain = new Chain();
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_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(),
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
new Pose2d(3, 0, new Rotation2d(0)),
m_drive.trajectoyConfig);
new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
// m_chain.setDefaultCommand(m_chain.run(() ->
// m_chain.atPercentage(
// processInput(m_controller.getRawAxis(2))
// )));
// new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
// 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));
}
public Command getAutonomousCommand()
{
return Commands.print("No autonomous command configured");
}
}
|
