diff --git a/Drive.java b/Drive.java new file mode 100644 index 0000000..12d1674 --- /dev/null +++ b/Drive.java @@ -0,0 +1,853 @@ + +package org.usfirst.frc4388.robot.subsystems; + +import java.util.ArrayList; +import java.util.Set; + +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.robot.OI; +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.utility.AdaptivePurePursuitController; +import org.usfirst.frc4388.utility.BHRMathUtils; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; +import org.usfirst.frc4388.utility.Kinematics; +import org.usfirst.frc4388.utility.MMTalonPIDController; +import org.usfirst.frc4388.utility.MPSoftwarePIDController; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; +import org.usfirst.frc4388.utility.MPTalonPIDController; +import org.usfirst.frc4388.utility.MPTalonPIDPathController; +import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController; +//import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.utility.MotionProfilePoint; +//import org.usfirst.frc4388.utility.MotionProfileBoxCar; +import org.usfirst.frc4388.utility.PIDParams; +import org.usfirst.frc4388.utility.Path; +import org.usfirst.frc4388.utility.PathGenerator; +import org.usfirst.frc4388.utility.RigidTransform2d; +import org.usfirst.frc4388.utility.Rotation2d; +import org.usfirst.frc4388.utility.SoftwarePIDController; +import org.usfirst.frc4388.utility.Translation2d; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +//import com.ctre.PigeonImu; +//import com.ctre.PigeonImu.CalibrationMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import com.kauailabs.navx.frc.AHRS; + +//import edu.wpi.first.wpilibj.DigitalInput; +//import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +//import edu.wpi.first.wpilibj.Solenoid; +//import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +/** + * + */ +public class Drive extends Subsystem implements ControlLoopable +{ + public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW}; + public static enum SpeedShiftState { HI, LO }; + public static enum ClimberState { DEPLOYED, RETRACTED }; + + public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches; + public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch; + + public static final double CLIMB_SPEED = 0.45; + + public static final double VOLTAGE_RAMP_RATE = 150; // Volts per second + public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full + public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full + + // Motion profile max velocities and accel times + public static final double MAX_TURN_RATE_DEG_PER_SEC = 320; + public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 120; //72; + public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC = 200; + public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320; + public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400; + public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270; + public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC = 400; + public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25; + public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80; + + + + + public static final double MP_STRAIGHT_T1 = 600; + public static final double MP_STRAIGHT_T2 = 300; + public static final double MP_TURN_T1 = 600; + public static final double MP_TURN_T2 = 300; + public static final double MP_MAX_TURN_T1 = 400; + public static final double MP_MAX_TURN_T2 = 200; + + // Motor controllers + private ArrayList motorControllers = new ArrayList(); + + private CANTalonEncoder leftDrive1; + private WPI_TalonSRX leftDrive2; +// private WPI_TalonSRX leftDrive3; + + private CANTalonEncoder rightDrive1; + private WPI_TalonSRX rightDrive2; +// private WPI_TalonSRX rightDrive3; + + private DifferentialDrive m_drive; + + //PID encoder and motor + private CANTalonEncoder elevatorRight; + private WPI_TalonSRX elevatorLeft; + + //private DigitalInput hopperSensorRed; + //private DigitalInput hopperSensorBlue; + + + private double climbSpeed; + + private boolean isRed = true; + + private double periodMs; + private double lastControlLoopUpdatePeriod = 0.0; + private double lastControlLoopUpdateTimestamp = 0.0; + + // Pneumatics + //private Solenoid speedShift; + + // Input devices + public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0; + public static final int DRIVER_INPUT_JOYSTICK_TANK = 1; + public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2; + public static final int DRIVER_INPUT_XBOX_CHEESY = 3; + public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4; + public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5; + public static final int DRIVER_INPUT_WHEEL = 6; + + public static final double STEER_NON_LINEARITY = 0.5; + public static final double MOVE_NON_LINEARITY = 1.0; + + public static final double STICK_DEADBAND = 0.02; + + private int m_moveNonLinear = 0; + private int m_steerNonLinear = -3; + + private double m_moveScale = 1.0; + private double m_steerScale = 1.0; + + private double m_moveInput = 0.0; + private double m_steerInput = 0.0; + + private double m_moveOutput = 0.0; + private double m_steerOutput = 0.0; + + private double m_moveTrim = 0.0; + private double m_steerTrim = 0.0; + + private boolean isFinished; + private DriveControlMode controlMode = DriveControlMode.JOYSTICK; + + private MPTalonPIDController mpStraightController; + private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons +// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni + private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); + + private MMTalonPIDController mmStraightController; + private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24); + + private MPSoftwarePIDController mpTurnController; // p i d a v g izone + private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels +// private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni + + private SoftwarePIDController pidTurnController; + //private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008 + private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008 + private double targetPIDAngle; + + private MPTalonPIDPathController mpPathController; + private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100); // 4 omni g=.3 + + //PID target + private double targetPIDPosition; + + private MPTalonPIDPathVelocityController mpPathVelocityController; + private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); + + private AdaptivePurePursuitController adaptivePursuitController; + private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); + + private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); + private RigidTransform2d currentPose = zeroPose; + private RigidTransform2d lastPose = zeroPose; + private double left_encoder_prev_distance_ = 0; + private double right_encoder_prev_distance_ = 0; + + //private PigeonImu gyroPigeon; + //private double[] yprPigeon = new double[3]; + private AHRS gyroNavX; + private boolean useGyroLock; + private double gyroLockAngleDeg; + //private double kPGyro = 0.04; + private double kPGyro = 0.0625; + private boolean isCalibrating = false; + private double gyroOffsetDeg = 0; + + public Drive() { + try { + leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); + leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID); +// leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID); + + rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); + rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); +// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); + + //gyroPigeon = new PigeonImu(leftDrive2); + gyroNavX = new AHRS(SPI.Port.kMXP); + + //hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID); + //hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID); + + //leftDrive1.clearStickyFaults(); + //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); + //leftDrive1.setNominalClosedLoopVoltage(12.0); + leftDrive1.clearStickyFaults(0); + leftDrive1.setInverted(false);//false on comp 2108, false on microbot + leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot + leftDrive1.setSafetyEnabled(false); + //leftDrive1.setCurrentLimit(15); + //leftDrive1.enableCurrentLimit(true); + leftDrive1.setNeutralMode(NeutralMode.Brake); + leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); + leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); + leftDrive1.configNominalOutputForward(+1.0f, 0); + leftDrive1.configNominalOutputReverse(-1.0f, 0); + + +// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { +// Driver.reportError("Could not detect left drive encoder encoder!", false); +// } + + + leftDrive2.setInverted(false);//false on comp 2108, false on microbot + leftDrive2.setSafetyEnabled(false); + leftDrive2.setNeutralMode(NeutralMode.Brake); + leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); + + + + //rightDrive1.clearStickyFaults(); + //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); + //rightDrive1.setNominalClosedLoopVoltage(12.0); + rightDrive1.clearStickyFaults(0); + rightDrive1.setInverted(true);//true on comp 2108, false on microbot + rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot + rightDrive1.setSafetyEnabled(false); + rightDrive1.setNeutralMode(NeutralMode.Brake); + rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); + rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); + rightDrive1.configNominalOutputForward(+1.0f, 0); + rightDrive1.configNominalOutputReverse(-1.0f, 0); +// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { +// DriverStation.reportError("Could not detect right drive encoder encoder!", false); +// } + + + rightDrive2.setInverted(true);//True on comp 2108, false on microbot + rightDrive2.setSafetyEnabled(false); + rightDrive2.setNeutralMode(NeutralMode.Brake); + rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); + + + + + motorControllers.add(leftDrive1); + motorControllers.add(rightDrive1); + + //m_drive = new RobotDrive(leftDrive1, rightDrive1); + //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); + //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true); + //m_drive.setSafetyEnabled(false); + m_drive = new DifferentialDrive(leftDrive1, rightDrive1); + //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true); //TODO URGENT: verify + //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify + m_drive.setSafetyEnabled(false); + + //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); + } + catch (Exception e) { + System.err.println("An error occurred in the DriveTrain constructor"); + } + } + + public void setToBrakeOnNeutral(boolean brakeVsCoast) { + if (brakeVsCoast) { + leftDrive1.setNeutralMode(NeutralMode.Brake); + leftDrive2.setNeutralMode(NeutralMode.Brake); + rightDrive1.setNeutralMode(NeutralMode.Brake); + rightDrive2.setNeutralMode(NeutralMode.Brake); + } else { + leftDrive1.setNeutralMode(NeutralMode.Coast); + leftDrive2.setNeutralMode(NeutralMode.Coast); + rightDrive1.setNeutralMode(NeutralMode.Coast); + rightDrive2.setNeutralMode(NeutralMode.Coast); + } + } + + @Override + public void initDefaultCommand() { + } + + public double getGyroAngleDeg() { + //return getGyroPigeonAngleDeg(); + return getGyroNavXAngleDeg(); + } + + //public double getGyroPigeonAngleDeg() { + // gyroPigeon.GetYawPitchRoll(yprPigeon); + // return -yprPigeon[0] + gyroOffsetDeg; + //} + + public double getGyroNavXAngleDeg() { + return gyroNavX.getAngle() + gyroOffsetDeg; + } + + public void resetGyro() { + //gyroPigeon.SetYaw(0); + gyroNavX.zeroYaw(); + } + + public void resetEncoders() { + mpStraightController.resetZeroPosition(); + } + + public void calibrateGyro() { + //gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature); + } + + public void endGyroCalibration() { + if (isCalibrating == true) { + isCalibrating = false; + } + } + + public void setGyroOffset(double offsetDeg) { + gyroOffsetDeg = offsetDeg; + } + + //public boolean isHopperSensorRedOn() { + // return hopperSensorRed.get(); + //} + + //public boolean isHopperSensorBlueOn() { + // return hopperSensorBlue.get(); + //} + + //public boolean isHopperSensorOn() { + // if (isRed() == true) { + // return isHopperSensorRedOn(); + // } + // else { + // return isHopperSensorBlueOn(); + // } + //} + + public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); + mmStraightController.setPID(mmStraightPIDParams); + mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); + setControlMode(DriveControlMode.MOTION_MAGIC); + } + + public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); + mpStraightController.setPID(mpStraightPIDParams); + mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); + setControlMode(DriveControlMode.MP_STRAIGHT); + } + + //public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + // double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); + // mpStraightController.setPID(mpStraightPIDParams); + // mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); + // setControlMode(DriveControlMode.MP_STRAIGHT); + //} + + public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { + mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); + setControlMode(DriveControlMode.MP_TURN); + } + + //public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) { + // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); + // setControlMode(DriveControlMode.MP_TURN); + //} + + public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { + mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES); + setControlMode(DriveControlMode.MP_TURN); + } + + public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { + mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); + setControlMode(DriveControlMode.MP_TURN); + } + + //public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) { + // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); + // setControlMode(DriveControlMode.MP_TURN); + //} + + public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { + this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg(); + pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); + setControlMode(DriveControlMode.PID_TURN); + } + + public void setPathMP(PathGenerator path) { + mpPathController.setPID(mpPathPIDParams); + mpPathController.setMPPathTarget(path); + setControlMode(DriveControlMode.MP_PATH); + } + + public void setPathVelocityMP(PathGenerator path) { + mpPathVelocityController.setPID(mpPathPIDParams); + mpPathVelocityController.setMPPathTarget(path); + setControlMode(DriveControlMode.MP_PATH_VELOCITY); + } + + public void setPathAdaptivePursuit(Path path, boolean reversed) { + currentPose = zeroPose; + lastPose = zeroPose; + left_encoder_prev_distance_ = 0; + right_encoder_prev_distance_ = 0; + adaptivePursuitController.setPID(adaptivePursuitPIDParams); + adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); + setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); + } + + public void setDriveHold(boolean status) { + if (status) { + setControlMode(DriveControlMode.HOLD); + } + else { + setControlMode(DriveControlMode.JOYSTICK); + } + } + + public void updatePose() { + double left_distance = leftDrive1.getPositionWorld(); + double right_distance = rightDrive1.getPositionWorld(); + Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); + lastPose = currentPose; + currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); + left_encoder_prev_distance_ = left_distance; + right_encoder_prev_distance_ = right_distance; + } + + public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) { + return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle); + } + + /** + * Path Markers are an optional functionality that name the various + * Waypoints in a Path with a String. This can make defining set locations + * much easier. + * + * @return Set of Strings with Path Markers that the robot has crossed. + */ + public synchronized Set getPathMarkersCrossed() { + if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) { + return null; + } else { + return adaptivePursuitController.getMarkersCrossed(); + } + } + + public synchronized void setControlMode(DriveControlMode controlMode) { + this.controlMode = controlMode; + if (controlMode == DriveControlMode.JOYSTICK) { + //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); + //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); + leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + } + else if (controlMode == DriveControlMode.MANUAL) { + //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); + //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); + leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + } + else if (controlMode == DriveControlMode.CLIMB) { + //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); + //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); + leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + } + else if (controlMode == DriveControlMode.HOLD) { + mpStraightController.setPID(mpHoldPIDParams); + //leftDrive1.changeControlMode(TalonControlMode.Position); + //leftDrive1.setPosition(0); + //leftDrive1.set(0); + //rightDrive1.changeControlMode(TalonControlMode.Position); + //rightDrive1.setPosition(0); + //rightDrive1.set(0); + leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + leftDrive1.set(ControlMode.Position, 0); + rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + rightDrive1.set(ControlMode.Position, 0); + } + isFinished = false; + } + + public synchronized void controlLoopUpdate() { + // Measure *actual* update period + double currentTimestamp = Timer.getFPGATimestamp(); + if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time + lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; + } + lastControlLoopUpdateTimestamp = currentTimestamp; + + // Do the update + if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { + driveWithJoystick(); + } + else if (!isFinished) { + if (controlMode == DriveControlMode.MP_STRAIGHT) { + isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MP_TURN) { + isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.PID_TURN) { + isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MP_PATH) { + isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { + isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MOTION_MAGIC) { + isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) { + updatePose(); + isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + } + } + } + + public void setSpeed(double speed) { + if (speed == 0) { + setControlMode(DriveControlMode.JOYSTICK); + } + else { + setControlMode(DriveControlMode.MANUAL); + rightDrive1.set(-speed); + leftDrive1.set(speed); + } + } + + public void setClimbSpeed(double climbSpeed) { + this.climbSpeed = climbSpeed; + if (climbSpeed == 0) { + setControlMode(DriveControlMode.JOYSTICK); + } + else { + setControlMode(DriveControlMode.CLIMB); + } + } + + public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) { + if (snapToAbsolute0or180) { + gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg()); + } + else { + gyroLockAngleDeg = getGyroAngleDeg(); + } + this.useGyroLock = useGyroLock; + } + + public void driveWithJoystick() { + if(m_drive == null) return; + // switch(m_controllerMode) { + // case CONTROLLER_JOYSTICK_ARCADE: + // m_moveInput = OI.getInstance().getJoystick1().getY(); + // m_steerInput = OI.getInstance().getJoystick1().getX(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_JOYSTICK_TANK: + // m_moveInput = OI.getInstance().getJoystick1().getY(); + // m_steerInput = OI.getInstance().getJoystick2().getY(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_drive.tankDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_JOYSTICK_CHEESY: + // m_moveInput = OI.getInstance().getJoystick1().getY(); + // m_steerInput = OI.getInstance().getJoystick2().getX(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_XBOX_CHEESY: + // boolean turbo = OI.getInstance().getDriveTrainController() + // .getLeftJoystickButton(); + // boolean slow = OI.getInstance().getDriveTrainController() + // .getRightJoystickButton(); + // double speedToUseMove, speedToUseSteer; + // if (turbo && !slow) { + // speedToUseMove = m_moveScaleTurbo; + // speedToUseSteer = m_steerScaleTurbo; + // } else if (!turbo && slow) { + // speedToUseMove = m_moveScaleSlow; + // speedToUseSteer = m_steerScaleSlow; + // } else { + // speedToUseMove = m_moveScale; + // speedToUseSteer = m_steerScale; + // } + + // m_moveInput = + // OI.getInstance().getDriveTrainController().getLeftYAxis(); + // m_steerInput = + // OI.getInstance().getDriveTrainController().getRightXAxis(); + m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis(); + m_steerInput = OI.getInstance().getDriverController().getRightXAxis(); + + if (controlMode == DriveControlMode.JOYSTICK) { + m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + } + else if (controlMode == DriveControlMode.CLIMB) { + m_moveOutput = climbSpeed; + } + + if (useGyroLock) { // If currently in gyro lock mode, + if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning, + setGyroLock(false, false); // turn off gyro lock mode + } + } else { // If not yet in gyro lock mode, + if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning, + setGyroLock(true, false); // gyro lock to current heading + } + } + + if (useGyroLock) { + double yawError = gyroLockAngleDeg - getGyroAngleDeg(); + m_steerOutput = kPGyro * yawError; + } else { + m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + } + + m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75); + // break; + // case CONTROLLER_XBOX_ARCADE_RIGHT: + // m_moveInput = + // OI.getInstance().getDrivetrainController().getRightYAxis(); + // m_steerInput = + // OI.getInstance().getDrivetrainController().getRightXAxis(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_XBOX_ARCADE_LEFT: + // m_moveInput = + // OI.getInstance().getDrivetrainController().getLeftYAxis(); + // m_steerInput = + // OI.getInstance().getDrivetrainController().getLeftXAxis(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // } + } + + public void rawMoveSteer(double move, double steer) { + m_drive.arcadeDrive(move, steer, false); + } + + public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { + leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); + rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); + } + + private boolean inDeadZone(double input) { + boolean inDeadZone; + if (Math.abs(input) < STICK_DEADBAND) { + inDeadZone = true; + } else { + inDeadZone = false; + } + return inDeadZone; + } + + public double adjustForSensitivity(double scale, double trim, + double steer, int nonLinearFactor, double wheelNonLinearity) { + if (inDeadZone(steer)) + return 0; + + steer += trim; + steer *= scale; + steer = limitValue(steer); + + int iterations = Math.abs(nonLinearFactor); + for (int i = 0; i < iterations; i++) { + if (nonLinearFactor > 0) { + steer = nonlinearStickCalcPositive(steer, wheelNonLinearity); + } else { + steer = nonlinearStickCalcNegative(steer, wheelNonLinearity); + } + } + return steer; + } + + private double limitValue(double value) { + if (value > 1.0) { + value = 1.0; + } else if (value < -1.0) { + value = -1.0; + } + return value; + } + + private double nonlinearStickCalcPositive(double steer, + double steerNonLinearity) { + return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer) + / Math.sin(Math.PI / 2.0 * steerNonLinearity); + } + + private double nonlinearStickCalcNegative(double steer, + double steerNonLinearity) { + return Math.asin(steerNonLinearity * steer) + / Math.asin(steerNonLinearity); + } + + //public void setShiftState(SpeedShiftState state) { + // if(state == SpeedShiftState.HI) { + // speedShift.set(true); + // } + // else if(state == SpeedShiftState.LO) { + // speedShift.set(false); + // } + //} + + public synchronized boolean isFinished() { + return isFinished; + } + + public synchronized void setFinished(boolean isFinished) { + this.isFinished = isFinished; + } + + @Override + public void setPeriodMs(long periodMs) { + //mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers); + mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers); + mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); + pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); + mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers); + mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers); + adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers); + + this.periodMs = periodMs; + } + + public double getPeriodMs() { + return periodMs; + } + + public boolean isRed() { + return isRed; + } + + public void setIsRed(boolean status) { + isRed = status; + } + + public static double rotationsToInches(double rotations) { + return rotations * (Constants.kDriveWheelDiameterInches * Math.PI); + } + + public static double rpmToInchesPerSecond(double rpm) { + return rotationsToInches(rpm) / 60; + } + + public static double inchesToRotations(double inches) { + return inches / (Constants.kDriveWheelDiameterInches * Math.PI); + } + + public static double inchesPerSecondToRpm(double inches_per_second) { + return inchesToRotations(inches_per_second) * 60; + } + + public double getLeftPositionWorld() { + return leftDrive1.getPositionWorld(); + } + + public double getRightPositionWorld() { + return rightDrive1.getPositionWorld(); + } + + public void updateStatus(Robot.OperationMode operationMode) { + if (operationMode == Robot.OperationMode.TEST) { + try { + SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); + SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld()); + SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld()); + SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12); + SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12); + //SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID)); + //SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID)); +// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID)); + //SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID)); + //SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID)); +// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID)); + //SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn()); + //SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn()); + SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); + //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); + SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg()); + MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); + double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; + SmartDashboard.putNumber("Gyro Delta", delta); + //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); + SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); + SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); + SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); + SmartDashboard.putNumber("Steer Output", m_steerOutput); + SmartDashboard.putNumber("Move Output", m_moveOutput); + SmartDashboard.putNumber("Steer Input", m_steerInput); + SmartDashboard.putNumber("Move Input", m_moveInput); + } + catch (Exception e) { + System.err.println("Drivetrain update status error"); + } + } + else { + + } + } + +} diff --git a/src/buttons/XBoxTriggerButton.java b/src/buttons/XBoxTriggerButton.java index bbb98cb..7abfe7c 100644 --- a/src/buttons/XBoxTriggerButton.java +++ b/src/buttons/XBoxTriggerButton.java @@ -4,10 +4,6 @@ import org.usfirst.frc4388.controller.XboxController; import edu.wpi.first.wpilibj.buttons.Button; -/** - * - * @author bselle - */ public class XBoxTriggerButton extends Button { public static final int RIGHT_TRIGGER = 0; diff --git a/src/org/usfirst/frc4388/controller/XboxController.java b/src/org/usfirst/frc4388/controller/XboxController.java index b7b8c49..040282a 100644 --- a/src/org/usfirst/frc4388/controller/XboxController.java +++ b/src/org/usfirst/frc4388/controller/XboxController.java @@ -1,12 +1,8 @@ -/* - * To change this template, choose Tools | Templates - * and open the template in the editor. - */ + package org.usfirst.frc4388.controller; import edu.wpi.first.wpilibj.Joystick; -//TODO Code support for Start/Back/XBox(?) buttons. /** * This is a wrapper for the WPILib Joystick class that represents an XBox * controller. diff --git a/src/org/usfirst/frc4388/robot/Constants.java b/src/org/usfirst/frc4388/robot/Constants.java index bbc4054..96a6f89 100644 --- a/src/org/usfirst/frc4388/robot/Constants.java +++ b/src/org/usfirst/frc4388/robot/Constants.java @@ -1,16 +1,14 @@ package org.usfirst.frc4388.robot; -//import org.usfirst.frc4388.utility.ConstantsBase; /** * A list of constants used by the rest of the robot code. This include physics * constants as well as constants determined through calibrations. */ -//public class Constants extends ConstantsBase { + public class Constants { - // Wheels - //public static double kDriveWheelDiameterInches = 5.9; + public static double kDriveWheelDiameterInches = 6.04; public static double kTrackLengthInches = 10; public static double kTrackWidthInches = 26.5; @@ -33,7 +31,6 @@ public class Constants { // Elevator public static int kElevatorEncoderTickesPerRev = 256; public static double kElevatorInchesOfTravelPerRev = 3.75; -// public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev; public static double kElevatorEncoderTicksPerInch = 126.36; public static double kElevatorBasicPercentOutputUp = -0.85; public static double kElevatorBasicPercentOutputDown =.7; @@ -72,12 +69,4 @@ public class Constants { public static double kPathFollowingMaxVel = 120.0; // inches/sec public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2 -// @Override -// public String getFileLocation() { -// return "~/constants.txt"; -// } - -// static { -// new Constants().loadFromFile(); -// } } diff --git a/src/org/usfirst/frc4388/robot/OI.java b/src/org/usfirst/frc4388/robot/OI.java index 92c20bb..c7ae75e 100644 --- a/src/org/usfirst/frc4388/robot/OI.java +++ b/src/org/usfirst/frc4388/robot/OI.java @@ -3,8 +3,6 @@ package org.usfirst.frc4388.robot; -// -//import org.usfirst.frc4388.buttons.XBoxDPadTriggerButton; import buttons.XBoxTriggerButton; import org.usfirst.frc4388.controller.IHandController; import org.usfirst.frc4388.controller.XboxController; @@ -44,39 +42,6 @@ public class OI m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID); m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); - //ELevator PID buttons - - //JoystickButton elevatorMaxScaleHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON); - //elevatorMaxScaleHeight.whenPressed(new PositionPIDMaxScaleHeight(50)); - - //JoystickButton elevatorLowScaleHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); - //elevatorLowScaleHeight.whenPressed(new PositionPIDLowScaleHeight(50)); - - //JoystickButton elevatorSwitchHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - //elevatorSwitchHeight.whenPressed(new PositionPIDLowScaleHeight(50)); - - - /* - JoystickButton elevatorLowestHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); - elevatorLowestHeight.whenPressed(new PositionPIDLowScaleHeight(50)); - */ - - - - //JoystickButton shiftDrivetrain = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - //shiftDrivetrain.whenPressed(new DriveSpeedShift(Drive.SpeedShiftState.HI)); - //shiftDrivetrain.whenReleased(new DriveSpeedShift(Drive.SpeedShiftState.LO)); - - //JoystickButton longShot = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); - //longShot.whenPressed(new ShootOn(ShotState.FAR, ShooterFeed.SHOOTER_FEED_SHOOT_FAR_SPEED, false)); - //longShot.whenReleased(new ShootOff()); - - //XBoxTriggerButton shortShot = new XBoxTriggerButton(m_driverXbox, XBoxTriggerButton.RIGHT_TRIGGER); - //shortShot.whenPressed(new ShootOn(ShotState.CLOSE, ShooterFeed.SHOOTER_FEED_SHOOT_CLOSE_SPEED, false)); - //shortShot.whenReleased(new ShootOff()); - - - XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); CarriageIntake.whenReleased(new IntakeSetSpeed(0.0)); @@ -85,21 +50,6 @@ public class OI CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); CarriageEject.whenReleased(new IntakeSetSpeed(0.0)); - - - //JoystickButton gearIntakeDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.X_BUTTON); - //gearIntakeDown.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_INTAKE)); - //gearIntakeDown.whenReleased(new IntakeSetPosition(IntakePosition.GEAR_PRESENT)); - - //JoystickButton gearIntakeRetract = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON); - //gearIntakeRetract.whenPressed(new IntakeSetPosition(IntakePosition.RETRACT)); - - //JoystickButton gearIntakePresent = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.B_BUTTON); - //gearIntakePresent.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT)); - - //JoystickButton gearIntakeDeploy = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.A_BUTTON); - //gearIntakeDeploy.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY)); - JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); climbUp.whenPressed(new InitiateClimber(true)); climbUp.whenReleased(new InitiateClimber(false)); @@ -113,349 +63,16 @@ public class OI shiftDown.whenPressed(new LEDIndicators(false)); - //XBoxDPadTriggerButton climberReverse = new XBoxDPadTriggerButton(m_driverXbox, XBoxDPadTriggerButton.DOWN); - //climberReverse.whenPressed(new ClimberSetSpeed(0.48, 0.0)); - //climberReverse.whenReleased(new ClimberSetSpeed(0.0, 0.0)); - - //JoystickButton toggleShooter = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.START_BUTTON); - //toggleShooter.whenPressed(new ShooterSetToggle(Shooter.SHOOTER_STAGE1_RPM_CLOSE, Shooter.SHOOTER_STAGE2_RPM_CLOSE)); + //Operator Xbox JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); openIntake.whenPressed(new IntakePosition(true)); JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); - - //XBoxTriggerButton shortShotOperator = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER); - //shortShotOperator.whenPressed(new ShootOn(ShotState.CLOSE, ShooterFeed.SHOOTER_FEED_SHOOT_CLOSE_SPEED, true)); - //shortShotOperator.whenReleased(new ShootOff()); - - //XBoxTriggerButton intakeGearOperator = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); - //intakeGearOperator.whenPressed(new GearIntakeRollerSetSpeed(GearIntake.GEAR_INTAKE_LOAD_SPEED)); - //intakeGearOperator.whenReleased(new GearIntakeRollerSetSpeed(0.0)); - - //JoystickButton gearIntakeEjectOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - //gearIntakeEjectOperator.whenPressed(new GearIntakeRollerSetSpeed(GearIntake.GEAR_INTAKE_EJECT_SPEED)); - //gearIntakeEjectOperator.whenReleased(new GearIntakeRollerSetSpeed(0.0)); - - //JoystickButton gearIntakeDownOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); - //gearIntakeDownOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_INTAKE)); - //gearIntakeDownOperator.whenReleased(new IntakeSetPosition(IntakePosition.GEAR_PRESENT)); - - //JoystickButton gearIntakeRetractOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON); - //gearIntakeRetractOperator.whenPressed(new IntakeSetPosition(IntakePosition.RETRACT)); - - //JoystickButton gearIntakePresentOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - //gearIntakePresentOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT)); - - //JoystickButton gearIntakeDeployOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); - //gearIntakeDeployOperator.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY)); - - //XBoxDPadTriggerButton climberForwardOperator = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.UP); - //climberForwardOperator.whenPressed(new ClimberSetSpeed(ShooterFeed.CLIMB_SPEED, 0.0)); - //climberForwardOperator.whenReleased(new ClimberSetSpeed(0.0, 0.0)); - - //XBoxDPadTriggerButton climberReverseOperator = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.DOWN); - //climberReverseOperator.whenPressed(new ClimberSetSpeed(0.48, 0.0)); - //climberReverseOperator.whenReleased(new ClimberSetSpeed(0.0, 0.0)); - - //XBoxDPadTriggerButton hopperOpen = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.LEFT); - //hopperOpen.whenPressed(new ShooterSetHopperPosition(HopperState.OPEN)); - - //XBoxDPadTriggerButton hopperClose = new XBoxDPadTriggerButton(m_operatorXbox, XBoxDPadTriggerButton.RIGHT); - //hopperClose.whenPressed(new ShooterSetHopperPosition(HopperState.CLOSE)); - - //JoystickButton toggleShooterOperator = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON); - //toggleShooterOperator.whenPressed(new ShooterSetToggle(Shooter.SHOOTER_STAGE1_RPM_CLOSE, Shooter.SHOOTER_STAGE2_RPM_CLOSE)); - - // Gear sensor switch -// GearSensorAnalogSwitch gearSwitch = new GearSensorAnalogSwitch(); -// gearSwitch.whenPressed(new IntakeSetPosition(IntakePosition.GEAR_PRESENT)); - - // SmartDashboard -// Button driveMP = new InternalButton(); -// driveMP.whenPressed(new DriveStraightMP(40, 20, true, false, 0)); -// SmartDashboard.putData("Drive Straight", driveMP); - - - //SmartDashboard.putData("Drive Straight 30in", new DriveStraightMP(30, 20, true, true, 0)); - //SmartDashboard.putData("Drive Straight 60in", new DriveStraightMP(60, 20, true, true, 0)); - - //SmartDashboard.putData("Basic Straight 60in", new DriveStraightBasic(60, 30, true, true, 0)); - //SmartDashboard.putData("Basic Straight -60in", new DriveStraightBasic(-60, 30, true, true, 0)); - //SmartDashboard.putData("Turn abs 90 TANK", new DriveTurnBasic(true, 90, 90, MPSoftwareTurnType.TANK)); - // SmartDashboard.putData("Turn rel -90 RIGHT ONLY", new DriveTurnBasic(false, -90, 90, MPSoftwareTurnType.RIGHT_SIDE_ONLY)); - //SmartDashboard.putData("RUN INTAKE", new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - // SmartDashboard.putData("Stop INTAKE", new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - //SmartDashboard.putData("move intake", new IntakePosition(true)); - //SmartDashboard.putData("move intake2", new IntakePosition(false)); - //SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED)); - - - //SmartDashboard.putData("move elevator", new ElevatorBasic(20)); - - ///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED)); - ///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED)); - - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); // forward , back to right - //SmartDashboard.putData("MP Move", new DriveStraightMP(-50, 30, true, true, 0)); // forward, back to the right - //SmartDashboard.putData("MP Move", new DriveStraightMP(-40, 30, true, true, 0)); //same af -50 - //SmartDashboard.putData("MP Move", new DriveStraightMP(-30, 30, true, true, 0));// same as above shorter - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 20, true, true, 0));// could not break ininita - //SmartDashboard.putData("MP Move", new DriveStraightMP(100, 20, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - //SmartDashboard.putData("MP Move", new DriveStraightMP(-60, 30, true, true, 0)); - - - - - - - - - - //List waypoints = new ArrayList<>(); - //waypoints.add(new Waypoint(new Translation2d(0, 0), 35.0)); - //waypoints.add(new Waypoint(new Translation2d(-35, 0), 35.0)); - //Path.addCircleArc(waypoints, -30.0, 45.0, 10, "hopperSensorOn"); - //waypoints.add(new Waypoint(new Translation2d(-85, -30), 35.0)); - -// List waypoints = new ArrayList<>(); -// waypoints.add(new Waypoint(new Translation2d(-96, 0), 50.0)); -// waypoints.add(new Waypoint(new Translation2d(0, 0), 35.0)); -// waypoints.add(new Waypoint(new Translation2d(-96, 0), 50.0)); -// waypoints.add(new Waypoint(new Translation2d(-61, 0), 50.0)); -// waypoints.add(new Waypoint(new Translation2d(-84.93, -10.16), 50.0)); - -// waypoints.add(new Waypoint(new Translation2d(-41, 0), 35.0)); -// waypoints.add(new Waypoint(new Translation2d(-65, -23), 35.0)); -// waypoints.add(new Waypoint(new Translation2d(-70, -39), 35.0)); - -// waypoints.add(new Waypoint(new Translation2d(-29, 0), 40.0)); -// Path.addCircleArc(waypoints, -30.0, 45.0, 10, null); -// waypoints.add(new Waypoint(new Translation2d(-68.6, -26.5), 40.0)); - - //Button driveAP = new InternalButton(); - //driveAP.whenPressed(new DrivePathAdaptivePursuit(new Path(waypoints), true)); - //SmartDashboard.putData("Drive Adaptive Pursuit", driveAP); - - //Button driveMM = new InternalButton(); - //driveMM.whenPressed(new DriveStraightMM(10, 450, 450, true, false, 0)); - //SmartDashboard.putData("Drive StraightMM", driveMM); - - //Button turnRelativePID = new InternalButton(); - //turnRelativePID.whenPressed(new DriveRelativeTurnPID(-10, MPSoftwareTurnType.RIGHT_SIDE_ONLY)); - //SmartDashboard.putData("Turn Relative 10 PID", turnRelativePID); - - //Button turnRelativePID7 = new InternalButton(); - //turnRelativePID7.whenPressed(new DriveRelativeTurnPID(7, MPSoftwareTurnType.LEFT_SIDE_ONLY)); - //SmartDashboard.putData("Turn Relative 7 PID", turnRelativePID7); - - //Button turnRelativeMP = new InternalButton(); - //turnRelativeMP.whenPressed(new DriveRelativeTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY )); - //SmartDashboard.putData("Turn Relative 10", turnRelativeMP); - /* - Button turnAbsoluteMP = new InternalButton(); - turnAbsoluteMP.whenPressed(new DriveAbsoluteTurnMP(45, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY)); - SmartDashboard.putData("Turn Absolute 45", turnAbsoluteMP); - - Button turnRelativePID = new InternalButton(); - turnRelativePID.whenPressed(new DriveRelativeTurnPID(45, MPSoftwareTurnType.TANK)); - SmartDashboard.putData("Turn Relative 45", turnRelativePID); -*/ -// Button gyroReset = new InternalButton(); -// gyroReset.whenPressed(new DriveGyroReset()); -// SmartDashboard.putData("Gyro Reset", gyroReset); - SmartDashboard.putData("Reset Gyro", new DriveGyroReset()); - //SmartDashboard.putData("reset Encoders", new drive.resetEncoders()); - SmartDashboard.putData("clampy boi", new IntakePosition(true)); - - // Camera - //Button cameraUpdateDashboard = new InternalButton(); - //cameraUpdateDashboard.whenPressed(new CameraUpdateDashboard()); - //SmartDashboard.putData("Camera Update", cameraUpdateDashboard); - - //Button cameraSaveImage = new InternalButton(); - //cameraSaveImage.whenPressed(new CameraSaveImage()); - //SmartDashboard.putData("Camera Save", cameraSaveImage); - - //Button cameraTurnToBestTarget = new InternalButton(); - //cameraTurnToBestTarget.whenPressed(new CameraTurnToBestTarget()); - //SmartDashboard.putData("Camera Turn To Best", cameraTurnToBestTarget); - - //Button cameraUpdateBestTarget = new InternalButton(); - //cameraUpdateBestTarget.whenPressed(new CameraUpdateBestTarget()); - //SmartDashboard.putData("Camera Update Best", cameraUpdateBestTarget); - - //Button incrementCameraOffsetPos = new InternalButton(); - //incrementCameraOffsetPos.whenPressed(new CameraIncrementOffset(0.5)); - //SmartDashboard.putData("Camera Offset Pos", incrementCameraOffsetPos); - - //Button incrementCameraOffsetNeg = new InternalButton(); - //incrementCameraOffsetNeg.whenPressed(new CameraIncrementOffset(-0.5)); - //SmartDashboard.putData("Camera Offset Neg", incrementCameraOffsetNeg); - -// Button cameraReadImage = new InternalButton(); -// cameraReadImage.whenPressed(new CameraReadAndProcessImage()); -// SmartDashboard.putData("Camera Read", cameraReadImage); -// -// Button cameraReadImageTurnToBestTarget = new InternalButton(); -// cameraReadImageTurnToBestTarget.whenPressed(new CameraReadImageTurnToBestTarget()); -// SmartDashboard.putData("Camera Read Turn", cameraReadImageTurnToBestTarget); -// -// Button magicCarpetOn05 = new InternalButton(); -// magicCarpetOn05.whenPressed(new ZarkerFeedSetSpeed(0.5)); -// SmartDashboard.putData("Magic Carpet 0.5", magicCarpetOn05); - // -// Button magicCarpetOff = new InternalButton(); -// magicCarpetOff.whenPressed(new ZarkerFeedSetSpeed(0.0)); -// SmartDashboard.putData("Magic Carpet Off", magicCarpetOff); - // -// Button shooterFeedOn10 = new InternalButton(); -// shooterFeedOn10.whenPressed(new ShooterFeedSetSpeed(1.0)); -// SmartDashboard.putData("Shooter Feed 1.0", shooterFeedOn10); - // -// Button shooterFeedOn05 = new InternalButton(); -// shooterFeedOn05.whenPressed(new ShooterFeedSetSpeed(0.5)); -// SmartDashboard.putData("Shooter Feed 0.5", shooterFeedOn05); - // -// Button shooterFeedOff = new InternalButton(); -// shooterFeedOff.whenPressed(new ShooterFeedSetSpeed(0.0)); -// SmartDashboard.putData("Shooter Feed Off", shooterFeedOff); - // -// Button shooterStage2RPMOnDash = new InternalButton(); -// shooterStage2RPMOnDash.whenPressed(new ShooterStage2SetRpmDashboard()); -// SmartDashboard.putData("Shooter Stage 2 Dash RPM", shooterStage2RPMOnDash); - // -// Button shooterStage2On10 = new InternalButton(); -// shooterStage2On10.whenPressed(new ShooterStage2SetSpeed(1.0)); -// SmartDashboard.putData("Shooter Stage 2 - 1.0", shooterStage2On10); - // -// Button shooterStage2On09 = new InternalButton(); -// shooterStage2On09.whenPressed(new ShooterStage2SetSpeed(0.9)); -// SmartDashboard.putData("Shooter Stage 2 - 0.9", shooterStage2On09); - // -// Button shooterStage2On08 = new InternalButton(); -// shooterStage2On08.whenPressed(new ShooterStage2SetSpeed(0.8)); -// SmartDashboard.putData("Shooter Stage 2 - 0.8", shooterStage2On08); - // -// Button shooterStage2On07 = new InternalButton(); -// shooterStage2On07.whenPressed(new ShooterStage2SetSpeed(0.7)); -// SmartDashboard.putData("Shooter Stage 2 - 0.7", shooterStage2On07); - // -// Button shooterStage2On06 = new InternalButton(); -// shooterStage2On06.whenPressed(new ShooterStage2SetSpeed(0.6)); -// SmartDashboard.putData("Shooter Stage 2 - 0.6", shooterStage2On06); - // -// Button shooterStage2On05 = new InternalButton(); -// shooterStage2On05.whenPressed(new ShooterStage2SetSpeed(0.5)); -// SmartDashboard.putData("Shooter Stage 2 - 0.5", shooterStage2On05); - // -// Button shooterStage2Off = new InternalButton(); -// shooterStage2Off.whenPressed(new ShooterStage2SetSpeed(0.0)); -// SmartDashboard.putData("Shooter Stage 2 - Off", shooterStage2Off); -// -// Button shooterStage1RPMOnDash = new InternalButton(); -// shooterStage1RPMOnDash.whenPressed(new ShooterStage1SetRpmDashboard()); -// SmartDashboard.putData("Shooter Stage 1 Dash RPM", shooterStage1RPMOnDash); - // -// Button shooterStage1On05 = new InternalButton(); -// shooterStage1On05.whenPressed(new ShooterStage1SetSpeed(0.5)); -// SmartDashboard.putData("Shooter Stage 1 - 0.5", shooterStage1On05); -// -// Button shooterStage1Off = new InternalButton(); -// shooterStage1Off.whenPressed(new ShooterStage1SetSpeed(0.0)); -// SmartDashboard.putData("Shooter Stage 1 - Off", shooterStage1Off); -// -// Button shooterBothRPMOnDash = new InternalButton(); -// shooterBothRPMOnDash.whenPressed(new ShooterSetRpmDashboard()); -// SmartDashboard.putData("Shooter Both Dash RPM", shooterBothRPMOnDash); - // -// Button shooterBothVBusOnDash = new InternalButton(); -// shooterBothVBusOnDash.whenPressed(new ShooterSetSpeedDashboard()); -// SmartDashboard.putData("Shooter Both Dash VBus", shooterBothVBusOnDash); - // -// Button allButLiftOff = new InternalButton(); -// allButLiftOff.whenPressed(new ShooterAllOff()); -// SmartDashboard.putData("Shooter All Off", allButLiftOff); -// -// Button allButLiftOn = new InternalButton(); -// allButLiftOn.whenPressed(new ShooterAllOn()); -// SmartDashboard.putData("Shooter All On", allButLiftOn); - // -// Button hopperShake = new InternalButton(); -// hopperShake.whenPressed(new ShooterSetHopperShake(2, 10, 1)); -// SmartDashboard.putData("Hopper Shake Button", hopperShake); - // -// Button intakeOn10 = new InternalButton(); -// intakeOn10.whenPressed(new GearIntakeRollerSetSpeed(1.0)); -// SmartDashboard.putData("Intake 1.0", intakeOn10); - // -// Button intakeOn08 = new InternalButton(); -// intakeOn08.whenPressed(new GearIntakeRollerSetSpeed(0.8)); -// SmartDashboard.putData("Intake 0.8", intakeOn08); - // -// Button intakeOn05 = new InternalButton(); -// intakeOn05.whenPressed(new GearIntakeRollerSetSpeed(0.5)); -// SmartDashboard.putData("Intake 0.5", intakeOn05); - // -// Button intakeOn04 = new InternalButton(); -// intakeOn04.whenPressed(new GearIntakeRollerSetSpeed(0.4)); -// SmartDashboard.putData("Intake 0.4", intakeOn04); - // -// Button intakeOff = new InternalButton(); -// intakeOff.whenPressed(new GearIntakeRollerSetSpeed(0.0)); -// SmartDashboard.putData("Intake Off", intakeOff); - // -// Button shooterShotPositionClose = new InternalButton(); -// shooterShotPositionClose.whenPressed(new ShooterSetShotPosition(Shooter.ShotState.CLOSE)); -// SmartDashboard.putData("Shooter Close Shot", shooterShotPositionClose); -// -// Button shooterShotPositionFar = new InternalButton(); -// shooterShotPositionFar.whenPressed(new ShooterSetShotPosition(Shooter.ShotState.FAR)); -// SmartDashboard.putData("Shooter Far Shot", shooterShotPositionFar); -// -// Button climberOn08 = new InternalButton(); -// climberOn08.whenPressed(new ClimberSetSpeed(0.8, 0.0)); -// climberOn08.whenReleased(new ClimberSetSpeed(0.0, 0.0)); -// SmartDashboard.putData("Climber 0.8", climberOn08); - // -// Button climberSetMaxAmps = new InternalButton(); -// climberSetMaxAmps.whenPressed(new ClimberSetMaxAmps(0.8, 20)); -// SmartDashboard.putData("Climber Max Amps", climberSetMaxAmps); -// - //jaci.pathfinder.Waypoint[] points = new jaci.pathfinder.Waypoint[] { - // new jaci.pathfinder.Waypoint(0, 0, 0), - // new jaci.pathfinder.Waypoint(-90, -16, Pathfinder.d2r(-45)) - //}; - - //PathGenerator path = new PathGenerator(points, 0.01, 160, 200.0, 700.0); - - //Button pathTest = new InternalButton(); - //pathTest.whenPressed(new DrivePathMP(path)); - //SmartDashboard.putData("Path Test", pathTest); - -// Button ledsOn = new InternalButton(); -// ledsOn.whenPressed(new LEDLightsSet(true)); -// SmartDashboard.putData("LEDs On", ledsOn); -// -// Button ledsOff = new InternalButton(); -// ledsOff.whenPressed(new LEDLightsSet(false)); -// SmartDashboard.putData("LEDs Off", ledsOff); -// -// Button intakeGearRoller = new InternalButton(); -// intakeGearRoller.whenPressed(new GearIntakeRollerSetSpeed(0.3)); -// SmartDashboard.putData("Gear Intake Clean Speed", intakeGearRoller); - // -// Button intakeGearStopRoller = new InternalButton(); -// intakeGearStopRoller.whenPressed(new GearIntakeRollerSetSpeed(0.0)); -// SmartDashboard.putData("Gear Intake Clean Off Speed", intakeGearStopRoller); + + SmartDashboard.putData("PRE GAME!!!!", new PreGame()); + } catch (Exception e) { System.err.println("An error occurred in the OI constructor"); } diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index 9efc772..f1ecdb3 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -1,18 +1,6 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// Java from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - package org.usfirst.frc4388.robot; -//import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.DriverStation; @@ -35,13 +23,7 @@ import org.usfirst.frc4388.robot.subsystems.Drive; import org.usfirst.frc4388.robot.subsystems.Carriage; import org.usfirst.frc4388.robot.subsystems.LED; import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;; -/** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the TimedRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.properties file in - * the project. - */ + public class Robot extends IterativeRobot { @@ -49,7 +31,7 @@ public class Robot extends IterativeRobot public static final Drive drive = new Drive(); - //public static final Elevator elevator = new Elevator(); + public static final Carriage carriage = new Carriage(); public static final Elevator elevator = new Elevator(); @@ -58,15 +40,8 @@ public class Robot extends IterativeRobot public static final Climber climber = new Climber(); public static final Pnumatics pnumatics = new Pnumatics(); - //public static FlashyBlinky flashyBlinky; - //public static Sensors sensors; - public static final long periodMS = 10; public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS); - //public static final PowerDistributionPanel pdp = new PowerDistributionPanel(); - - //public static final String ElevatorAuton = null; - public static enum OperationMode { TEST, COMPETITION }; public static OperationMode operationMode = OperationMode.TEST; @@ -82,33 +57,20 @@ public class Robot extends IterativeRobot private Command LRautonomousCommand; private Command LLautonomousCommand; - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ public void robotInit() { //Printing game data to riolog String gameData = DriverStation.getInstance().getGameSpecificMessage(); System.out.println(gameData); CameraServer.getInstance().startAutomaticCapture(); - CameraServer.getInstance().putVideo("res", 640, 480); + CameraServer.getInstance().putVideo("res", 300, 220); try { - //drive = new Drive(); - //controlLoop = new ControlLooper("Main control loop", periodMS); oi = OI.getInstance(); - //camera.initialize(); controlLoop.addLoopable(drive); controlLoop.addLoopable(elevator); - -// Waypoint[] points = new Waypoint[] { -// new Waypoint(0, 0, 0), -// new Waypoint(-95, -9, Pathfinder.d2r(-27)) -// }; -// -// PathGenerator boilerPath = new PathGenerator(points, 0.01, 120, 200.0, 700.0); + operationModeChooser = new SendableChooser(); operationModeChooser.addDefault("Test", OperationMode.TEST); @@ -200,11 +162,6 @@ public class Robot extends IterativeRobot } } - /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. - */ public void disabledInit(){ } @@ -214,23 +171,12 @@ public class Robot extends IterativeRobot updateStatus(); } - /** - * This autonomous (along with the chooser code above) shows how to select between different autonomous modes - * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW - * Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box - * below the Gyro - * - * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example) - * or additional comparisons to the switch structure below with additional strings & commands. - */ public void autonomousInit() { updateChoosers(); - // Schedule the autonomous command (example) controlLoop.start(); drive.endGyroCalibration(); drive.resetEncoders(); - //elevator.resetElevatorEncoder(); drive.resetGyro(); drive.setIsRed(getAlliance().equals(Alliance.Red)); @@ -265,46 +211,30 @@ public class Robot extends IterativeRobot public void autonomousPeriodic() { Scheduler.getInstance().run(); - //Robot.elevator.setControlMode(DriveControlMode.RAW); updateStatus(); } public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. if (RRautonomousCommand != null) RRautonomousCommand.cancel(); if (RLautonomousCommand != null) RLautonomousCommand.cancel(); if (LRautonomousCommand != null) LRautonomousCommand.cancel(); if (LLautonomousCommand != null) LLautonomousCommand.cancel(); - drive.setToBrakeOnNeutral(false); // coast to avoid tipping when driver stops suddenly - //MotionProfileCache.getInstance().release(); + drive.setToBrakeOnNeutral(false); updateChoosers(); controlLoop.start(); drive.resetEncoders(); drive.endGyroCalibration(); - //shooter.setStage1Speed(0); - //shooter.setStage2Speed(0); - //shooterFeed.setSpeed(0); - //zarkerFeed.setSpeed(0); - //shooter.setHopperPosition(HopperState.CLOSE); - //Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + updateStatus(); } - /** - * This function is called periodically during operator control - */ + public void teleopPeriodic() { Scheduler.getInstance().run(); updateStatus(); } - /** - * This function is called periodically during test mode - */ public void testPeriodic() { LiveWindow.run(); updateStatus(); @@ -325,11 +255,7 @@ public class Robot extends IterativeRobot public void updateStatus() { drive.updateStatus(operationMode); elevator.updateStatus(operationMode); - //carriage.updateStatus(operationMode); - //shooter.updateStatus(operationMode); - //shooterFeed.updateStatus(operationMode); - //zarkerFeed.updateStatus(operationMode); - //camera.updateStatus(operationMode); + } } diff --git a/src/org/usfirst/frc4388/robot/RobotMap.java b/src/org/usfirst/frc4388/robot/RobotMap.java index 08f5e02..64e015f 100644 --- a/src/org/usfirst/frc4388/robot/RobotMap.java +++ b/src/org/usfirst/frc4388/robot/RobotMap.java @@ -1,28 +1,23 @@ package org.usfirst.frc4388.robot; -/** - * The RobotMap is a mapping from the ports sensors and actuators are wired into - * to a variable name. This provides flexibility changing wiring, makes checking - * the wiring easier and significantly reduces the number of magic numbers - * floating around. - */ + public class RobotMap { // USB Port IDs public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; - - //Controller IDs + // MOTORS public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; -// public static final int DRIVETRAIN_LEFT_MOTOR3_CAN_ID = 2; + public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; -// public static final int DRIVETRAIN_RIGHT_MOTOR3_CAN_ID = 13; + + //carriage motors public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; @@ -37,12 +32,5 @@ public class RobotMap { public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; public static final int OPEN_INTAKE_PCM_ID = 4; public static final int CLOSE_INTAKE_PCM_ID = 5; - //public static final int HOPPER_POSITION_PCM_ID = 5; - - // DIO - //public static final int HOPPER_SENSOR_RED_DIO_ID = 4; - //public static final int HOPPER_SENSOR_BLUE_DIO_ID = 5; - // Analog - //public static final int GEAR_SENSOR_ANALOG_ID = 0; } diff --git a/src/org/usfirst/frc4388/robot/commands/PreGame.java b/src/org/usfirst/frc4388/robot/commands/PreGame.java new file mode 100644 index 0000000..8a82320 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/PreGame.java @@ -0,0 +1,26 @@ +package org.usfirst.frc4388.robot.commands; + + +import org.usfirst.frc4388.robot.commands.DriveGyroReset; +import org.usfirst.frc4388.robot.commands.DriveSpeedShift; +import org.usfirst.frc4388.robot.commands.DriveStraightBasic; + +import org.usfirst.frc4388.robot.commands.IntakePosition; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * + */ +public class PreGame extends CommandGroup { + + public PreGame() { + + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(-10, 50, true, true, 0)); + + + + } +} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java index 9a86b8b..cb8935c 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java @@ -28,8 +28,8 @@ public class Cube2Right extends CommandGroup { addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-300, 70, true, true, 0)); - addSequential(new DriveStraightBasic(1, 30, true, true, 0)); + addSequential(new DriveStraightBasic(-259, 70, true, true, 0)); + addSequential(new ElevatorBasic(70)); addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); addSequential(new WaitCommand(.5)); @@ -39,24 +39,22 @@ public class Cube2Right extends CommandGroup { addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-10, 30, true, true, 0)); + addSequential(new DriveStraightBasic(-16, 40, true, true, 0)); addParallel(new ElevatorBasic(4)); ////////////////////////////////////////////////////////////////////////// - addSequential(new DriveTurnBasic(true, -69, 100, MPSoftwareTurnType.TANK)); + addSequential(new DriveTurnBasic(true, -65, 100, MPSoftwareTurnType.TANK)); ////////////////////////////////////////////////////////////////////////// addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED)); ///////////////////////////////////////////////////////////// - addSequential(new DriveStraightBasic(10, 30, true, true, 0)); - addSequential(new DriveStraightBasic(80, 60, true, true, 0)); - addSequential(new DriveStraightBasic(33, 20, true, true, 0)); + addSequential(new DriveStraightBasic(110, 70, true, true, 0)); + addSequential(new DriveStraightBasic(42, 60, true, true, 0)); ///////////////////////////////////////////////////////////// - addSequential(new WaitCommand(.5)); + addSequential(new IntakePosition(true)); - //addSequential(new WaitCommand(.3)); - addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addParallel(new TimeoutBecaseYea()); addSequential(new ElevatorBasic(28)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java index 86a31af..21b2f6c 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -38,9 +38,9 @@ public class RightStartLeftScore extends CommandGroup { addSequential(new DriveStraightBasic(155, 60, true, true, 0)); addSequential(new ElevatorBasic(30)); addSequential(new WaitCommand(.2)); - addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveTurnBasic(true, -85, 150, MPSoftwareTurnType.TANK)); addParallel(new TimeoutBecaseYea()); - addSequential(new DriveStraightBasic(10, 60, true, true, 0)); + addSequential(new DriveStraightBasic(15, 60, true, true, 0)); addSequential(new WaitCommand(3)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java index efc4879..d92f317 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -31,10 +31,11 @@ public class ScaleFrom3 extends CommandGroup { addSequential(new DriveStraightBasic(-10, 20, true, true, 0)); - addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); + addSequential(new DriveStraightBasic(-270, 60, true, true, 0)); addSequential(new ElevatorBasic(70)); addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(10, 20, true, true, 0)); //addSequential(new DriveStraightBasic(5, 20, true, true, 0)); addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java b/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java index 0cf2d00..6fc033a 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java @@ -24,7 +24,7 @@ public class TimeoutBecaseYea extends CommandGroup { public TimeoutBecaseYea() { - addSequential(new WaitCommand(1.5)); + addSequential(new WaitCommand(1.3)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); diff --git a/src/org/usfirst/frc4388/robot/subsystems/Drive.java b/src/org/usfirst/frc4388/robot/subsystems/Drive.java index 12d1674..b984af3 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -39,6 +39,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.DoubleSolenoid; //import edu.wpi.first.wpilibj.DigitalInput; //import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; @@ -110,6 +111,7 @@ public class Drive extends Subsystem implements ControlLoopable //private DigitalInput hopperSensorBlue; + private double climbSpeed; private boolean isRed = true; @@ -208,6 +210,7 @@ public class Drive extends Subsystem implements ControlLoopable rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); // rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); //gyroPigeon = new PigeonImu(leftDrive2); gyroNavX = new AHRS(SPI.Port.kMXP); @@ -687,8 +690,15 @@ public class Drive extends Subsystem implements ControlLoopable } public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); + + if (elevatorRight.getSelectedSensorPosition(0) >= 3550) { + leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5); + rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5); + } + else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ { + leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); + rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); + } } private boolean inDeadZone(double input) {