1 package org
.usfirst
.frc3501
.RiceCatRobot
.subsystems
;
3 import org
.usfirst
.frc3501
.RiceCatRobot
.RobotMap
;
5 import edu
.wpi
.first
.wpilibj
.CANJaguar
;
6 import edu
.wpi
.first
.wpilibj
.Encoder
;
7 import edu
.wpi
.first
.wpilibj
.CounterBase
.EncodingType
;
8 import edu
.wpi
.first
.wpilibj
.command
.Subsystem
;
10 public class DriveTrain
extends Subsystem
{
11 public static final double MOTOR_MAX_VAL
= 1;
12 public static final double MOTOR_MIN_VAL
= -1;
14 private CANJaguar frontLeft
, frontRight
, rearLeft
, rearRight
;
15 private Encoder leftEncoder
, rightEncoder
;
18 frontLeft
= new CANJaguar(RobotMap
.DRIVE_FRONT_LEFT
);
19 frontRight
= new CANJaguar(RobotMap
.DRIVE_FRONT_RIGHT
);
20 rearLeft
= new CANJaguar(RobotMap
.DRIVE_REAR_LEFT
);
21 rearRight
= new CANJaguar(RobotMap
.DRIVE_REAR_RIGHT
);
22 leftEncoder
= new Encoder(RobotMap
.DRIVE_LEFT_A
, RobotMap
.DRIVE_LEFT_B
,
23 false, EncodingType
.k4X
);
24 rightEncoder
= new Encoder(RobotMap
.DRIVE_RIGHT_A
, RobotMap
.DRIVE_RIGHT_B
,
25 false, EncodingType
.k4X
);
26 leftEncoder
.setDistancePerPulse(RobotMap
.DISTANCE_PER_PULSE
);
27 rightEncoder
.setDistancePerPulse(RobotMap
.DISTANCE_PER_PULSE
);
30 public void resetEncoders() {
35 public double getRightSpeed() {
36 // Returns in per second
37 return rightEncoder
.getRate();
40 public double getLeftSpeed() {
41 return leftEncoder
.getRate();
44 public double getRightDistance() {
45 // Returns distance in in
46 return rightEncoder
.getDistance();
49 public double getLeftDistance() {
50 // Returns distance in in
51 return leftEncoder
.getDistance();
58 public void setMotorSpeeds(double leftSpeed
, double rightSpeed
) {
59 if (Math
.abs(leftSpeed
) < RobotMap
.DRIVE_DEAD_ZONE
) {
62 if (Math
.abs(rightSpeed
) < RobotMap
.DRIVE_DEAD_ZONE
) {
65 this.frontLeft
.set(leftSpeed
);
66 this.frontRight
.set(-rightSpeed
);
67 this.rearLeft
.set(leftSpeed
);
68 this.rearRight
.set(-rightSpeed
);
71 public void arcadeDrive(double yVal
, double twist
) {
72 if (yVal
< 0 && Math
.abs(yVal
) < 0.1125) {
74 } else if (yVal
> 0 && Math
.abs(yVal
) < 0.25) {
76 } else if (yVal
> 0) {
78 } else if (yVal
< 0) {
81 if (Math
.abs(twist
) < RobotMap
.DRIVE_DEAD_ZONE
) {
85 double leftMotorSpeed
;
86 double rightMotorSpeed
;
87 // adjust the sensitivity (yVal+rootof (yval)) / 2
88 yVal
= (yVal
+ Math
.signum(yVal
) * Math
.sqrt(Math
.abs(yVal
))) / 2;
89 // adjust the sensitivity (twist+rootof (twist)) / 2
90 twist
= (twist
+ Math
.signum(twist
) * Math
.sqrt(Math
.abs(twist
))) / 2;
93 leftMotorSpeed
= yVal
- twist
;
94 rightMotorSpeed
= Math
.max(yVal
, twist
);
96 leftMotorSpeed
= Math
.max(yVal
, -twist
);
97 rightMotorSpeed
= yVal
+ twist
;
101 leftMotorSpeed
= -Math
.max(-yVal
, twist
);
102 rightMotorSpeed
= yVal
+ twist
;
104 leftMotorSpeed
= yVal
- twist
;
105 rightMotorSpeed
= -Math
.max(-yVal
, -twist
);
108 setMotorSpeeds(leftMotorSpeed
, rightMotorSpeed
);
112 protected void initDefaultCommand() {