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.CANJaguar;
   7 import edu.wpi.first.wpilibj.RobotDrive;
   8 import edu.wpi.first.wpilibj.command.Subsystem;
   9
  10 public class Drivetrain extends Subsystem {
  11
  12     private RobotDrive robotDrive;
  13
  14     public Drivetrain() {
  15         CANJaguar frontLeft  = new CANJaguar(RobotMap.FRONT_LEFT_ADDRESS);
  16         CANJaguar frontRight = new CANJaguar(RobotMap.FRONT_RIGHT_ADDRESS);
  17         CANJaguar rearLeft   = new CANJaguar(RobotMap.REAR_LEFT_ADDRESS);
  18         CANJaguar rearRight  = new CANJaguar(RobotMap.REAR_RIGHT_ADDRESS);
  19
  20         robotDrive = new RobotDrive(
  21                 frontLeft,  rearLeft,
  22                 frontRight, rearRight);
  23     }
  24
  25     public void drive(double forward, double twist) {
  26         if (Math.abs(forward) < RobotMap.MIN_DRIVE_JOYSTICK_INPUT)
  27             forward = 0;
  28         if (Math.abs(twist) < RobotMap.MIN_DRIVE_JOYSTICK_INPUT)
  29             twist = 0;
  30
  31         robotDrive.arcadeDrive(
  32                 RobotMap.MAX_DRIVE_SPEED * adjust(forward),
  33                 RobotMap.MAX_DRIVE_SPEED * adjust(twist),
  34                 false);
  35     }
  36
  37     public void goForward(double speed) {
  38         robotDrive.arcadeDrive(speed, 0);
  39     }
  40
  41     public void stop() {
  42         robotDrive.arcadeDrive(0, 0);
  43     }
  44
  45     // output is avg of `x` and `sqrt(x)`
  46     private double adjust(double x) {
  47         return (x + Math.signum(x) * Math.sqrt(Math.abs(x))) / 2;
  48     }
  49
  50     public void initDefaultCommand() {
  51         setDefaultCommand(new DriveWithJoysticks());
  52     }
  53 }