a8716b6ba9f472edf2e0ddd3d2bc3090e8fab7d6
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 inches per second
34 return rightEncoder
.getRate();
37 public double getLeftSpeed() {
38 return leftEncoder
.getRate();
41 public double getAverageSpeed() {
42 return (getRightSpeed() + getLeftSpeed())/2;
45 public double getRightDistance() {
46 // Returns distance in inches
47 return rightEncoder
.getDistance();
50 public double getLeftDistance() {
51 // Returns distance in inches
52 return leftEncoder
.getDistance();
59 public void setMotorSpeeds(double leftSpeed
, double rightSpeed
) {
60 if (Math
.abs(leftSpeed
) < RobotMap
.DRIVE_DEAD_ZONE
) {
63 if (Math
.abs(rightSpeed
) < RobotMap
.DRIVE_DEAD_ZONE
) {
66 this.frontLeft
.set(leftSpeed
);
67 this.frontRight
.set(-rightSpeed
);
68 this.rearLeft
.set(leftSpeed
);
69 this.rearRight
.set(-rightSpeed
);
72 public void arcadeDrive(double yVal
, double twist
) {
73 if (yVal
< 0 && Math
.abs(yVal
) < 0.1125) {
75 } else if (yVal
> 0 && Math
.abs(yVal
) < 0.25) {
77 } else if (yVal
> 0) {
79 } else if (yVal
< 0) {
82 if (Math
.abs(twist
) < RobotMap
.DRIVE_DEAD_ZONE
) {
86 double leftMotorSpeed
;
87 double rightMotorSpeed
;
88 // adjust the sensitivity (yVal+rootof (yval)) / 2
89 yVal
= (yVal
+ Math
.signum(yVal
) * Math
.sqrt(Math
.abs(yVal
))) / 2;
90 // adjust the sensitivity (twist+rootof (twist)) / 2
91 twist
= (twist
+ Math
.signum(twist
) * Math
.sqrt(Math
.abs(twist
))) / 2;
94 leftMotorSpeed
= yVal
- twist
;
95 rightMotorSpeed
= Math
.max(yVal
, twist
);
97 leftMotorSpeed
= Math
.max(yVal
, -twist
);
98 rightMotorSpeed
= yVal
+ twist
;
102 leftMotorSpeed
= -Math
.max(-yVal
, twist
);
103 rightMotorSpeed
= yVal
+ twist
;
105 leftMotorSpeed
= yVal
- twist
;
106 rightMotorSpeed
= -Math
.max(-yVal
, -twist
);
109 setMotorSpeeds(leftMotorSpeed
, rightMotorSpeed
);
113 protected void initDefaultCommand() {