de1efe61d9b239ce71822b240a41af4a3f4cb5b0
1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.RobotMap
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.DriveWithJoysticks
;
6 import edu
.wpi
.first
.wpilibj
.RobotDrive
;
7 import edu
.wpi
.first
.wpilibj
.command
.Subsystem
;
9 public class Drivetrain
extends Subsystem
{
11 private final RobotDrive robotDrive
;
15 robotDrive
= new RobotDrive(
16 RobotMap
.frontLeft
, RobotMap
.rearLeft
,
17 RobotMap
.frontRight
, RobotMap
.rearRight
);
20 public void drive(double forward
, double twist
) {
21 if (Math
.abs(forward
) < RobotMap
.MIN_DRIVE_JOYSTICK_INPUT
)
23 if (Math
.abs(twist
) < RobotMap
.MIN_DRIVE_JOYSTICK_INPUT
)
26 robotDrive
.arcadeDrive(
27 RobotMap
.MAX_DRIVE_SPEED
* adjust(forward
),
28 RobotMap
.MAX_DRIVE_SPEED
* adjust(twist
),
32 public void driveRaw(double forward
, double twist
) {
33 robotDrive
.arcadeDrive(forward
, twist
, false);
36 public void goForward(double speed
) {
37 robotDrive
.arcadeDrive(speed
, 0);
41 robotDrive
.arcadeDrive(0, 0);
44 // output is avg of `x` and `sqrt(x)`
45 private double adjust(double x
) {
46 return (x
+ Math
.signum(x
) * Math
.sqrt(Math
.abs(x
))) / 2;
49 public void initDefaultCommand() {
50 setDefaultCommand(new DriveWithJoysticks());