challenge-bot.com Git - 3501/3501-spark-go/blob - src/org/usfirst/frc/team3501/robot/subsystems/Drivetrain.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   3 import org.usfirst.frc.team3501.robot.RobotMap;
   4 import org.usfirst.frc.team3501.robot.commands.DriveWithJoysticks;
   5
   6 import edu.wpi.first.wpilibj.RobotDrive;
   7 import edu.wpi.first.wpilibj.command.Subsystem;
   8
   9 public class Drivetrain extends Subsystem {
  10
  11     private final RobotDrive robotDrive;
  12
  13     public Drivetrain() {
  14
  15         robotDrive = new RobotDrive(
  16                 RobotMap.frontLeft,  RobotMap.rearLeft,
  17                 RobotMap.frontRight, RobotMap.rearRight);
  18     }
  19
  20     public void drive(double forward, double twist) {
  21         if (Math.abs(forward) < RobotMap.MIN_DRIVE_JOYSTICK_INPUT)
  22             forward = 0;
  23         if (Math.abs(twist)   < RobotMap.MIN_DRIVE_JOYSTICK_INPUT)
  24             twist = 0;
  25
  26         robotDrive.arcadeDrive(
  27                 RobotMap.MAX_DRIVE_SPEED * adjust(forward),
  28                 RobotMap.MAX_DRIVE_SPEED * adjust(twist),
  29                 false);
  30     }
  31
  32     public void driveRaw(double forward, double twist) {
  33         robotDrive.arcadeDrive(forward, twist, false);
  34     }
  35
  36     public void goForward(double speed) {
  37         robotDrive.arcadeDrive(speed, 0);
  38     }
  39
  40     public void stop() {
  41         robotDrive.arcadeDrive(0, 0);
  42     }
  43
  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;
  47     }
  48
  49     public void initDefaultCommand() {
  50         setDefaultCommand(new DriveWithJoysticks());
  51     }
  52 }