}
public static class DriveTrain {
- // Drivetrain Motor Related Ports
+ public static final int TANK_DRIVE = 0;
+ public static final int ARCADE_DRIVE = 1;
+ public static final int DRIVE_TYPE = TANK_DRIVE;
+
+ // Drivetrain Motor related ports
public static final int DRIVE_FRONT_LEFT = 1;
public static final int DRIVE_REAR_LEFT = 2;
public static final int DRIVE_FRONT_RIGHT = 6;
public static final int DRIVE_REAR_RIGHT = 5;
- // Encoder related ports
- public final static int ENCODER_LEFT_A = 0;
- public final static int ENCODER_LEFT_B = 1;
- public final static int ENCODER_RIGHT_A = 3;
- public final static int ENCODER_RIGHT_B = 4;
-
+ // Drivetrain shifter related ports
public static final int LEFT_SHIFT_MODULE = PCM_MODULE_B;
public static final int LEFT_SHIFT_FORWARD = 3;
public static final int LEFT_SHIFT_REVERSE = 6;
public static final int RIGHT_SHIFT_FORWARD = 2;
public static final int RIGHT_SHIFT_REVERSE = 7;
+ // Encoder related ports
+ public final static int ENCODER_LEFT_A = 0;
+ public final static int ENCODER_LEFT_B = 1;
+ public final static int ENCODER_RIGHT_A = 3;
+ public final static int ENCODER_RIGHT_B = 4;
+
public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256;
public static double kp = 0.013, ki = 0.000015, kd = -0.002;
package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
import edu.wpi.first.wpilibj.CANTalon;
this.disable();
leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
- Constants.DriveTrain.LEFT_SHIFT_FORWARD, Constants.DriveTrain.LEFT_SHIFT_REVERSE);
- rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_SHIFT_MODULE,
- Constants.DriveTrain.RIGHT_SHIFT_FORWARD, Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
+ Constants.DriveTrain.LEFT_SHIFT_FORWARD,
+ Constants.DriveTrain.LEFT_SHIFT_REVERSE);
+ rightGearPiston = new DoubleSolenoid(
+ Constants.DriveTrain.RIGHT_SHIFT_MODULE,
+ Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
+ Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
}
@Override
}
public void stop() {
- drive(0, 0);
+ setMotorSpeeds(0, 0);
}
public void resetEncoders() {
output = Math.signum(output) * 0.3;
left = output;
right = output + drift * Constants.DriveTrain.kp / 10;
- drive(left, right);
+ setMotorSpeeds(left, right);
pidOutput = output;
}
return getAvgEncoderDistance();
}
- /*
- * @param left and right setpoints to set to the left and right side of tank
- * inverted is for Logan, wants the robot to invert all controls left = right
- * and right = left negative input is required for the regular rotation
- * because RobotDrive tankdrive method drives inverted
- */
- public void drive(double left, double right) {
- robotDrive.tankDrive(-left, -right);
+ public void joystickDrive(double left, double right) {
+ int type = Constants.DriveTrain.DRIVE_TYPE;
+ double k = (isFlipped() ? -1 : 1);
+ if (type == Constants.DriveTrain.TANK_DRIVE) {
+ robotDrive.tankDrive(-left * k, -right * k);
+ } else if (type == Constants.DriveTrain.ARCADE_DRIVE) {
+ robotDrive.arcadeDrive(left * k, right);
+ }
}
public void setMotorSpeeds(double left, double right) {
- // positive setpoint to left side makes it go backwards
- // positive setpoint to right side makes it go forwards.
- frontLeft.set(-left);
- rearLeft.set(-left);
- frontRight.set(right);
- rearRight.set(right);
+ double k = (Robot.driveTrain.isFlipped() ? -1 : 1);
+ robotDrive.tankDrive(-left * k, -right * k);
}
/**