From 8614a6b683419bfa4793b9a01de86a8fb32157fd Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sat, 2 Mar 2019 16:04:05 -0700 Subject: [PATCH] Update drive tain started convesion, stoped for testing, not ready to use --- .../frc4388/robot/subsystems/Drive.java | 303 +++++++----------- 1 file changed, 117 insertions(+), 186 deletions(-) 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 6f787b4..583c139 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 @@ -1,4 +1,3 @@ - package org.usfirst.frc4388.robot.subsystems; import java.util.ArrayList; @@ -10,9 +9,11 @@ import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; import org.usfirst.frc4388.utility.control.AdaptivePurePursuitController; import org.usfirst.frc4388.utility.BHRMathUtils; -//import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.TalonSRXEncoder; +import org.usfirst.frc4388.utility.TalonSRXFactory; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.control.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; @@ -22,13 +23,13 @@ import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController; 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.control.Path; import org.usfirst.frc4388.utility.math.RigidTransform2d; import org.usfirst.frc4388.utility.math.Rotation2d; import org.usfirst.frc4388.utility.SoftwarePIDController; import org.usfirst.frc4388.utility.math.Translation2d; + import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -37,15 +38,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; 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 com.revrobotics.CANSparkMax; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.wpilibj.DoubleSolenoid; //import edu.wpi.first.wpilibj.DigitalInput; @@ -99,24 +91,14 @@ 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; - private CANSparkMax rightDrive1; - private CANSparkMax rightDrive2; - private CANEncoder left_distance; - private CANEncoder right_distance; + private TalonSRXEncoder leftDrive1; - - - - private CANSparkMax m_motor; - private CANEncoder m_encoder; + private TalonSRXEncoder rightDrive1; private DifferentialDrive m_drive; - //PID encoder and motor //private DigitalInput hopperSensorRed; //private DigitalInput hopperSensorBlue; @@ -170,6 +152,9 @@ public class Drive extends Subsystem implements ControlLoopable 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 @@ -203,96 +188,69 @@ public class Drive extends Subsystem implements ControlLoopable private AHRS gyroNavX; private boolean useGyroLock; private double gyroLockAngleDeg; - private double kPGyro = 0.07; - //private double kPGyro = 0.0625; - + //private double kPGyro = 0.04; + private double kPGyro = 0.0625; private boolean isCalibrating = false; private double gyroOffsetDeg = 0; 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); - //encoderLeft = new CANEncoder(leftDrive1); - - - rightDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless); - rightDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless); - - - //leftDrive1.restoreFactoryDefaults(); + leftDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); - //System.err.println("After Constructors."); - - //gyroPigeon = new PigeonImu(leftDrive2); + rightDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); + + + + gyroNavX = new AHRS(SPI.Port.kMXP); - leftDrive2.follow(leftDrive1); - rightDrive2.follow(rightDrive1); + //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.clearFaults(); - leftDrive1.setInverted(false);//false on practice, true on comp - //leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark - //leftDrive1.setSafetyEnabled(false); //not needed for spark + 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.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? + 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 practice. true on comp - //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.clearFaults(); - rightDrive1.setInverted(false);//false on comp, false on practice - //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); + 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(false);//True on comp 2019, false on practice - //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_Controller); - motorControllers.add(rightDrive1_Controller); - //System.err.println("After motorControllers."); + + motorControllers.add(leftDrive1); + motorControllers.add(rightDrive1); //m_drive = new RobotDrive(leftDrive1, rightDrive1); //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); @@ -303,11 +261,8 @@ 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"); } @@ -315,15 +270,15 @@ public class Drive extends Subsystem implements ControlLoopable public void setToBrakeOnNeutral(boolean brakeVsCoast) { if (brakeVsCoast) { - leftDrive1.setIdleMode(IdleMode.kBrake); - leftDrive2.setIdleMode(IdleMode.kBrake); - rightDrive1.setIdleMode(IdleMode.kBrake); - rightDrive2.setIdleMode(IdleMode.kBrake); + leftDrive1.setNeutralMode(NeutralMode.Brake); + + rightDrive1.setNeutralMode(NeutralMode.Brake); + } else { - leftDrive1.setIdleMode(IdleMode.kCoast); - leftDrive2.setIdleMode(IdleMode.kCoast); - rightDrive1.setIdleMode(IdleMode.kCoast); - rightDrive2.setIdleMode(IdleMode.kCoast); + leftDrive1.setNeutralMode(NeutralMode.Coast); + + rightDrive1.setNeutralMode(NeutralMode.Coast); + } } @@ -384,35 +339,22 @@ public class Drive extends Subsystem implements ControlLoopable // 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) { + + //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); @@ -449,7 +391,7 @@ public class Drive extends Subsystem implements ControlLoopable pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); setControlMode(DriveControlMode.PID_TURN); } -/* + public void setPathMP(PathGenerator path) { mpPathController.setPID(mpPathPIDParams); mpPathController.setMPPathTarget(path); @@ -471,7 +413,7 @@ public class Drive extends Subsystem implements ControlLoopable adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); } -*/ + public void setDriveHold(boolean status) { if (status) { setControlMode(DriveControlMode.HOLD); @@ -482,14 +424,13 @@ public class Drive extends Subsystem implements ControlLoopable } public void updatePose() { - //m_encoder = m_motor.getEncoder(); - left_distance = leftDrive1.getEncoder(); - right_distance = rightDrive1.getEncoder(); + 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; + 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) { @@ -503,46 +444,34 @@ public class Drive extends Subsystem implements ControlLoopable * * @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(0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving + 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(0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving + 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(0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving + 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); @@ -551,11 +480,11 @@ public class Drive extends Subsystem implements ControlLoopable //rightDrive1.changeControlMode(TalonControlMode.Position); //rightDrive1.setPosition(0); //rightDrive1.set(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); - }*/ + 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; } @@ -584,13 +513,16 @@ public class Drive extends Subsystem implements ControlLoopable else if (controlMode == DriveControlMode.MP_PATH) { isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); } - /*else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { + 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); - }*/ + } } } @@ -704,7 +636,7 @@ public class Drive extends Subsystem implements ControlLoopable 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: @@ -738,10 +670,14 @@ public class Drive extends Subsystem implements ControlLoopable public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { - - leftDrive1.set(leftPercentOutput); - rightDrive1.set(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) { @@ -814,14 +750,14 @@ public class Drive extends Subsystem implements ControlLoopable @Override public void setPeriodMs(long periodMs) { - /* FIX TODAY + //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; } @@ -854,34 +790,37 @@ public class Drive extends Subsystem implements ControlLoopable } public double getLeftPositionWorld() { - return 0;//leftDrive1.getPositionWorld(); FIX TODAY + return leftDrive1.getPositionWorld(); } public double getRightPositionWorld() { - return 0;//rightDrive1.getPositionWorld(); FIX TODAY + return rightDrive1.getPositionWorld(); } public void updateStatus(Robot.OperationMode operationMode) { if (operationMode == Robot.OperationMode.TEST) { try { - SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg()); SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); - - - SmartDashboard.putNumber("Right Pos Ticks", right_distance.getPosition());//rightDrive1.getSelectedSensorPosition(0)); FIX TODAY - SmartDashboard.putNumber("Left Pos Ticks", left_distance.getPosition());//leftDrive1.getSelectedSensorPosition(0)); - - - //SmartDashboard.putNumber("Right Pos Inches", -encoderRight.getPosition());//rightDrive1.getPositionWorld()); - //SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld()); - //SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12); - //SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12); + 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", Math.abs(getGyroAngleDeg() % 360)); - //MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); - //double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; - //SmartDashboard.putNumber("Gyro Delta", delta); + 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); @@ -890,22 +829,14 @@ public class Drive extends Subsystem implements ControlLoopable SmartDashboard.putNumber("Move Output", m_moveOutput); SmartDashboard.putNumber("Steer Input", m_steerInput); SmartDashboard.putNumber("Move Input", m_moveInput); - SmartDashboard.putString("MODE", "TEST"); - //if (left_distance.getPosition() != 0 && right_distance.getPosition() != 0){ - // SmartDashboard.putNumber("Distance Inches", (left_distance.getPosition()-right_distance.getPosition())/2);//rightDrive1.getPositionWorld()); - //} } catch (Exception e) { - System.err.println("Drivetrain update status error" +e.getMessage()); + System.err.println("Drivetrain update status error"); } } - else if (operationMode == Robot.OperationMode.COMPETITION) { - SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360)); - SmartDashboard.putString("MODE", "SICKO"); - if (left_distance.getPosition() != 0 && left_distance.getPosition() != 0){ - SmartDashboard.putNumber("Distance Inches", (left_distance.getPosition()-right_distance.getPosition())/2);//rightDrive1.getPositionWorld()); - } - SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); + else { + } } + } \ No newline at end of file