diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 36fea3e..4395adb 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -45,16 +45,6 @@ public class Robot extends IterativeRobot public static OperationMode operationMode = OperationMode.TEST; private SendableChooser operationModeChooser; - private SendableChooser RRautonTaskChooser; - private SendableChooser RLautonTaskChooser; - private SendableChooser LRautonTaskChooser; - private SendableChooser LLautonTaskChooser; - - private Command RRautonomousCommand; - private Command RLautonomousCommand; - private Command LRautonomousCommand; - private Command LLautonomousCommand; - public void robotInit() { //Printing game data to riolog @@ -116,15 +106,17 @@ public class Robot extends IterativeRobot } public void teleopInit() { - if (RRautonomousCommand != null) RRautonomousCommand.cancel(); - if (RLautonomousCommand != null) RLautonomousCommand.cancel(); - if (LRautonomousCommand != null) LRautonomousCommand.cancel(); - if (LLautonomousCommand != null) LLautonomousCommand.cancel(); + System.err.println("Beginning of teleopInit."); + drive.setToBrakeOnNeutral(false); - updateChoosers(); - controlLoop.start(); - drive.resetEncoders(); - drive.endGyroCalibration(); + updateChoosers(); + System.err.println("TeleopInit after UpdateChoosers"); + controlLoop.start(); + System.err.println("TeleopInit after controlLoop"); + //drive.resetEncoders(); + //System.err.println("TeleopInit after resetEncoders"); + drive.endGyroCalibration(); + System.err.println("TeleopInit after endGyroCalibrations"); updateStatus(); } @@ -143,10 +135,6 @@ public class Robot extends IterativeRobot private void updateChoosers() { operationMode = (OperationMode)operationModeChooser.getSelected(); - RRautonomousCommand = (Command)RRautonTaskChooser.getSelected(); - RLautonomousCommand = (Command)RLautonTaskChooser.getSelected(); - LRautonomousCommand = (Command)LRautonTaskChooser.getSelected(); - LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); } public Alliance getAlliance() { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 2c65d1f..ee34bbe 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -38,7 +38,9 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -93,7 +95,7 @@ public class Drive extends Subsystem implements ControlLoopable public static final double MP_MAX_TURN_T2 = 200; // Motor controllers - private ArrayList motorControllers = new ArrayList(); + private ArrayList motorControllers = new ArrayList(); private CANSparkMax leftDrive1; private CANSparkMax leftDrive2; @@ -198,6 +200,8 @@ public class Drive extends Subsystem implements ControlLoopable public Drive() { try { + //System.err.println("Beginning of Drive."); + leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless); //leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); leftDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless); @@ -208,6 +212,9 @@ public class Drive extends Subsystem implements ControlLoopable rightDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless); encoderRight = new CANEncoder(rightDrive1); rightDrive2.follow(rightDrive1); + + //System.err.println("After Constructors."); + //gyroPigeon = new PigeonImu(leftDrive2); gyroNavX = new AHRS(SPI.Port.kMXP); @@ -217,17 +224,17 @@ public class Drive extends Subsystem implements ControlLoopable //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.clearFaults(); + leftDrive1.setInverted(true);//false on comp 2108, false on microbot + //leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark + //leftDrive1.setSafetyEnabled(false); //not needed for spark //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); + leftDrive1.setIdleMode(IdleMode.kBrake); + //leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); // not needed for spark? + //leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); // not needed for spark? + //leftDrive1.configNominalOutputForward(+1.0f, 0); // not needed for spark? + //leftDrive1.configNominalOutputReverse(-1.0f, 0); // not needed for spark? // if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { @@ -235,40 +242,44 @@ public class Drive extends Subsystem implements ControlLoopable // } - leftDrive2.setInverted(false);//false on comp 2108, false on microbot - leftDrive2.setSafetyEnabled(false); - leftDrive2.setNeutralMode(NeutralMode.Brake); - leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); + leftDrive2.setInverted(true);//false on comp 2108, false on microbot + //leftDrive2.setSafetyEnabled(false); + leftDrive2.setIdleMode(IdleMode.kBrake); + //leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); // set above //rightDrive1.clearStickyFaults(); //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //rightDrive1.setNominalClosedLoopVoltage(12.0); - rightDrive1.clearStickyFaults(0); + rightDrive1.clearFaults(); 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); + //rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot + //rightDrive1.setSafetyEnabled(false); + rightDrive1.setIdleMode(IdleMode.kBrake); + //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()); + //rightDrive2.setSafetyEnabled(false); + rightDrive2.setIdleMode(IdleMode.kBrake); + //rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); + //System.err.println("After motor settings."); - + CANPIDController leftDrive1_Controller = new CANPIDController(leftDrive1); + CANPIDController rightDrive1_Controller = new CANPIDController(rightDrive1); - motorControllers.add(leftDrive1); - motorControllers.add(rightDrive1); + motorControllers.add(leftDrive1_Controller); + motorControllers.add(rightDrive1_Controller); + + //System.err.println("After motorControllers."); //m_drive = new RobotDrive(leftDrive1, rightDrive1); //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); @@ -279,8 +290,11 @@ public class Drive extends Subsystem implements ControlLoopable //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify m_drive.setSafetyEnabled(false); + //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); + //System.err.println("End of Drive."); } + catch (Exception e) { System.err.println("An error occurred in the DriveTrain constructor"); } @@ -288,15 +302,15 @@ public class Drive extends Subsystem implements ControlLoopable public void setToBrakeOnNeutral(boolean brakeVsCoast) { if (brakeVsCoast) { - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive2.setNeutralMode(NeutralMode.Brake); + leftDrive1.setIdleMode(IdleMode.kBrake); + leftDrive2.setIdleMode(IdleMode.kBrake); + rightDrive1.setIdleMode(IdleMode.kBrake); + rightDrive2.setIdleMode(IdleMode.kBrake); } else { - leftDrive1.setNeutralMode(NeutralMode.Coast); - leftDrive2.setNeutralMode(NeutralMode.Coast); - rightDrive1.setNeutralMode(NeutralMode.Coast); - rightDrive2.setNeutralMode(NeutralMode.Coast); + leftDrive1.setIdleMode(IdleMode.kCoast); + leftDrive2.setIdleMode(IdleMode.kCoast); + rightDrive1.setIdleMode(IdleMode.kCoast); + rightDrive2.setIdleMode(IdleMode.kCoast); } } @@ -435,8 +449,8 @@ public class Drive extends Subsystem implements ControlLoopable } public void updatePose() { - double left_distance = leftDrive1.getPositionWorld(); - double right_distance = rightDrive1.getPositionWorld(); + double left_distance = 0;//leftDrive1.getPositionWorld(); + double right_distance = 0;//rightDrive1.getPositionWorld(); FIX TODAY Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); lastPose = currentPose; currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); @@ -468,20 +482,20 @@ public class Drive extends Subsystem implements ControlLoopable 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 + leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(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 + leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(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 + leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving } else if (controlMode == DriveControlMode.HOLD) { mpStraightController.setPID(mpHoldPIDParams); @@ -491,10 +505,10 @@ public class Drive extends Subsystem implements ControlLoopable //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); + //leftDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout + leftDrive1.set(0); + //rightDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout + rightDrive1.set(0); } isFinished = false; } @@ -678,14 +692,10 @@ public class Drive extends Subsystem implements ControlLoopable public void rawDriveLeftRight(double leftPercentOutput, double 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); - } + + leftDrive1.set(leftPercentOutput); + rightDrive1.set(rightPercentOutput); + } private boolean inDeadZone(double input) { @@ -758,13 +768,14 @@ public class Drive extends Subsystem implements ControlLoopable @Override public void setPeriodMs(long periodMs) { + /* FIX TODAY 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; } @@ -797,23 +808,23 @@ public class Drive extends Subsystem implements ControlLoopable } public double getLeftPositionWorld() { - return leftDrive1.getPositionWorld(); + return 0;//leftDrive1.getPositionWorld(); FIX TODAY } public double getRightPositionWorld() { - return rightDrive1.getPositionWorld(); + return 0;//rightDrive1.getPositionWorld(); FIX TODAY } 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("Right Pos Ticks", 0);//rightDrive1.getSelectedSensorPosition(0)); FIX TODAY + SmartDashboard.putNumber("Left Pos Ticks", 0);//leftDrive1.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Pos Inches", 0);//rightDrive1.getPositionWorld()); + SmartDashboard.putNumber("Left Pos Inches", 0);//leftDrive1.getPositionWorld()); + SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12); + SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//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));