From 4add2e05bc68c43abf4592f4749d39b3e1266aa2 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Sat, 2 Mar 2019 14:21:13 -0700 Subject: [PATCH] DO AT COMP, update spark maxes, makde changes to optimize driving uptated spark maxes to make the able to fallow tallon, todo. drive needs to fix to make "smart diving" capable, also lol 118 is second place at there regional --- .../usfirst/frc4388/robot/subsystems/Arm.java | 2 +- .../frc4388/robot/subsystems/Drive.java | 87 ++++++++++--------- 2019robot/vendordeps/REVRobotics.json | 33 +++++-- 3 files changed, 77 insertions(+), 45 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 84835f8..ec85781 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -291,7 +291,7 @@ public class Arm extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick(joyStickSpeed*.60); + setSpeedJoystick((joyStickSpeed*.30)+.12); } /* public void setShiftState(ElevatorSpeedShiftState state) { 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 fc649c6..6f787b4 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 @@ -42,6 +42,10 @@ 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; @@ -101,8 +105,14 @@ public class Drive extends Subsystem implements ControlLoopable private CANSparkMax leftDrive2; private CANSparkMax rightDrive1; private CANSparkMax rightDrive2; - private CANEncoder encoderLeft; - private CANEncoder encoderRight; + private CANEncoder left_distance; + private CANEncoder right_distance; + + + + + private CANSparkMax m_motor; + private CANEncoder m_encoder; private DifferentialDrive m_drive; @@ -206,19 +216,21 @@ public class Drive extends Subsystem implements ControlLoopable 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); - leftDrive2.follow(leftDrive1); + //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); - encoderRight = new CANEncoder(rightDrive1); - rightDrive2.follow(rightDrive1); + + + //leftDrive1.restoreFactoryDefaults(); //System.err.println("After Constructors."); //gyroPigeon = new PigeonImu(leftDrive2); 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); @@ -226,7 +238,7 @@ public class Drive extends Subsystem implements ControlLoopable //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //leftDrive1.setNominalClosedLoopVoltage(12.0); leftDrive1.clearFaults(); - leftDrive1.setInverted(false);//false on comp 2108, false on microbot + 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.setCurrentLimit(15); @@ -243,7 +255,7 @@ public class Drive extends Subsystem implements ControlLoopable // } - leftDrive2.setInverted(false);//false on comp 2108, false on microbot + leftDrive2.setInverted(false);//false on practice. true on comp //leftDrive2.setSafetyEnabled(false); leftDrive2.setIdleMode(IdleMode.kBrake); //leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); // set above @@ -254,7 +266,7 @@ public class Drive extends Subsystem implements ControlLoopable //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //rightDrive1.setNominalClosedLoopVoltage(12.0); rightDrive1.clearFaults(); - rightDrive1.setInverted(false);//true on comp 2108, false on microbot + 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); @@ -267,7 +279,7 @@ public class Drive extends Subsystem implements ControlLoopable // } - rightDrive2.setInverted(false);//True on comp 2108, false on microbot + rightDrive2.setInverted(false);//True on comp 2019, false on practice //rightDrive2.setSafetyEnabled(false); rightDrive2.setIdleMode(IdleMode.kBrake); //rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); @@ -470,13 +482,14 @@ public class Drive extends Subsystem implements ControlLoopable } public void updatePose() { - double left_distance = encoderLeft.getPosition(); - double right_distance = encoderRight.getPosition(); + //m_encoder = m_motor.getEncoder(); + left_distance = leftDrive1.getEncoder(); + right_distance = rightDrive1.getEncoder(); 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) { @@ -853,26 +866,22 @@ public class Drive extends Subsystem implements ControlLoopable try { SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg()); SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); - 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", -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("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.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.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); + //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); @@ -882,19 +891,19 @@ public class Drive extends Subsystem implements ControlLoopable SmartDashboard.putNumber("Steer Input", m_steerInput); SmartDashboard.putNumber("Move Input", m_moveInput); SmartDashboard.putString("MODE", "TEST"); - if (encoderLeft.getPosition() != 0 && encoderRight.getPosition() != 0){ - SmartDashboard.putNumber("Distance Inches", (encoderLeft.getPosition()-encoderRight.getPosition())/2);//rightDrive1.getPositionWorld()); - } + //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"); + System.err.println("Drivetrain update status error" +e.getMessage()); } } else if (operationMode == Robot.OperationMode.COMPETITION) { SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360)); SmartDashboard.putString("MODE", "SICKO"); - if (encoderLeft.getPosition() != 0 && encoderRight.getPosition() != 0){ - SmartDashboard.putNumber("Distance Inches", (encoderLeft.getPosition()-encoderRight.getPosition())/2);//rightDrive1.getPositionWorld()); + 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); } diff --git a/2019robot/vendordeps/REVRobotics.json b/2019robot/vendordeps/REVRobotics.json index 368fd8a..a70dc13 100644 --- a/2019robot/vendordeps/REVRobotics.json +++ b/2019robot/vendordeps/REVRobotics.json @@ -1,7 +1,7 @@ { "fileName": "REVRobotics.json", "name": "REVRobotics", - "version": "1.0.27", + "version": "1.1.9", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "http://www.revrobotics.com/content/sw/max/sdk/maven/" @@ -11,15 +11,26 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-java", - "version": "1.0.27" + "version": "1.1.9" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.1.9", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] } ], - "jniDependencies": [], "cppDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-cpp", - "version": "1.0.27", + "version": "1.1.9", "libName": "libSparkMax", "headerClassifier": "headers", "sharedLibrary": false, @@ -27,6 +38,18 @@ "binaryPlatforms": [ "linuxathena" ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.1.9", + "libName": "libSparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] } ] -} +} \ No newline at end of file