mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
elijah changes for drive
Drive working, utilities do not work
This commit is contained in:
Evan Lanham
@@ -45,16 +45,6 @@ public class Robot extends IterativeRobot
|
||||
public static OperationMode operationMode = OperationMode.TEST;
|
||||
|
||||
private SendableChooser<OperationMode> operationModeChooser;
|
||||
private SendableChooser<Command> RRautonTaskChooser;
|
||||
private SendableChooser<Command> RLautonTaskChooser;
|
||||
private SendableChooser<Command> LRautonTaskChooser;
|
||||
private SendableChooser<Command> 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() {
|
||||
|
||||
@@ -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<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
|
||||
private ArrayList<CANPIDController> motorControllers = new ArrayList<CANPIDController>();
|
||||
|
||||
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));
|
||||
|
||||
Reference in New Issue
Block a user