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

elijah changes for drive

Browse Source 
Drive working, utilities do not work
This commit is contained in:
Evan Lanham
2019-01-21 08:38:26 -08:00
parent 792b3b5ce2
commit 855cfdb3d8
2 changed files with 86 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+10 -22
View File
@@ -45,16 +45,6 @@ public class Robot extends IterativeRobot
public static OperationMode operationMode = OperationMode.TEST; public static OperationMode operationMode = OperationMode.TEST;
private SendableChooser<OperationMode> operationModeChooser; 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() public void robotInit()
{ {
//Printing game data to riolog //Printing game data to riolog
@@ -116,15 +106,17 @@ public class Robot extends IterativeRobot
} }
public void teleopInit() { public void teleopInit() {
if (RRautonomousCommand != null) RRautonomousCommand.cancel(); System.err.println("Beginning of teleopInit.");
if (RLautonomousCommand != null) RLautonomousCommand.cancel();
if (LRautonomousCommand != null) LRautonomousCommand.cancel();
if (LLautonomousCommand != null) LLautonomousCommand.cancel();
drive.setToBrakeOnNeutral(false); drive.setToBrakeOnNeutral(false);
updateChoosers(); updateChoosers();
controlLoop.start(); System.err.println("TeleopInit after UpdateChoosers");
drive.resetEncoders(); controlLoop.start();
drive.endGyroCalibration(); System.err.println("TeleopInit after controlLoop");
//drive.resetEncoders();
//System.err.println("TeleopInit after resetEncoders");
drive.endGyroCalibration();
System.err.println("TeleopInit after endGyroCalibrations");
updateStatus(); updateStatus();
} }
@@ -143,10 +135,6 @@ public class Robot extends IterativeRobot
private void updateChoosers() { private void updateChoosers() {
operationMode = (OperationMode)operationModeChooser.getSelected(); operationMode = (OperationMode)operationModeChooser.getSelected();
RRautonomousCommand = (Command)RRautonTaskChooser.getSelected();
RLautonomousCommand = (Command)RLautonTaskChooser.getSelected();
LRautonomousCommand = (Command)LRautonTaskChooser.getSelected();
LLautonomousCommand = (Command)LLautonTaskChooser.getSelected();
} }
public Alliance getAlliance() { public Alliance getAlliance() {
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+75 -64
View File
@@ -38,7 +38,9 @@ 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.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.DoubleSolenoid; 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; public static final double MP_MAX_TURN_T2 = 200;
// Motor controllers // Motor controllers
private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>(); private ArrayList<CANPIDController> motorControllers = new ArrayList<CANPIDController>();
private CANSparkMax leftDrive1; private CANSparkMax leftDrive1;
private CANSparkMax leftDrive2; private CANSparkMax leftDrive2;
@@ -198,6 +200,8 @@ public class Drive extends Subsystem implements ControlLoopable
public Drive() { public Drive() {
try { 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, MotorType.kBrushless);
//leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); //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); 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); rightDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless);
encoderRight = new CANEncoder(rightDrive1); encoderRight = new CANEncoder(rightDrive1);
rightDrive2.follow(rightDrive1); rightDrive2.follow(rightDrive1);
//System.err.println("After Constructors.");
//gyroPigeon = new PigeonImu(leftDrive2); //gyroPigeon = new PigeonImu(leftDrive2);
gyroNavX = new AHRS(SPI.Port.kMXP); gyroNavX = new AHRS(SPI.Port.kMXP);
@@ -217,17 +224,17 @@ public class Drive extends Subsystem implements ControlLoopable
//leftDrive1.clearStickyFaults(); //leftDrive1.clearStickyFaults();
//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
//leftDrive1.setNominalClosedLoopVoltage(12.0); //leftDrive1.setNominalClosedLoopVoltage(12.0);
leftDrive1.clearStickyFaults(0); leftDrive1.clearFaults();
leftDrive1.setInverted(false);//false on comp 2108, false on microbot leftDrive1.setInverted(true);//false on comp 2108, false on microbot
leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark
leftDrive1.setSafetyEnabled(false); //leftDrive1.setSafetyEnabled(false); //not needed for spark
//leftDrive1.setCurrentLimit(15); //leftDrive1.setCurrentLimit(15);
//leftDrive1.enableCurrentLimit(true); //leftDrive1.enableCurrentLimit(true);
leftDrive1.setNeutralMode(NeutralMode.Brake); leftDrive1.setIdleMode(IdleMode.kBrake);
leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); //leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); // not needed for spark?
leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); //leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); // not needed for spark?
leftDrive1.configNominalOutputForward(+1.0f, 0); //leftDrive1.configNominalOutputForward(+1.0f, 0); // not needed for spark?
leftDrive1.configNominalOutputReverse(-1.0f, 0); //leftDrive1.configNominalOutputReverse(-1.0f, 0); // not needed for spark?
// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // 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.setInverted(true);//false on comp 2108, false on microbot
leftDrive2.setSafetyEnabled(false); //leftDrive2.setSafetyEnabled(false);
leftDrive2.setNeutralMode(NeutralMode.Brake); leftDrive2.setIdleMode(IdleMode.kBrake);
leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); //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.clearStickyFaults(0); rightDrive1.clearFaults();
rightDrive1.setInverted(true);//true on comp 2108, false on microbot 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.setNeutralMode(NeutralMode.Brake); rightDrive1.setIdleMode(IdleMode.kBrake);
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(true);//True on comp 2108, false on microbot rightDrive2.setInverted(true);//True on comp 2108, false on microbot
rightDrive2.setSafetyEnabled(false); //rightDrive2.setSafetyEnabled(false);
rightDrive2.setNeutralMode(NeutralMode.Brake); rightDrive2.setIdleMode(IdleMode.kBrake);
rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); //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);
motorControllers.add(leftDrive1); //System.err.println("After motorControllers.");
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);
@@ -279,8 +290,11 @@ 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");
} }
@@ -288,15 +302,15 @@ public class Drive extends Subsystem implements ControlLoopable
public void setToBrakeOnNeutral(boolean brakeVsCoast) { public void setToBrakeOnNeutral(boolean brakeVsCoast) {
if (brakeVsCoast) { if (brakeVsCoast) {
leftDrive1.setNeutralMode(NeutralMode.Brake); leftDrive1.setIdleMode(IdleMode.kBrake);
leftDrive2.setNeutralMode(NeutralMode.Brake); leftDrive2.setIdleMode(IdleMode.kBrake);
rightDrive1.setNeutralMode(NeutralMode.Brake); rightDrive1.setIdleMode(IdleMode.kBrake);
rightDrive2.setNeutralMode(NeutralMode.Brake); rightDrive2.setIdleMode(IdleMode.kBrake);
} else { } else {
leftDrive1.setNeutralMode(NeutralMode.Coast); leftDrive1.setIdleMode(IdleMode.kCoast);
leftDrive2.setNeutralMode(NeutralMode.Coast); leftDrive2.setIdleMode(IdleMode.kCoast);
rightDrive1.setNeutralMode(NeutralMode.Coast); rightDrive1.setIdleMode(IdleMode.kCoast);
rightDrive2.setNeutralMode(NeutralMode.Coast); rightDrive2.setIdleMode(IdleMode.kCoast);
} }
} }
@@ -435,8 +449,8 @@ public class Drive extends Subsystem implements ControlLoopable
} }
public void updatePose() { public void updatePose() {
double left_distance = leftDrive1.getPositionWorld(); double left_distance = 0;//leftDrive1.getPositionWorld();
double right_distance = rightDrive1.getPositionWorld(); double right_distance = 0;//rightDrive1.getPositionWorld(); FIX TODAY
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);
@@ -468,20 +482,20 @@ public class Drive extends Subsystem implements ControlLoopable
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(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(ControlMode.PercentOutput, 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) { else if (controlMode == DriveControlMode.MANUAL) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus); //leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus); //rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.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(ControlMode.PercentOutput, 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) { else if (controlMode == DriveControlMode.CLIMB) {
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus); //leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus); //rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
leftDrive1.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(ControlMode.PercentOutput, 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) { else if (controlMode == DriveControlMode.HOLD) {
mpStraightController.setPID(mpHoldPIDParams); mpStraightController.setPID(mpHoldPIDParams);
@@ -491,10 +505,10 @@ 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); //TODO: verify want 0="Primary closed-loop", with no timeout //leftDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout
leftDrive1.set(ControlMode.Position, 0); leftDrive1.set(0);
rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout //rightDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout
rightDrive1.set(ControlMode.Position, 0); rightDrive1.set(0);
} }
isFinished = false; isFinished = false;
} }
@@ -678,14 +692,10 @@ 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(ControlMode.PercentOutput, leftPercentOutput*.5); leftDrive1.set(leftPercentOutput);
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5); rightDrive1.set(rightPercentOutput);
}
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) {
@@ -758,13 +768,14 @@ public class Drive extends Subsystem implements ControlLoopable
@Override @Override
public void setPeriodMs(long periodMs) { public void setPeriodMs(long periodMs) {
/* FIX TODAY
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;
} }
@@ -797,23 +808,23 @@ public class Drive extends Subsystem implements ControlLoopable
} }
public double getLeftPositionWorld() { public double getLeftPositionWorld() {
return leftDrive1.getPositionWorld(); return 0;//leftDrive1.getPositionWorld(); FIX TODAY
} }
public double getRightPositionWorld() { public double getRightPositionWorld() {
return rightDrive1.getPositionWorld(); return 0;//rightDrive1.getPositionWorld(); FIX TODAY
} }
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("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Right Pos Ticks", 0);//rightDrive1.getSelectedSensorPosition(0)); FIX TODAY
SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Left Pos Ticks", 0);//leftDrive1.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld()); SmartDashboard.putNumber("Right Pos Inches", 0);//rightDrive1.getPositionWorld());
SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld()); SmartDashboard.putNumber("Left Pos Inches", 0);//leftDrive1.getPositionWorld());
SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12); SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12);
SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.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 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 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("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));