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 private CANJaguar frontLeft
, frontRight
, rearLeft
, rearRight
;
12 private Encoder leftEncoder
, rightEncoder
;
15 frontLeft
= new CANJaguar(RobotMap
.DRIVE_FRONT_LEFT
);
16 frontRight
= new CANJaguar(RobotMap
.DRIVE_FRONT_RIGHT
);
17 rearLeft
= new CANJaguar(RobotMap
.DRIVE_REAR_LEFT
);
18 rearRight
= new CANJaguar(RobotMap
.DRIVE_REAR_RIGHT
);
19 leftEncoder
= new Encoder(RobotMap
.DRIVE_LEFT_A
, RobotMap
.DRIVE_LEFT_B
,
20 false, EncodingType
.k4X
);
21 rightEncoder
= new Encoder(RobotMap
.DRIVE_RIGHT_A
,
22 RobotMap
.DRIVE_RIGHT_B
, false, EncodingType
.k4X
);
23 leftEncoder
.setDistancePerPulse(RobotMap
.DISTANCE_PER_PULSE
);
24 rightEncoder
.setDistancePerPulse(RobotMap
.DISTANCE_PER_PULSE
);
27 public void resetEncoders() {
32 public double getRightSpeed() {
33 // Returns in per second
34 return rightEncoder
.getRate();
37 public double getLeftSpeed() {
38 return leftEncoder
.getRate();
41 public double getRightDistance() {
42 // Returns distance in in
43 return rightEncoder
.getDistance();
46 public double getLeftDistance() {
47 // Returns distance in in
48 return leftEncoder
.getDistance();
51 public void setMotorSpeeds(double leftSpeed
, double rightSpeed
) {
52 if (Math
.abs(leftSpeed
) < RobotMap
.DRIVE_DEAD_ZONE
) {
55 if (Math
.abs(rightSpeed
) < RobotMap
.DRIVE_DEAD_ZONE
) {
58 this.frontLeft
.set(leftSpeed
);
59 this.frontRight
.set(-rightSpeed
);
60 this.rearLeft
.set(leftSpeed
);
61 this.rearRight
.set(-rightSpeed
);
64 public void arcadeDrive(double yVal
, double twist
) {
65 if (yVal
< 0 && Math
.abs(yVal
) < 0.1125) {
67 } else if (yVal
> 0 && Math
.abs(yVal
) < 0.25) {
69 } else if (yVal
> 0) {
71 } else if (yVal
< 0) {
74 if (Math
.abs(twist
) < RobotMap
.DRIVE_DEAD_ZONE
) {
78 double leftMotorSpeed
;
79 double rightMotorSpeed
;
80 // adjust the sensitivity (yVal+rootof (yval)) / 2
81 yVal
= (yVal
+ Math
.signum(yVal
) * Math
.sqrt(Math
.abs(yVal
))) / 2;
82 // adjust the sensitivity (twist+rootof (twist)) / 2
83 twist
= (twist
+ Math
.signum(twist
) * Math
.sqrt(Math
.abs(twist
))) / 2;
86 leftMotorSpeed
= yVal
- twist
;
87 rightMotorSpeed
= Math
.max(yVal
, twist
);
89 leftMotorSpeed
= Math
.max(yVal
, -twist
);
90 rightMotorSpeed
= yVal
+ twist
;
94 leftMotorSpeed
= -Math
.max(-yVal
, twist
);
95 rightMotorSpeed
= yVal
+ twist
;
97 leftMotorSpeed
= yVal
- twist
;
98 rightMotorSpeed
= -Math
.max(-yVal
, -twist
);
101 setMotorSpeeds(leftMotorSpeed
, rightMotorSpeed
);
105 protected void initDefaultCommand() {