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
   5 import edu.wpi.first.wpilibj.CANJaguar;
   6 import edu.wpi.first.wpilibj.RobotDrive;
   7 import edu.wpi.first.wpilibj.command.Subsystem;
   8
   9 public class Drivetrain extends Subsystem {
  10
  11     RobotDrive robotDrive;
  12
  13     public Drivetrain() {
  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);
  18
  19         robotDrive = new RobotDrive(
  20                 frontLeft,  rearLeft,
  21                 frontRight, rearRight);
  22     }
  23
  24     public void drive(double forward, double twist) {
  25         if (Math.abs(forward) < RobotMap.MIN_DRIVE_JOYSTICK_INPUT)
  26             forward = 0;
  27         if (Math.abs(twist) < RobotMap.MIN_DRIVE_JOYSTICK_INPUT)
  28             twist = 0;
  29
  30         robotDrive.arcadeDrive(
  31                 RobotMap.MAX_DRIVE_SPEED * adjust(forward),
  32                 RobotMap.MAX_DRIVE_SPEED * adjust(twist),
  33                 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 }
  51