Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 00:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Update drive tain

Browse Source 
started convesion, stoped for testing, not ready to use
This commit is contained in:
lukesta182
2019-03-02 16:04:05 -07:00
parent 4add2e05bc
commit 8614a6b683
1 changed files with 117 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+117 -186
View File
@@ -1,4 +1,3 @@
package org.usfirst.frc4388.robot.subsystems; package org.usfirst.frc4388.robot.subsystems;
import java.util.ArrayList; import java.util.ArrayList;
@@ -10,9 +9,11 @@ import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap; import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.utility.control.AdaptivePurePursuitController; import org.usfirst.frc4388.utility.control.AdaptivePurePursuitController;
import org.usfirst.frc4388.utility.BHRMathUtils; 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.ControlLoopable;
import org.usfirst.frc4388.utility.control.Kinematics; 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;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import org.usfirst.frc4388.utility.MPTalonPIDController; 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.MotionProfilePoint;
//import org.usfirst.frc4388.utility.MotionProfileBoxCar; //import org.usfirst.frc4388.utility.MotionProfileBoxCar;
import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.PIDParams;
//import org.usfirst.frc4388.utility.Path; import org.usfirst.frc4388.utility.control.Path;
//import org.usfirst.frc4388.utility.PathGenerator;
import org.usfirst.frc4388.utility.math.RigidTransform2d; import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.math.Rotation2d; import org.usfirst.frc4388.utility.math.Rotation2d;
import org.usfirst.frc4388.utility.SoftwarePIDController; import org.usfirst.frc4388.utility.SoftwarePIDController;
import org.usfirst.frc4388.utility.math.Translation2d; import org.usfirst.frc4388.utility.math.Translation2d;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.FeedbackDevice;
@@ -37,15 +38,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.kauailabs.navx.frc.AHRS; 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.DoubleSolenoid;
//import edu.wpi.first.wpilibj.DigitalInput; //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; public static final double MP_MAX_TURN_T2 = 200;
// Motor controllers // Motor controllers
private ArrayList<CANPIDController> motorControllers = new ArrayList<CANPIDController>(); private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
private CANSparkMax leftDrive1; private TalonSRXEncoder leftDrive1;
private CANSparkMax leftDrive2;
private CANSparkMax rightDrive1;
private CANSparkMax rightDrive2;
private CANEncoder left_distance;
private CANEncoder right_distance;
private TalonSRXEncoder rightDrive1;
private CANSparkMax m_motor;
private CANEncoder m_encoder;
private DifferentialDrive m_drive; private DifferentialDrive m_drive;
//PID encoder and motor
//private DigitalInput hopperSensorRed; //private DigitalInput hopperSensorRed;
//private DigitalInput hopperSensorBlue; //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(.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 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 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 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.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 AHRS gyroNavX;
private boolean useGyroLock; private boolean useGyroLock;
private double gyroLockAngleDeg; private double gyroLockAngleDeg;
private double kPGyro = 0.07; //private double kPGyro = 0.04;
//private double kPGyro = 0.0625; private double kPGyro = 0.0625;
private boolean isCalibrating = false; private boolean isCalibrating = false;
private double gyroOffsetDeg = 0; private double gyroOffsetDeg = 0;
public Drive() { public Drive() {
try { try {
//System.err.println("Beginning of Drive."); leftDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
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();
//System.err.println("After Constructors."); rightDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder);
//gyroPigeon = new PigeonImu(leftDrive2);
gyroNavX = new AHRS(SPI.Port.kMXP); gyroNavX = new AHRS(SPI.Port.kMXP);
leftDrive2.follow(leftDrive1);
rightDrive2.follow(rightDrive1);
//hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID); //hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID);
//hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID); //hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID);
//leftDrive1.clearStickyFaults(); //leftDrive1.clearStickyFaults();
//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
//leftDrive1.setNominalClosedLoopVoltage(12.0); //leftDrive1.setNominalClosedLoopVoltage(12.0);
leftDrive1.clearFaults(); leftDrive1.clearStickyFaults(0);
leftDrive1.setInverted(false);//false on practice, true on comp leftDrive1.setInverted(false);//false on comp 2108, false on microbot
//leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot
//leftDrive1.setSafetyEnabled(false); //not needed for spark leftDrive1.setSafetyEnabled(false);
//leftDrive1.setCurrentLimit(15); //leftDrive1.setCurrentLimit(15);
//leftDrive1.enableCurrentLimit(true); //leftDrive1.enableCurrentLimit(true);
leftDrive1.setIdleMode(IdleMode.kBrake); leftDrive1.setNeutralMode(NeutralMode.Brake);
//leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); // not needed for spark? leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
//leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); // not needed for spark? leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
//leftDrive1.configNominalOutputForward(+1.0f, 0); // not needed for spark? leftDrive1.configNominalOutputForward(+1.0f, 0);
//leftDrive1.configNominalOutputReverse(-1.0f, 0); // not needed for spark? leftDrive1.configNominalOutputReverse(-1.0f, 0);
// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
// Driver.reportError("Could not detect left drive encoder encoder!", false); // 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.clearStickyFaults();
//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
//rightDrive1.setNominalClosedLoopVoltage(12.0); //rightDrive1.setNominalClosedLoopVoltage(12.0);
rightDrive1.clearFaults(); rightDrive1.clearStickyFaults(0);
rightDrive1.setInverted(false);//false on comp, false on practice rightDrive1.setInverted(true);//true on comp 2108, false on microbot
//rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
//rightDrive1.setSafetyEnabled(false); rightDrive1.setSafetyEnabled(false);
rightDrive1.setIdleMode(IdleMode.kBrake); rightDrive1.setNeutralMode(NeutralMode.Brake);
//rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
//rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
//rightDrive1.configNominalOutputForward(+1.0f, 0); rightDrive1.configNominalOutputForward(+1.0f, 0);
//rightDrive1.configNominalOutputReverse(-1.0f, 0); rightDrive1.configNominalOutputReverse(-1.0f, 0);
// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
// DriverStation.reportError("Could not detect right drive encoder encoder!", false); // 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 = new RobotDrive(leftDrive1, rightDrive1);
//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); //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.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify
m_drive.setSafetyEnabled(false); m_drive.setSafetyEnabled(false);
//speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID);
//System.err.println("End of Drive.");
} }
catch (Exception e) { catch (Exception e) {
System.err.println("An error occurred in the DriveTrain constructor"); 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) { public void setToBrakeOnNeutral(boolean brakeVsCoast) {
if (brakeVsCoast) { if (brakeVsCoast) {
leftDrive1.setIdleMode(IdleMode.kBrake); leftDrive1.setNeutralMode(NeutralMode.Brake);
leftDrive2.setIdleMode(IdleMode.kBrake);
rightDrive1.setIdleMode(IdleMode.kBrake); rightDrive1.setNeutralMode(NeutralMode.Brake);
rightDrive2.setIdleMode(IdleMode.kBrake);
} else { } else {
leftDrive1.setIdleMode(IdleMode.kCoast); leftDrive1.setNeutralMode(NeutralMode.Coast);
leftDrive2.setIdleMode(IdleMode.kCoast);
rightDrive1.setIdleMode(IdleMode.kCoast); rightDrive1.setNeutralMode(NeutralMode.Coast);
rightDrive2.setIdleMode(IdleMode.kCoast);
} }
} }
@@ -384,35 +339,22 @@ public class Drive extends Subsystem implements ControlLoopable
// return isHopperSensorBlueOn(); // 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) { public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
mpStraightController.setPID(mpStraightPIDParams); mpStraightController.setPID(mpStraightPIDParams);
mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true);
setControlMode(DriveControlMode.MP_STRAIGHT); 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(); // double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
// mpStraightController.setPID(mpStraightPIDParams); // mpStraightController.setPID(mpStraightPIDParams);
// mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); // mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true);
@@ -449,7 +391,7 @@ public class Drive extends Subsystem implements ControlLoopable
pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType);
setControlMode(DriveControlMode.PID_TURN); setControlMode(DriveControlMode.PID_TURN);
} }
/*
public void setPathMP(PathGenerator path) { public void setPathMP(PathGenerator path) {
mpPathController.setPID(mpPathPIDParams); mpPathController.setPID(mpPathPIDParams);
mpPathController.setMPPathTarget(path); mpPathController.setMPPathTarget(path);
@@ -471,7 +413,7 @@ public class Drive extends Subsystem implements ControlLoopable
adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25);
setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); setControlMode(DriveControlMode.ADAPTIVE_PURSUIT);
} }
*/
public void setDriveHold(boolean status) { public void setDriveHold(boolean status) {
if (status) { if (status) {
setControlMode(DriveControlMode.HOLD); setControlMode(DriveControlMode.HOLD);
@@ -482,14 +424,13 @@ public class Drive extends Subsystem implements ControlLoopable
} }
public void updatePose() { public void updatePose() {
//m_encoder = m_motor.getEncoder(); double left_distance = leftDrive1.getPositionWorld();
left_distance = leftDrive1.getEncoder(); double right_distance = rightDrive1.getPositionWorld();
right_distance = rightDrive1.getEncoder();
Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg());
lastPose = currentPose; lastPose = currentPose;
// currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
// left_encoder_prev_distance_ = left_distance; left_encoder_prev_distance_ = left_distance;
// right_encoder_prev_distance_ = right_distance; right_encoder_prev_distance_ = right_distance;
} }
public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) { 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. * @return Set of Strings with Path Markers that the robot has crossed.
*/ */
/*
public synchronized Set<String> getPathMarkersCrossed() { public synchronized Set<String> getPathMarkersCrossed() {
if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) { if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) {
return null; return null;
} else { } else {
return adaptivePursuitController.getMarkersCrossed(); return adaptivePursuitController.getMarkersCrossed();
} }
}*/ }
public synchronized void setControlMode(DriveControlMode controlMode) { public synchronized void setControlMode(DriveControlMode controlMode) {
this.controlMode = controlMode; this.controlMode = controlMode;
if (controlMode == DriveControlMode.JOYSTICK) { if (controlMode == DriveControlMode.JOYSTICK) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus); //leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus); //rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.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(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) { else if (controlMode == DriveControlMode.MANUAL) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus); //leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus); //rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.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(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) { else if (controlMode == DriveControlMode.CLIMB) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus); //leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus); //rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.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(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) { else if (controlMode == DriveControlMode.HOLD) {
mpStraightController.setPID(mpHoldPIDParams); mpStraightController.setPID(mpHoldPIDParams);
//leftDrive1.changeControlMode(TalonControlMode.Position); //leftDrive1.changeControlMode(TalonControlMode.Position);
@@ -551,11 +480,11 @@ public class Drive extends Subsystem implements ControlLoopable
//rightDrive1.changeControlMode(TalonControlMode.Position); //rightDrive1.changeControlMode(TalonControlMode.Position);
//rightDrive1.setPosition(0); //rightDrive1.setPosition(0);
//rightDrive1.set(0); //rightDrive1.set(0);
//leftDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
leftDrive1.set(0); leftDrive1.set(ControlMode.Position, 0);
//rightDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
rightDrive1.set(0); rightDrive1.set(ControlMode.Position, 0);
}*/ }
isFinished = false; isFinished = false;
} }
@@ -584,13 +513,16 @@ public class Drive extends Subsystem implements ControlLoopable
else if (controlMode == DriveControlMode.MP_PATH) { else if (controlMode == DriveControlMode.MP_PATH) {
isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg());
} }
/*else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
} }
else if (controlMode == DriveControlMode.MOTION_MAGIC) {
isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) { else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
updatePose(); updatePose();
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
}*/ }
} }
} }
@@ -704,7 +636,7 @@ public class Drive extends Subsystem implements ControlLoopable
m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
} }
m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75); m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);
// break; // break;
// case CONTROLLER_XBOX_ARCADE_RIGHT: // case CONTROLLER_XBOX_ARCADE_RIGHT:
@@ -738,10 +670,14 @@ public class Drive extends Subsystem implements ControlLoopable
public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
if (elevatorRight.getSelectedSensorPosition(0) >= 3550) {
leftDrive1.set(leftPercentOutput); leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5);
rightDrive1.set(rightPercentOutput); 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) { private boolean inDeadZone(double input) {
@@ -814,14 +750,14 @@ public class Drive extends Subsystem implements ControlLoopable
@Override @Override
public void setPeriodMs(long periodMs) { public void setPeriodMs(long periodMs) {
/* FIX TODAY //mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers);
mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers); mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers);
mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers);
pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers);
mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers); mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers);
mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers); mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers);
adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers); adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers);
*/
this.periodMs = periodMs; this.periodMs = periodMs;
} }
@@ -854,34 +790,37 @@ public class Drive extends Subsystem implements ControlLoopable
} }
public double getLeftPositionWorld() { public double getLeftPositionWorld() {
return 0;//leftDrive1.getPositionWorld(); FIX TODAY return leftDrive1.getPositionWorld();
} }
public double getRightPositionWorld() { public double getRightPositionWorld() {
return 0;//rightDrive1.getPositionWorld(); FIX TODAY return rightDrive1.getPositionWorld();
} }
public void updateStatus(Robot.OperationMode operationMode) { public void updateStatus(Robot.OperationMode operationMode) {
if (operationMode == Robot.OperationMode.TEST) { if (operationMode == Robot.OperationMode.TEST) {
try { try {
SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg());
SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); 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 Ticks", right_distance.getPosition());//rightDrive1.getSelectedSensorPosition(0)); FIX TODAY SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld());
SmartDashboard.putNumber("Left Pos Ticks", left_distance.getPosition());//leftDrive1.getSelectedSensorPosition(0)); 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 Inches", -encoderRight.getPosition());//rightDrive1.getPositionWorld()); //SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID));
//SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld()); //SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID));
//SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12); // SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));
//SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12); //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.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
//SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg());
SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360)); SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg());
//MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint();
//double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0;
//SmartDashboard.putNumber("Gyro Delta", delta); SmartDashboard.putNumber("Gyro Delta", delta);
//SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating);
SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating());
SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
@@ -890,22 +829,14 @@ public class Drive extends Subsystem implements ControlLoopable
SmartDashboard.putNumber("Move Output", m_moveOutput); SmartDashboard.putNumber("Move Output", m_moveOutput);
SmartDashboard.putNumber("Steer Input", m_steerInput); SmartDashboard.putNumber("Steer Input", m_steerInput);
SmartDashboard.putNumber("Move Input", m_moveInput); 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) { 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) { else {
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);
} }
} }
} }