1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.RobotMap
;
5 import edu
.wpi
.first
.wpilibj
.CANJaguar
;
6 import edu
.wpi
.first
.wpilibj
.RobotDrive
;
7 import edu
.wpi
.first
.wpilibj
.command
.Subsystem
;
9 public class Drivetrain
extends Subsystem
{
11 RobotDrive robotDrive
;
14 CANJaguar frontLeft
= new CANJaguar(RobotMap
.FRONT_LEFT_ADDRESS
);
15 CANJaguar frontRight
= new CANJaguar(RobotMap
.FRONT_RIGHT_ADDRESS
);
16 CANJaguar rearLeft
= new CANJaguar(RobotMap
.REAR_LEFT_ADDRESS
);
17 CANJaguar rearRight
= new CANJaguar(RobotMap
.REAR_RIGHT_ADDRESS
);
19 robotDrive
= new RobotDrive(
21 frontRight
, rearRight
);
24 public void drive(double forward
, double twist
) {
25 if (Math
.abs(forward
) < RobotMap
.MIN_DRIVE_JOYSTICK_INPUT
)
27 if (Math
.abs(twist
) < RobotMap
.MIN_DRIVE_JOYSTICK_INPUT
)
30 robotDrive
.arcadeDrive(
31 RobotMap
.MAX_DRIVE_SPEED
* adjust(forward
),
32 RobotMap
.MAX_DRIVE_SPEED
* adjust(twist
),
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() {}