blob: 3580633d3cbd74ab98f05c2cbce719f98c650c78 (
plain)
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
|
// 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 javax.smartcardio.CommandAPDU;
import edu.wpi.first.math.geometry.Pose2d;
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;
import frc.robot.subsystems.Drivetrain;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
private final GenericHID m_controller = Constants.controller;
public RobotContainer()
{
configureBindings();
}
private double deadzone(double low, double high, double val) {
if(val > low && val < high) return 0.0;
return val;
}
private void configureBindings()
{
m_drive.setDefaultCommand(m_drive.run(() ->
m_drive.tank(
deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed
)));
// debug controls
new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
Pose2d pose = m_drive.pose2d();
System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees());
}));
new Trigger(() -> m_controller.getRawButton(2)).onTrue(Commands.runOnce(() -> {
System.out.println("Reset");
m_drive.reset();
}));
new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(8))
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Quasistatic Forward")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(7))
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Quasistatic Reverse")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(8))
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Dynamic Forward")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(7))
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Dynamic Reverse")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
// new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.runOnce(() -> {
// System.out.println("Running Quasistatis routine");
// if(m_controller.getRawButton(8)) {
// System.out.println("Reverse");
// Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse));
// } else {
// System.out.println("Forward");
// Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward));
// }
// }));
// new Trigger(() -> m_controller.getRawButton(4) && m_controller()).onTrue(() -> {
// System.out.println("Running Dynamic routine");
// if(m_controller.getRawButton(8)) {
// System.out.println("Reverse");
// return m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse);
// } else {
// System.out.println("Forward");
// return m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward);
// }
// }());
}
public Command getAutonomousCommand()
{
return Commands.print("No autonomous command configured");
}
}
|
