From b82be52769ebb52057749021ae48a669d840dd1f Mon Sep 17 00:00:00 2001 From: Trogdor14 Date: Tue, 13 Aug 2019 18:59:34 -0400 Subject: [PATCH 1/9] attempting to add 6 spark motors --- .vscode/launch.json | 14 ++ src/main/java/frc/robot/RobotMap.java | 10 ++ .../frc/robot/commands/DriveWithJoystick.java | 3 + .../frc/robot/subsystems/DriveSystem.java | 142 ++++++++---------- 4 files changed, 93 insertions(+), 76 deletions(-) create mode 100644 .vscode/launch.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..ac60bd5 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,14 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "type": "java", + "name": "Debug (Launch) - Current File", + "request": "launch", + "mainClass": "${file}" + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 4a97874..cd54d7d 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -46,14 +46,24 @@ public class RobotMap { public static final int LOGI_LEFT_B = 5; /* Can addresses */ + // Motors + private static final int DRV_LEFTLEAD = 0; + private static final int DRV_LEFTFOLLOW_1 = 1; + private static final int DRV_LEFTFOLLOW_2 = 2; + private static final int DRV_RIGHTLEAD = 3; + private static final int DRV_RIGHTFOLLOW_1 = 4; + private static final int DRV_RIGHTFOLLOW_2 = 5; + +/* public static final int DRV_LEFT_MASTER = 1; public static final int DRV_LEFT_FOLLOW_1 = 2; public static final int DRV_LEFT_FOLLOW_2 = 3; public static final int DRV_RIGHT_MASTER = 4; public static final int DRV_RIGHT_FOLLOW_1 = 5; public static final int DRV_RIGHT_FOLLOW_2 = 6; +*/ public static final int LFT_MASTER = 7; public static final int LFT_FOLLOW_1 = 8; diff --git a/src/main/java/frc/robot/commands/DriveWithJoystick.java b/src/main/java/frc/robot/commands/DriveWithJoystick.java index 0424984..a8eb909 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc/robot/commands/DriveWithJoystick.java @@ -103,6 +103,9 @@ private void tankDrive() speed_y_left = oi.getJoystickDriveLeftYAxis() * -1.0; speed_y_right = oi.getJoystickDriveRightYAxis(); + //will this work??? IDK??? + myRobot.tankDrive(speed_y_left, speed_y_right); + if(Math.abs(speed_y_left) > DEADZONE || Math.abs(speed_y_right) > DEADZONE) { Bob.drive(speed_y_left, speed_y_right); diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index a4af234..b85ed3c 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -21,9 +21,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/** - * An example subsystem. You can replace me with your own Subsystem. - */ +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import com.revrobotics.CANPIDController; +import com.revrobotics.ControlType; + public class DriveSystem extends Subsystem { public SendableChooser arcade_chooser = new SendableChooser<>(); @@ -34,19 +37,20 @@ public class DriveSystem extends Subsystem { // Instance of the class private static final DriveSystem INSTANCE = new DriveSystem(); - // Motor Controllers - private VictorSPX climbDrive; - private TalonSRX leftMaster; - private TalonSRX rightMaster; - private TalonSRX leftSlave1; - private TalonSRX leftSlave2; - // private TalonSRX leftSlave3; - private TalonSRX rightSlave1; - private TalonSRX rightSlave2; - // private TalonSRX rightSlave3; + private DifferentialDrive myRobot; - // Current Variables + //setting the motors + private CANSparkMax leftLead; + private CANSparkMax leftFollow1; + private CANSparkMax leftFollow2; + private CANSparkMax rightLead; + private CANSparkMax rightFollow1; + private CANSparkMax rightFollow2; + + private CANPIDController pidController; + public double kP, kD, kIz, kFF, kMaxOutput, kMinOutput; + // Current Variables private static final int AMPS = 35; private static final int TIMEOUT_MS = 10; private static final int PEAK_CURRENT_TIME = 2000; @@ -63,22 +67,36 @@ public class DriveSystem extends Subsystem { public static int init_Right; // NavX - private boolean arcade; public DriveSystem() { NavX = new AHRS(SPI.Port.kMXP); System.out.println("Constructer NavX: " +NavX.getAngle()); - // Instantiate Motor Controllers - leftMaster = new TalonSRX(RobotMap.DRV_LEFT_MASTER); - rightMaster = new TalonSRX(RobotMap.DRV_RIGHT_MASTER); - leftSlave1 = new TalonSRX(RobotMap.DRV_LEFT_FOLLOW_1); - leftSlave2 = new TalonSRX(RobotMap.DRV_LEFT_FOLLOW_2); + myRobot = new DifferentialDrive(leftLead, rightLead); - rightSlave1 = new TalonSRX(RobotMap.DRV_RIGHT_FOLLOW_1); - rightSlave2 = new TalonSRX(RobotMap.DRV_RIGHT_FOLLOW_2); + + //instantiate motor controllers + leftLead = new CANSparkMax(DRV_LEFTLEAD, MotorType.kBrushless); + leftFollow1 = new CANSparkMAx(DRV_LEFTFOLLOW_1, MotorType.kBrushless); + leftFollow2 = new CANSparkMAx(DRV_LEFTFOLLOW_2, MotorType.kBrushless); + rightLead = new CANSparkMax(DRV_RIGHTLEAD, MotorType.kBrushless); + rightFollow1 = new CANSparkMax(DRV_RIGHTFOLLOW_1, MotorType.kBrushless); + rightFollow2 = new CANSparkMax(DRV_RIGHTFOLLOW_2, MotorType.kBrushless); + + pidController = leftLead.getPIDController(); + pidController = rightLead.getPIDController(); + + //PID coefficients + //TODO: find out if these are the values we want + kP = 0.1; + kI = 1e-4; + kD = 1; + kIz = 0; + kFF = 0; + kMaxOutput = 1; + kMinOutput = -1; climbDrive = new VictorSPX(RobotMap.CLIMBDRIVE); @@ -93,7 +111,6 @@ public DriveSystem() { @Override public void initDefaultCommand() { - } public Boolean getArcade() { @@ -101,12 +118,10 @@ public Boolean getArcade() { } public void setArcadeDrive(boolean enable) { - arcade = enable; } public static DriveSystem getInstance() { - return INSTANCE; } @@ -125,7 +140,6 @@ private void inititalizeDriveSystem() { * leftSlave1.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); * leftSlave1.enableCurrentLimit(true); * - * * rightMaster.configPeakCurrentLimit(ZERO, ZERO); * rightMaster.configPeakCurrentDuration(ZERO, ZERO); * rightMaster.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); @@ -136,11 +150,6 @@ private void inititalizeDriveSystem() { * rightSlave1.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); * rightSlave1.enableCurrentLimit(true); * - */ - - // Left Talon Current Limiting Not used for Test Robot - - /* * leftSlave2.configPeakCurrentLimit(ZERO, ZERO); * leftSlave2.configPeakCurrentDuration(ZERO, ZERO); * leftSlave2.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); @@ -150,11 +159,7 @@ private void inititalizeDriveSystem() { * leftSlave3.configPeakCurrentDuration(ZERO, ZERO); * leftSlave3.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); * leftSlave3.enableCurrentLimit(true); - */ - - // Right Talon Current Limiting Not used for Test Robot - - /* + * * rightSlave2.configPeakCurrentLimit(ZERO, ZERO); * rightSlave2.configPeakCurrentDuration(ZERO, ZERO); * rightSlave2.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); @@ -172,10 +177,10 @@ private void inititalizeDriveSystem() { //climbDrive.enableCurrentLimit(true); /* - * Configures the open loop ramp to set the motor to ramp up to speed after a - * specifiec time other than jerking to full speed. - * - */ + TODO: Transfer this over to SparkMax + Configures the open loop ramp to set the motor to ramp up to speed after a + specifiec time other than jerking to full speed. + leftMaster.configOpenloopRamp(RAMP_TIME, 0); leftSlave1.configOpenloopRamp(RAMP_TIME, 0); leftSlave2.configOpenloopRamp(RAMP_TIME, 0); @@ -183,10 +188,7 @@ private void inititalizeDriveSystem() { rightMaster.configOpenloopRamp(RAMP_TIME, 0); rightSlave1.configOpenloopRamp(RAMP_TIME, 0); rightSlave2.configOpenloopRamp(RAMP_TIME, 0); - - // leftSlave3.configOpenloopRamp(RAMP_TIME, 0); - - // rightSlave3.configOpenloopRamp(RAMP_TIME, 0); + */ // Setting the PID loop for the master controllers rightMaster.config_kP(0, TIMEOUT_MS); @@ -201,19 +203,27 @@ private void inititalizeDriveSystem() { leftMaster.set(ControlMode.PercentOutput, 0.0); leftSlave1.set(ControlMode.PercentOutput, 0.0); - leftSlave1.follow(leftMaster); leftSlave2.set(ControlMode.PercentOutput, 0.0); - leftSlave2.follow(leftMaster); - // leftSlave3.set(ControlMode.PercentOutput, 0.0); - /// leftSlave3.follow(leftMaster); rightMaster.set(ControlMode.PercentOutput, 0.0); rightSlave1.set(ControlMode.PercentOutput, 0.0); - rightSlave1.follow(rightMaster); rightSlave2.set(ControlMode.PercentOutput, 0.0); - rightSlave2.follow(leftMaster); - // rightSlave3.set(ControlMode.PercentOutput, 0.0); - // rightSlave3.follow(leftMaster); + + //sets the follows to follow the leads + leftFollow1.follow(leftLead); + leftFollow2.follow(leftLead); + rightFollow1.follow(rightLead); + rightFollow2.follow(rightLead); + + //set PID coefficients + pidController.setP(kP); + pidController.setI(kI); + pidController.setD(kD); + pidController.setIZone(kIz); + pidController.setFF(kFF); + pidController.setOutputRange(kMinOutput, kMaxOutput); + + pidController.setReference(rotations,ControlType.kPosition); slow = false; turbo =false; @@ -223,6 +233,7 @@ private void inititalizeDriveSystem() { } + //TODO convert this slow stuff to the sparks public void drive(Double LeftSpeed, Double RightSpeed) { setArcadeDrive(arcade_chooser.getSelected()); if (slow) { @@ -237,22 +248,14 @@ public void drive(Double LeftSpeed, Double RightSpeed) { RightSpeed = RightSpeed * .8; LeftSpeed = LeftSpeed * .8; } -// System.out.println(RightSpeed); - rightMaster.set(ControlMode.PercentOutput, RightSpeed); rightSlave1.set(ControlMode.PercentOutput, RightSpeed); rightSlave2.set(ControlMode.PercentOutput, RightSpeed); -// rightSlave3.set(ControlMode.PercentOutput, RightSpeed); - leftMaster.set(ControlMode.PercentOutput, LeftSpeed); leftSlave1.set(ControlMode.PercentOutput, LeftSpeed); -leftSlave2.set(ControlMode.PercentOutput, LeftSpeed); -// leftSlave3.set(ControlMode.PercentOutput, LeftSpeed); - - - +leftSlave2.set(ControlMode.PercentOutput, LeftSpeed); } @@ -261,12 +264,10 @@ public void stopDrive() { rightMaster.set(ControlMode.PercentOutput, 0.0); rightSlave1.set(ControlMode.PercentOutput, 0.0); rightSlave2.set(ControlMode.PercentOutput, 0.0); - // rightSlave3.set(ControlMode.PercentOutput, 0.0); leftMaster.set(ControlMode.PercentOutput, 0.0); leftSlave1.set(ControlMode.PercentOutput, 0.0); leftSlave2.set(ControlMode.PercentOutput, 0.0); - // leftSlave3.set(ControlMode.PercentOutput, 0.0); } public void driveSetSpeed(double Left_Speed, double Right_Speed) { @@ -287,43 +288,32 @@ public int getRightMasterEncoder() { return rightMaster.getSensorCollection().getPulseWidthPosition(); } +/* public void setSlow(boolean slowSetting) { - this.slow = slowSetting; } public boolean isInSlowMode() { - return slow; } public void setTurbo(boolean turboSetting) { - this.turbo = turboSetting; } public boolean isInTurboMode() { - return turbo; } +*/ public double getGyro(boolean backwards) { double angle; - /* - System.out.println("angle "+ NavX.getAngle()); - System.out.println("pitch "+ NavX.getPitch()); - System.out.println("roll "+ NavX.getRoll()); - System.out.println("Yaw "+ NavX.getYaw()); - System.out.println("Rotating" + NavX.isRotating()); - */ - if (backwards) { angle = (((((NavX.getAngle() + 180)) % 360) + 360) % 360); } else { angle = ((((NavX.getAngle()) % 360) + 360) % 360); } - // System.out.println("angle "+ angle); return angle; } From d25ffd45bb8489661ff6bea3be1ecc57063016e4 Mon Sep 17 00:00:00 2001 From: connellsensational Date: Sat, 7 Sep 2019 11:29:26 -0400 Subject: [PATCH 2/9] made sparkmax stuff actually work --- .wpilib/wpilib_preferences.json | 6 + src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotMap.java | 12 +- .../commands/Autonomous/DriveOffPlatform.java | 28 -- .../commands/ClimbCommands/DriveControl.java | 5 +- .../frc/robot/commands/DriveToDistance.java | 120 --------- .../frc/robot/commands/DriveWithJoystick.java | 3 - .../frc/robot/subsystems/DriveSystem.java | 254 ++++++------------ vendordeps/REVRobotics.json | 64 +++++ 9 files changed, 162 insertions(+), 331 deletions(-) create mode 100644 .wpilib/wpilib_preferences.json delete mode 100644 src/main/java/frc/robot/commands/Autonomous/DriveOffPlatform.java delete mode 100644 src/main/java/frc/robot/commands/DriveToDistance.java create mode 100644 vendordeps/REVRobotics.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..9025fdb --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "none", + "teamNumber": 342 +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4887630..bb7f200 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,7 +22,6 @@ import frc.robot.commands.HatchGrab; import frc.robot.commands.PneumaticsWithCANifier; import frc.robot.commands.LiftWithJoystick; -import frc.robot.commands.Autonomous.DriveOffPlatform; import frc.robot.subsystems.CameraVisionSystem; import frc.robot.subsystems.ExampleSubsystem; import frc.robot.commands.LiftToHeight; diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index cd54d7d..9565b9b 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -48,12 +48,12 @@ public class RobotMap { /* Can addresses */ // Motors - private static final int DRV_LEFTLEAD = 0; - private static final int DRV_LEFTFOLLOW_1 = 1; - private static final int DRV_LEFTFOLLOW_2 = 2; - private static final int DRV_RIGHTLEAD = 3; - private static final int DRV_RIGHTFOLLOW_1 = 4; - private static final int DRV_RIGHTFOLLOW_2 = 5; + public static final int DRV_LEFTLEAD = 0; + public static final int DRV_LEFTFOLLOW_1 = 1; + public static final int DRV_LEFTFOLLOW_2 = 2; + public static final int DRV_RIGHTLEAD = 3; + public static final int DRV_RIGHTFOLLOW_1 = 4; + public static final int DRV_RIGHTFOLLOW_2 = 5; /* diff --git a/src/main/java/frc/robot/commands/Autonomous/DriveOffPlatform.java b/src/main/java/frc/robot/commands/Autonomous/DriveOffPlatform.java deleted file mode 100644 index db93267..0000000 --- a/src/main/java/frc/robot/commands/Autonomous/DriveOffPlatform.java +++ /dev/null @@ -1,28 +0,0 @@ - -package frc.robot.commands.Autonomous; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.DriveToDistance; -import frc.robot.commands.DriveWithJoystick; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/** - * An example command. You can replace me with your own command. - */ -public class DriveOffPlatform extends CommandGroup { - -private DriveToDistance drive_to_distance; -private DriveWithJoystick drive_with_joystick; - - - public DriveOffPlatform() { - - drive_to_distance = new DriveToDistance(10); - //drive_with_joystick = new DriveWithJoystick(); - - addSequential(drive_to_distance); - //addSequential(drive_with_joystick); - -} -} diff --git a/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java b/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java index 450962b..5759c8e 100644 --- a/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java +++ b/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java @@ -43,7 +43,7 @@ protected void initialize() { @Override protected void execute() { - drive.driveWinch(0.5); + } @@ -56,7 +56,8 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { - drive.driveWinch(0.0); + + } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/DriveToDistance.java b/src/main/java/frc/robot/commands/DriveToDistance.java deleted file mode 100644 index 06e7619..0000000 --- a/src/main/java/frc/robot/commands/DriveToDistance.java +++ /dev/null @@ -1,120 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.DriveSystem; - -/** - * This will allow the robot to go forward a certain distance in autonomous - * Been tested on - */ - -public class DriveToDistance extends Command { - - private DriveSystem Bob; - - private double goal; - - //private boolean backwards; - - private double init_Left; - private double init_Right; - private double current_Left; - private double current_Right; - private double left_rotation_count; - private double right_rotation_count; - - //private double degrees_off_zero; - private double left_speed; - private double right_speed; - - //private static final double kP = -50; - private static final double SPEED_CONST = 0.5; - public static final double TEST_DISTANCE = 1.5; - - - - public DriveToDistance(double distance) { - /* - taking out this parameter because we aren't using the gyro right now - - , boolean backwards) - - */ - - Bob = DriveSystem.getInstance(); - //requires(Bob); - - goal = distance; - /* - also moving this out of code for now - - this.backwards = backwards; - - */ - - } - - protected void initialize() { - - init_Left = Bob.getLeftMasterEncoder(); - init_Right = Bob.getRightMasterEncoder(); - - } - - protected void execute() { - - left_speed = SPEED_CONST; - right_speed = SPEED_CONST; - - current_Left = Bob.getLeftMasterEncoder() - init_Left; - current_Right = Bob.getRightMasterEncoder() - init_Right; - - left_rotation_count = Math.abs(current_Left / 4096); - right_rotation_count = Math.abs(current_Right / 4096); - - /* if(backwards) { - Bob.driveSetSpeed(left_speed * -1.0, right_speed * -1.0); - }else { - Bob.driveSetSpeed(left_speed, right_speed); - } - */ - Bob.drive(left_speed, right_speed * -1); - - SmartDashboard.putNumber("left", left_rotation_count); - SmartDashboard.putNumber("right", right_rotation_count); - - System.out.println("left: " + left_rotation_count); - System.out.println("right: " + right_rotation_count); - - } - - @Override - protected boolean isFinished() { - - if(left_rotation_count >= goal || right_rotation_count >= goal) { - - return true; - - }else { - return false; - } - } - - protected void end() { - - Bob.stopDrive(); - } - - protected void interrupted() { - - end(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveWithJoystick.java b/src/main/java/frc/robot/commands/DriveWithJoystick.java index a8eb909..0424984 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc/robot/commands/DriveWithJoystick.java @@ -103,9 +103,6 @@ private void tankDrive() speed_y_left = oi.getJoystickDriveLeftYAxis() * -1.0; speed_y_right = oi.getJoystickDriveRightYAxis(); - //will this work??? IDK??? - myRobot.tankDrive(speed_y_left, speed_y_right); - if(Math.abs(speed_y_left) > DEADZONE || Math.abs(speed_y_right) > DEADZONE) { Bob.drive(speed_y_left, speed_y_right); diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index b85ed3c..3b77ac7 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -7,9 +7,11 @@ package frc.robot.subsystems; +/* import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; +*/ import edu.wpi.first.wpilibj.SPI; import com.kauailabs.navx.frc.AHRS; @@ -26,11 +28,14 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import com.revrobotics.CANPIDController; import com.revrobotics.ControlType; +import frc.robot.RobotMap; public class DriveSystem extends Subsystem { + //Smart Dashboard arcade mode chooser public SendableChooser arcade_chooser = new SendableChooser<>(); + // NavX AHRS NavX; @@ -47,8 +52,9 @@ public class DriveSystem extends Subsystem { private CANSparkMax rightFollow1; private CANSparkMax rightFollow2; - private CANPIDController pidController; - public double kP, kD, kIz, kFF, kMaxOutput, kMinOutput; + private CANPIDController rightPidController; + private CANPIDController leftPidController; + private double kP, kD, kI, kIz, kFF, kMaxOutput, kMinOutput, maxRPM; // Current Variables private static final int AMPS = 35; @@ -72,33 +78,28 @@ public class DriveSystem extends Subsystem { public DriveSystem() { NavX = new AHRS(SPI.Port.kMXP); - System.out.println("Constructer NavX: " +NavX.getAngle()); - - myRobot = new DifferentialDrive(leftLead, rightLead); - + System.out.println("Constructer NavX: " + NavX.getAngle()); //instantiate motor controllers - leftLead = new CANSparkMax(DRV_LEFTLEAD, MotorType.kBrushless); - leftFollow1 = new CANSparkMAx(DRV_LEFTFOLLOW_1, MotorType.kBrushless); - leftFollow2 = new CANSparkMAx(DRV_LEFTFOLLOW_2, MotorType.kBrushless); - rightLead = new CANSparkMax(DRV_RIGHTLEAD, MotorType.kBrushless); - rightFollow1 = new CANSparkMax(DRV_RIGHTFOLLOW_1, MotorType.kBrushless); - rightFollow2 = new CANSparkMax(DRV_RIGHTFOLLOW_2, MotorType.kBrushless); - - pidController = leftLead.getPIDController(); - pidController = rightLead.getPIDController(); - - //PID coefficients - //TODO: find out if these are the values we want - kP = 0.1; - kI = 1e-4; - kD = 1; + leftLead = new CANSparkMax(RobotMap.DRV_LEFTLEAD, MotorType.kBrushless); + leftFollow1 = new CANSparkMax(RobotMap.DRV_LEFTFOLLOW_1, MotorType.kBrushless); + leftFollow2 = new CANSparkMax(RobotMap.DRV_LEFTFOLLOW_2, MotorType.kBrushless); + rightLead = new CANSparkMax(RobotMap.DRV_RIGHTLEAD, MotorType.kBrushless); + rightFollow1 = new CANSparkMax(RobotMap.DRV_RIGHTFOLLOW_1, MotorType.kBrushless); + rightFollow2 = new CANSparkMax(RobotMap.DRV_RIGHTFOLLOW_2, MotorType.kBrushless); + + leftPidController = leftLead.getPIDController(); + rightPidController = rightLead.getPIDController(); + + // PID Coefficients + kP = 4.8e-5; + kI = 5.0e-7; + kD = 0; kIz = 0; kFF = 0; kMaxOutput = 1; - kMinOutput = -1; - - climbDrive = new VictorSPX(RobotMap.CLIMBDRIVE); + kMinOutput = -1; + maxRPM = 5700; inititalizeDriveSystem(); @@ -127,114 +128,43 @@ public static DriveSystem getInstance() { private void inititalizeDriveSystem() { - // Not Current Limiting DriveSystem() anymore this year. - - /* - * leftMaster.configPeakCurrentLimit(ZERO, ZERO); - * leftMaster.configPeakCurrentDuration(ZERO, ZERO); - * leftMaster.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * leftMaster.enableCurrentLimit(true); - * - * leftSlave1.configPeakCurrentLimit(ZERO, ZERO); - * leftSlave1.configPeakCurrentDuration(ZERO, ZERO); - * leftSlave1.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * leftSlave1.enableCurrentLimit(true); - * - * rightMaster.configPeakCurrentLimit(ZERO, ZERO); - * rightMaster.configPeakCurrentDuration(ZERO, ZERO); - * rightMaster.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * rightMaster.enableCurrentLimit(true); - * - * rightSlave1.configPeakCurrentLimit(ZERO, ZERO); - * rightSlave1.configPeakCurrentDuration(ZERO, ZERO); - * rightSlave1.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * rightSlave1.enableCurrentLimit(true); - * - * leftSlave2.configPeakCurrentLimit(ZERO, ZERO); - * leftSlave2.configPeakCurrentDuration(ZERO, ZERO); - * leftSlave2.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * leftSlave2.enableCurrentLimit(true); - * - * leftSlave3.configPeakCurrentLimit(ZERO, ZERO); - * leftSlave3.configPeakCurrentDuration(ZERO, ZERO); - * leftSlave3.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * leftSlave3.enableCurrentLimit(true); - * - * rightSlave2.configPeakCurrentLimit(ZERO, ZERO); - * rightSlave2.configPeakCurrentDuration(ZERO, ZERO); - * rightSlave2.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * rightSlave2.enableCurrentLimit(true); - * - * rightSlave3.configPeakCurrentLimit(ZERO, ZERO); - * rightSlave3.configPeakCurrentDuration(ZERO, ZERO); - * rightSlave3.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - * rightSlave3.enableCurrentLimit(true); - */ - - //climbDrive.configPeakCurrentLimit(AMPS, TIMEOUT_MS); - //climbDrive.configPeakCurrentDuration(PEAK_CURRENT_TIME, TIMEOUT_MS); - //climbDrive.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - //climbDrive.enableCurrentLimit(true); - - /* - TODO: Transfer this over to SparkMax - Configures the open loop ramp to set the motor to ramp up to speed after a - specifiec time other than jerking to full speed. - - leftMaster.configOpenloopRamp(RAMP_TIME, 0); - leftSlave1.configOpenloopRamp(RAMP_TIME, 0); - leftSlave2.configOpenloopRamp(RAMP_TIME, 0); - - rightMaster.configOpenloopRamp(RAMP_TIME, 0); - rightSlave1.configOpenloopRamp(RAMP_TIME, 0); - rightSlave2.configOpenloopRamp(RAMP_TIME, 0); - */ - // Setting the PID loop for the master controllers - rightMaster.config_kP(0, TIMEOUT_MS); - rightMaster.config_kI(0, TIMEOUT_MS); - rightMaster.config_kD(0, TIMEOUT_MS); - rightMaster.config_kF(0, TIMEOUT_MS); - - leftMaster.config_kP(0, TIMEOUT_MS); - leftMaster.config_kI(0, TIMEOUT_MS); - leftMaster.config_kD(0, TIMEOUT_MS); - leftMaster.config_kF(0, TIMEOUT_MS); - - leftMaster.set(ControlMode.PercentOutput, 0.0); - leftSlave1.set(ControlMode.PercentOutput, 0.0); - leftSlave2.set(ControlMode.PercentOutput, 0.0); - - rightMaster.set(ControlMode.PercentOutput, 0.0); - rightSlave1.set(ControlMode.PercentOutput, 0.0); - rightSlave2.set(ControlMode.PercentOutput, 0.0); + rightPidController.setP(kP); + rightPidController.setI(kI); + rightPidController.setD(kD); + rightPidController.setIZone(kIz); + rightPidController.setFF(kFF); + rightPidController.setOutputRange(kMinOutput, kMaxOutput); + + leftPidController.setP(kP); + leftPidController.setI(kI); + leftPidController.setD(kD); + leftPidController.setIZone(kIz); + leftPidController.setFF(kFF); + leftPidController.setOutputRange(kMinOutput, kMaxOutput); + + leftLead.set(0.0); + leftFollow1.set(0.0); + leftFollow2.set(0.0); + + rightLead.set(0.0); + rightFollow1.set(0.0); + rightFollow2.set(0.0); //sets the follows to follow the leads leftFollow1.follow(leftLead); leftFollow2.follow(leftLead); rightFollow1.follow(rightLead); rightFollow2.follow(rightLead); - - //set PID coefficients - pidController.setP(kP); - pidController.setI(kI); - pidController.setD(kD); - pidController.setIZone(kIz); - pidController.setFF(kFF); - pidController.setOutputRange(kMinOutput, kMaxOutput); - - pidController.setReference(rotations,ControlType.kPosition); + slow = false; - turbo =false; + turbo = false; - init_Left = getLeftMasterEncoder(); - init_Right = getRightMasterEncoder(); } - //TODO convert this slow stuff to the sparks - public void drive(Double LeftSpeed, Double RightSpeed) { + public void drive(double LeftSpeed, double RightSpeed) { setArcadeDrive(arcade_chooser.getSelected()); if (slow) { System.out.println("Changing Speeds to Slow Speeds"); @@ -249,63 +179,35 @@ public void drive(Double LeftSpeed, Double RightSpeed) { LeftSpeed = LeftSpeed * .8; } -rightMaster.set(ControlMode.PercentOutput, RightSpeed); -rightSlave1.set(ControlMode.PercentOutput, RightSpeed); -rightSlave2.set(ControlMode.PercentOutput, RightSpeed); + rightLead.set(RightSpeed); + rightFollow1.set(RightSpeed); + rightFollow2.set(RightSpeed); -leftMaster.set(ControlMode.PercentOutput, LeftSpeed); -leftSlave1.set(ControlMode.PercentOutput, LeftSpeed); -leftSlave2.set(ControlMode.PercentOutput, LeftSpeed); + leftLead.set(LeftSpeed); + leftFollow1.set(LeftSpeed); + leftFollow2.set(LeftSpeed); } public void stopDrive() { - rightMaster.set(ControlMode.PercentOutput, 0.0); - rightSlave1.set(ControlMode.PercentOutput, 0.0); - rightSlave2.set(ControlMode.PercentOutput, 0.0); + rightLead.set(0.0); + rightFollow1.set(0.0); + rightFollow2.set(0.0); - leftMaster.set(ControlMode.PercentOutput, 0.0); - leftSlave1.set(ControlMode.PercentOutput, 0.0); - leftSlave2.set(ControlMode.PercentOutput, 0.0); + leftLead.set(0.0); + leftFollow1.set(0.0); + leftFollow2.set(0.0); } - public void driveSetSpeed(double Left_Speed, double Right_Speed) { + public void driveSetSpeed(double LeftSpeed, double RightSpeed) { // argument is in position change per 100ms - rightMaster.set(ControlMode.Velocity, Right_Speed); - leftMaster.set(ControlMode.Velocity, Left_Speed); - - } - - public int getLeftMasterEncoder() { + rightLead.set(RightSpeed); + leftLead.set(LeftSpeed); - return leftMaster.getSensorCollection().getPulseWidthPosition(); } - public int getRightMasterEncoder() { - - return rightMaster.getSensorCollection().getPulseWidthPosition(); - } - -/* - public void setSlow(boolean slowSetting) { - this.slow = slowSetting; - } - - public boolean isInSlowMode() { - return slow; - } - - public void setTurbo(boolean turboSetting) { - this.turbo = turboSetting; - } - - public boolean isInTurboMode() { - return turbo; - } -*/ - public double getGyro(boolean backwards) { double angle; @@ -325,20 +227,30 @@ public void resetGyro() { NavX.reset(); } - public double toMeters() { + public double getTilt() { + return NavX.getPitch(); + } + + public boolean isInSlowMode() { + + return slow; + } - double current = getRightMasterEncoder() - init_Right; - // Conversion Enc Value per wheel Rotation = 1.4 - // meters per wheel rotation = 0.31415 - return (current * 1.4) / 0.31415; + public void setSlow(boolean slowSetting) { + this.slow = slowSetting; } - public void driveWinch(double speed) { - climbDrive.set(ControlMode.PercentOutput, -speed); + public void setTurbo(boolean turboSetting) { + + this.turbo = turboSetting; } - public double getTilt() { - return NavX.getPitch(); + public boolean isInTurboMode() { + + return turbo; } + + + } diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json new file mode 100644 index 0000000..5a22a7a --- /dev/null +++ b/vendordeps/REVRobotics.json @@ -0,0 +1,64 @@ +{ + "fileName": "REVRobotics.json", + "name": "REVRobotics", + "version": "1.4.0", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-java", + "version": "1.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.4.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxathena", + "linuxraspbian" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-cpp", + "version": "1.4.0", + "libName": "libSparkMax", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxathena", + "linuxraspbian" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.4.0", + "libName": "libSparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file From ed31e7a6de19f4f7a69aaf850ebdb0f650784b14 Mon Sep 17 00:00:00 2001 From: connellsensational Date: Sat, 7 Sep 2019 12:01:06 -0400 Subject: [PATCH 3/9] added navx readings methods to drivesystem --- src/main/java/frc/robot/commands/LiftToBottom.java | 1 + src/main/java/frc/robot/subsystems/ClimbSystem.java | 2 +- src/main/java/frc/robot/subsystems/DriveSystem.java | 12 +++++++++++- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/LiftToBottom.java b/src/main/java/frc/robot/commands/LiftToBottom.java index abedca7..67e8bda 100644 --- a/src/main/java/frc/robot/commands/LiftToBottom.java +++ b/src/main/java/frc/robot/commands/LiftToBottom.java @@ -1,3 +1,4 @@ + /*----------------------------------------------------------------------------*/ /* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ diff --git a/src/main/java/frc/robot/subsystems/ClimbSystem.java b/src/main/java/frc/robot/subsystems/ClimbSystem.java index 472c135..6e560a1 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSystem.java @@ -84,7 +84,7 @@ public boolean isOut() { } public void extend(double WenchSpeed) { - double pitch = drive.getTilt(); + double pitch = drive.getPitch(); System.out.println("pitch"+pitch); //wench.set(ControlMode.PercentOutput, WenchSpeed); diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index 3b77ac7..e57ff9e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -227,9 +227,19 @@ public void resetGyro() { NavX.reset(); } - public double getTilt() { + // Gets pitch, yaw, and roll of robot using navx + public double getPitch() { + System.out.println("Pitch: " + NavX.getPitch()); return NavX.getPitch(); } + public double getYaw() { + System.out.println("Yaw: " + NavX.getYaw()); + return NavX.getYaw(); + } + public double getRoll() { + System.out.println("Roll: " + NavX.getRoll()); + return NavX.getRoll(); + } public boolean isInSlowMode() { From 128e9a715661935db0d73350df57188b190bf89b Mon Sep 17 00:00:00 2001 From: Trogdor14 Date: Tue, 24 Sep 2019 18:22:32 -0400 Subject: [PATCH 4/9] figuring out talon/spark stuff --- src/main/java/frc/robot/Robot.java | 2 - src/main/java/frc/robot/RobotMap.java | 12 ++-- .../frc/robot/commands/DriveWithJoystick.java | 4 +- .../frc/robot/subsystems/DriveSystem.java | 65 +++++++++++++------ vendordeps/REVRobotics.json | 10 +-- 5 files changed, 59 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bb7f200..8dd1fc5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -176,7 +176,6 @@ public void autonomousInit() { HatchGrab.start(); wristNow.start(); fistIntake.start(); - System.out.println("You mad bro?: "); /*m_autonomousCommand = m_chooser.getSelected(); @@ -227,7 +226,6 @@ public void teleopInit() { wristNow.start(); fistIntake.start(); wenchControl.start(); - System.out.println("You mad bro?: "); //HatchGrab.start(); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 9565b9b..fc8d142 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -48,12 +48,12 @@ public class RobotMap { /* Can addresses */ // Motors - public static final int DRV_LEFTLEAD = 0; - public static final int DRV_LEFTFOLLOW_1 = 1; - public static final int DRV_LEFTFOLLOW_2 = 2; - public static final int DRV_RIGHTLEAD = 3; - public static final int DRV_RIGHTFOLLOW_1 = 4; - public static final int DRV_RIGHTFOLLOW_2 = 5; + public static final int DRV_LEFTLEAD = 4; + public static final int DRV_LEFTFOLLOW_1 = 5; + public static final int DRV_LEFTFOLLOW_2 = 6; + public static final int DRV_RIGHTLEAD = 1; + public static final int DRV_RIGHTFOLLOW_1 = 2; + public static final int DRV_RIGHTFOLLOW_2 = 3; /* diff --git a/src/main/java/frc/robot/commands/DriveWithJoystick.java b/src/main/java/frc/robot/commands/DriveWithJoystick.java index 0424984..a9f7ccc 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc/robot/commands/DriveWithJoystick.java @@ -19,7 +19,7 @@ public class DriveWithJoystick extends Command { private double speed_y_left; private double speed_y_right; - private static final double DEADZONE = 0.2; + private static final double DEADZONE = 0.5; private OI oi; private DriveSystem Bob; @@ -108,7 +108,7 @@ private void tankDrive() Bob.drive(speed_y_left, speed_y_right); } else { - Bob.drive(0.0,0.0); + Bob.stopDrive(); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index e57ff9e..ec664e6 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -22,7 +22,8 @@ import frc.robot.RobotMap; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - +import com.revrobotics.CANSparkMaxLowLevel; +//import com.revrobotics.CANSparkMaxLowLevel.ConfigParameter; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -30,6 +31,8 @@ import com.revrobotics.ControlType; import frc.robot.RobotMap; + + public class DriveSystem extends Subsystem { //Smart Dashboard arcade mode chooser @@ -91,6 +94,8 @@ public DriveSystem() { leftPidController = leftLead.getPIDController(); rightPidController = rightLead.getPIDController(); + //leftLead.setParameter(4, 1); + // PID Coefficients kP = 4.8e-5; kI = 5.0e-7; @@ -127,21 +132,29 @@ public static DriveSystem getInstance() { } private void inititalizeDriveSystem() { + leftLead.restoreFactoryDefaults(); + leftFollow1.restoreFactoryDefaults(); + leftFollow2.restoreFactoryDefaults(); + + rightLead.restoreFactoryDefaults(); + rightFollow1.restoreFactoryDefaults(); + rightFollow2.restoreFactoryDefaults(); + // Setting the PID loop for the master controllers - rightPidController.setP(kP); + /*rightPidController.setP(kP); rightPidController.setI(kI); rightPidController.setD(kD); rightPidController.setIZone(kIz); rightPidController.setFF(kFF); - rightPidController.setOutputRange(kMinOutput, kMaxOutput); + //rightPidController.setOutputRange(kMinOutput, kMaxOutput); leftPidController.setP(kP); leftPidController.setI(kI); leftPidController.setD(kD); leftPidController.setIZone(kIz); leftPidController.setFF(kFF); - leftPidController.setOutputRange(kMinOutput, kMaxOutput); + //leftPidController.setOutputRange(kMinOutput, kMaxOutput);*/ leftLead.set(0.0); leftFollow1.set(0.0); @@ -161,43 +174,57 @@ private void inititalizeDriveSystem() { slow = false; turbo = false; + rightLead.setSmartCurrentLimit(30); + rightFollow1.setSmartCurrentLimit(30); + rightFollow2.setSmartCurrentLimit(30); + + leftLead.setSmartCurrentLimit(30); + leftFollow1.setSmartCurrentLimit(30); + leftFollow2.setSmartCurrentLimit(30); + rightLead.enableVoltageCompensation(12.0); + rightFollow1.enableVoltageCompensation(12.0); + rightFollow2.enableVoltageCompensation(12.0); + + leftLead.enableVoltageCompensation(12.0); + leftFollow1.enableVoltageCompensation(12.0); + leftFollow2.enableVoltageCompensation(12.0); } public void drive(double LeftSpeed, double RightSpeed) { setArcadeDrive(arcade_chooser.getSelected()); if (slow) { - System.out.println("Changing Speeds to Slow Speeds"); + //System.out.println("Changing Speeds to Slow Speeds"); LeftSpeed = LeftSpeed / SLOW_DOWN_SCALAR; RightSpeed = RightSpeed / SLOW_DOWN_SCALAR; }else if(turbo){ - System.out.println("Changing Speeds to turbo Speeds"); + //System.out.println("Changing Speeds to turbo Speeds"); LeftSpeed = LeftSpeed *1; RightSpeed = RightSpeed *1; }else { RightSpeed = RightSpeed * .8; LeftSpeed = LeftSpeed * .8; } - - rightLead.set(RightSpeed); - rightFollow1.set(RightSpeed); - rightFollow2.set(RightSpeed); + System.out.println("Right Speeds: " + RightSpeed + "\nLeft Speeds: " + LeftSpeed); + rightLead.set(0.3); + //rightFollow1.set(RightSpeed); + //rightFollow2.set(RightSpeed); leftLead.set(LeftSpeed); - leftFollow1.set(LeftSpeed); - leftFollow2.set(LeftSpeed); + //leftFollow1.set(LeftSpeed); + //leftFollow2.set(LeftSpeed); } public void stopDrive() { rightLead.set(0.0); - rightFollow1.set(0.0); - rightFollow2.set(0.0); + //rightFollow1.set(0.0); + //rightFollow2.set(0.0); leftLead.set(0.0); - leftFollow1.set(0.0); - leftFollow2.set(0.0); + //leftFollow1.set(0.0); + //leftFollow2.set(0.0); } public void driveSetSpeed(double LeftSpeed, double RightSpeed) { @@ -229,15 +256,15 @@ public void resetGyro() { // Gets pitch, yaw, and roll of robot using navx public double getPitch() { - System.out.println("Pitch: " + NavX.getPitch()); + //System.out.println("Pitch: " + NavX.getPitch()); return NavX.getPitch(); } public double getYaw() { - System.out.println("Yaw: " + NavX.getYaw()); + //System.out.println("Yaw: " + NavX.getYaw()); return NavX.getYaw(); } public double getRoll() { - System.out.println("Roll: " + NavX.getRoll()); + // System.out.println("Roll: " + NavX.getRoll()); return NavX.getRoll(); } diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json index 5a22a7a..8e700ff 100644 --- a/vendordeps/REVRobotics.json +++ b/vendordeps/REVRobotics.json @@ -1,7 +1,7 @@ { "fileName": "REVRobotics.json", "name": "REVRobotics", - "version": "1.4.0", + "version": "1.4.1", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "http://www.revrobotics.com/content/sw/max/sdk/maven/" @@ -11,14 +11,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-java", - "version": "1.4.0" + "version": "1.4.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-driver", - "version": "1.4.0", + "version": "1.4.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -33,7 +33,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-cpp", - "version": "1.4.0", + "version": "1.4.1", "libName": "libSparkMax", "headerClassifier": "headers", "sharedLibrary": false, @@ -48,7 +48,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-driver", - "version": "1.4.0", + "version": "1.4.1", "libName": "libSparkMaxDriver", "headerClassifier": "headers", "sharedLibrary": false, From 9533aa55c22bb83ba060118a6b285f843960d1a0 Mon Sep 17 00:00:00 2001 From: Trogdor14 Date: Sat, 28 Sep 2019 11:22:47 -0400 Subject: [PATCH 5/9] Correct sparky implementation --- .wpilib/wpilib_preferences.json | 2 +- src/main/java/frc/robot/OI.java | 11 +- src/main/java/frc/robot/Robot.java | 8 +- .../commands/ClimbCommands/DriveControl.java | 70 ---------- .../robot/commands/ClimbCommands/HookIn.java | 56 -------- .../robot/commands/ClimbCommands/HookOut.java | 67 ---------- .../commands/ClimbCommands/WenchControl.java | 63 --------- .../frc/robot/commands/DriveWithJoystick.java | 2 +- .../frc/robot/subsystems/ClimbSystem.java | 121 ------------------ .../frc/robot/subsystems/DriveSystem.java | 2 +- 10 files changed, 7 insertions(+), 395 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java delete mode 100644 src/main/java/frc/robot/commands/ClimbCommands/HookIn.java delete mode 100644 src/main/java/frc/robot/commands/ClimbCommands/HookOut.java delete mode 100644 src/main/java/frc/robot/commands/ClimbCommands/WenchControl.java delete mode 100644 src/main/java/frc/robot/subsystems/ClimbSystem.java diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 9025fdb..b989a57 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,5 +1,5 @@ { - "currentLanguage": "none", + "currentLanguage": "java", "enableCppIntellisense": false, "projectYear": "none", "teamNumber": 342 diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index c624324..43823a3 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -17,9 +17,6 @@ import frc.robot.commands.ToggleSlowDrive; import frc.robot.commands.WristToPosition; import frc.robot.commands.XboxRumble; -import frc.robot.commands.ClimbCommands.DriveControl; -import frc.robot.commands.ClimbCommands.HookIn; -import frc.robot.commands.ClimbCommands.HookOut; import frc.robot.commands.LiftToHeight.LiftHeight; import frc.robot.commands.LiftToHeightPID.LiftPosition; import frc.robot.commands.WristToPosition.WristPosition; @@ -94,14 +91,11 @@ public class OI { private Command liftToTop = new LiftToTop(); private Command liftToMiddle = new LiftToMiddle(); private Command liftToLow = new LiftToBottom(); - private Command driveControl = new DriveControl(); private Command engageOveride = new EngageOveride(); private Command HatchRelease = new HatchRelease(); - private Command hookOut = new HookOut(); - private Command hookIn = new HookIn(); - + private Command wristToPositionCargo = new WristToPosition(WristPosition.Cargo); @@ -200,9 +194,6 @@ private OI() { // xbox_drive_X.whileHeld(RotateToAngle270); // xbox_drive_Y.whileHeld(RotateToAngle0); - xbox_drive_startbutton.whileHeld(driveControl); - xbox_drive_backbutton.whenPressed(hookOut); - diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8dd1fc5..e389a37 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,7 +30,6 @@ import frc.robot.subsystems.DriveSystem; import edu.wpi.cscore.VideoMode.PixelFormat; import frc.robot.commands.FistIntake; -import frc.robot.commands.ClimbCommands.WenchControl; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.SPI; @@ -56,7 +55,7 @@ public class Robot extends TimedRobot { private LiftSystem lift; private DriveSystem drive; private Command fistIntake; - private Command wenchControl; + //private Command wenchControl; AHRS NavX; @@ -101,7 +100,6 @@ public void robotInit() { HatchGrab = new HatchGrab(); wristNow = new WristWithJoystick(); fistIntake = new FistIntake(); - wenchControl = new WenchControl(); lift.SetTrueZero(); //NavX = new AHRS(SPI.Port.kMXP); @@ -218,14 +216,14 @@ public void teleopInit() { * ((DriveWithJoystick) driveNow).setArcadeDrive(arcade_chooser.getSelected()); */ lift.resetHoldPosition(); - wenchControl.start(); + //wenchControl.start(); System.out.println("Starting Commands: "); driveNow.start(); liftNow.start(); HatchGrab.start(); wristNow.start(); fistIntake.start(); - wenchControl.start(); + //wenchControl.start(); //HatchGrab.start(); diff --git a/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java b/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java deleted file mode 100644 index 5759c8e..0000000 --- a/src/main/java/frc/robot/commands/ClimbCommands/DriveControl.java +++ /dev/null @@ -1,70 +0,0 @@ -//method in drive that sets the output on the wench wheel -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands.ClimbCommands; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.OI; -import frc.robot.Robot; -import frc.robot.subsystems.ClimbSystem; -import frc.robot.subsystems.DriveSystem; - -/** - * An example command. You can replace me with your own command. - */ -public class DriveControl extends Command { - - private ClimbSystem climb; - private OI oi; - private DriveSystem drive; - - private double driveSpeed; - - - public DriveControl() { - - climb = ClimbSystem.getInstance(); - oi = OI.getInstance(); - drive = DriveSystem.getInstance(); - - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - - - - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() { - - - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - end(); - } - -} diff --git a/src/main/java/frc/robot/commands/ClimbCommands/HookIn.java b/src/main/java/frc/robot/commands/ClimbCommands/HookIn.java deleted file mode 100644 index 9a1abb4..0000000 --- a/src/main/java/frc/robot/commands/ClimbCommands/HookIn.java +++ /dev/null @@ -1,56 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands.ClimbCommands; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; -import frc.robot.subsystems.ClimbSystem; - -/** - * An example command. You can replace me with your own command. - */ -public class HookIn extends Command { - - private ClimbSystem climb; - - public HookIn() { - - climb = ClimbSystem.getInstance(); - - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - // nothing should be here - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - @Override - protected void end() { - climb.hookIn(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/main/java/frc/robot/commands/ClimbCommands/HookOut.java b/src/main/java/frc/robot/commands/ClimbCommands/HookOut.java deleted file mode 100644 index 2a8ff0a..0000000 --- a/src/main/java/frc/robot/commands/ClimbCommands/HookOut.java +++ /dev/null @@ -1,67 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands.ClimbCommands; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.Robot; -import frc.robot.subsystems.ClimbSystem; -import frc.robot.subsystems.LiftSystem; -import edu.wpi.first.wpilibj.DigitalInput; - -/** - * An example command. You can replace me with your own command. - */ -public class HookOut extends Command { - - private ClimbSystem climb; - private LiftSystem lift; - - public HookOut() { - - climb = ClimbSystem.getInstance(); - lift = LiftSystem.getInstance(); - - - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - - //nothing should be here - - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - @Override - protected void end() { - - if (!lift.getBottomLimitSwitch()||lift.getOverride()){ - climb.hookOut(); - } - - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/main/java/frc/robot/commands/ClimbCommands/WenchControl.java b/src/main/java/frc/robot/commands/ClimbCommands/WenchControl.java deleted file mode 100644 index 3e916e0..0000000 --- a/src/main/java/frc/robot/commands/ClimbCommands/WenchControl.java +++ /dev/null @@ -1,63 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands.ClimbCommands; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.command.Command; -import frc.robot.OI; -import frc.robot.Robot; -import frc.robot.subsystems.ClimbSystem; -import frc.robot.subsystems.LiftSystem; - -/** - * An example command. You can replace me with your own command. - */ -public class WenchControl extends Command { - - private ClimbSystem climb; - private OI oi; - - private double WenchSpeed = 0; - - public WenchControl() { - climb = ClimbSystem.getInstance(); - oi = OI.getInstance(); - - } - - @Override - protected void initialize() { - } - - @Override - protected void execute() { - - WenchSpeed = oi.getCombinedXboxTriggers(); - - - climb.extend(WenchSpeed); - - } - - @Override - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() { - climb.stop(); - } - - @Override - protected void interrupted() { - end(); - } -} diff --git a/src/main/java/frc/robot/commands/DriveWithJoystick.java b/src/main/java/frc/robot/commands/DriveWithJoystick.java index a9f7ccc..2b4f9bb 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc/robot/commands/DriveWithJoystick.java @@ -19,7 +19,7 @@ public class DriveWithJoystick extends Command { private double speed_y_left; private double speed_y_right; - private static final double DEADZONE = 0.5; + private static final double DEADZONE = 0.2; private OI oi; private DriveSystem Bob; diff --git a/src/main/java/frc/robot/subsystems/ClimbSystem.java b/src/main/java/frc/robot/subsystems/ClimbSystem.java deleted file mode 100644 index 6e560a1..0000000 --- a/src/main/java/frc/robot/subsystems/ClimbSystem.java +++ /dev/null @@ -1,121 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.subsystems; - -import frc.robot.RobotMap; -import frc.robot.OI; - -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.wpilibj.DigitalInput; - -public class ClimbSystem extends Subsystem { - - private DoubleSolenoid pneumaticSuspension; - private static final ClimbSystem INSTANCE = new ClimbSystem(); - - private TalonSRX wench; - private DigitalInput limitSwitch3; - - private static final int AMPS = 20; - private static final int TIMEOUT_MS = 1; - private static final int ZERO = 0; - private static final double RAMP_TIME = 0.2; - - private OI oi; - private DriveSystem drive; - - public ClimbSystem() { - wench = new TalonSRX(RobotMap.CLIMB); - limitSwitch3 = new DigitalInput(RobotMap.CLIMB_LIMIT_SWITCH); - initializeClimbSystem(); - oi = OI.getInstance(); - drive = DriveSystem.getInstance(); - } - - @Override - public void initDefaultCommand() { - } - - public static ClimbSystem getInstance() { - return INSTANCE; - } - - public void initializeClimbSystem() { - pneumaticSuspension = new DoubleSolenoid(RobotMap.CAN_PCM, RobotMap.CLIMB_E, RobotMap.CLIMB_R); - - wench.configPeakCurrentLimit(ZERO, ZERO); - wench.configPeakCurrentDuration(ZERO, ZERO); - wench.configContinuousCurrentLimit(AMPS, TIMEOUT_MS); - wench.enableCurrentLimit(true); - - wench.configOpenloopRamp(RAMP_TIME, 0); - - wench.config_kP(0, TIMEOUT_MS); - wench.config_kI(0, TIMEOUT_MS); - wench.config_kD(0, TIMEOUT_MS); - wench.config_kF(0, TIMEOUT_MS); - - wench.set(ControlMode.PercentOutput, 0.0); - } - - public void hookIn() { - pneumaticSuspension.set(Value.kReverse); - } - - public void hookOut() { - pneumaticSuspension.set(Value.kForward); - } - - public boolean isOut() { - if (pneumaticSuspension.get().equals(Value.kForward)) { - return true; - } else { - return false; - } - } - - public void extend(double WenchSpeed) { - double pitch = drive.getPitch(); - System.out.println("pitch"+pitch); - //wench.set(ControlMode.PercentOutput, WenchSpeed); - - - if(limitSwitch3.get()){ - wench.set(ControlMode.PercentOutput, WenchSpeed); - } - - /* - if (pitch > 10.0) { - wench.set(ControlMode.PercentOutput, WenchSpeed); - } else if (pitch > -5) { - wench.set(ControlMode.PercentOutput, WenchSpeed * 0.5); - } else { - wench.set(ControlMode.PercentOutput, 0.0); - } - */ - - } - - public void extendSetSpeed(double Wench_Speed) { - - wench.set(ControlMode.Velocity, Wench_Speed); - - } - - public void stopExtend() { - wench.set(ControlMode.PercentOutput, 0.0); - } - - public void stop() { - wench.set(ControlMode.PercentOutput, 0.0); - } -} diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index ec664e6..9b47b74 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -206,7 +206,7 @@ public void drive(double LeftSpeed, double RightSpeed) { LeftSpeed = LeftSpeed * .8; } System.out.println("Right Speeds: " + RightSpeed + "\nLeft Speeds: " + LeftSpeed); - rightLead.set(0.3); + rightLead.set(RightSpeed); //rightFollow1.set(RightSpeed); //rightFollow2.set(RightSpeed); From cca5ee868feec4c87a5aff0fed769599a8719d13 Mon Sep 17 00:00:00 2001 From: Elizabeth O'Neill Date: Thu, 17 Oct 2019 18:10:00 -0400 Subject: [PATCH 6/9] Changed Speeds --- src/main/java/frc/robot/OI.java | 2 +- src/main/java/frc/robot/commands/HatchGrab.java | 4 ++-- .../frc/robot/commands/PneumaticsWithCANifier.java | 4 ++-- src/main/java/frc/robot/subsystems/DriveSystem.java | 12 ++++++------ 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 43823a3..9a2c896 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -184,7 +184,7 @@ private OI() { - xbox_drive_leftBumper.whileHeld(turboDrive); + //xbox_drive_leftBumper.whileHeld(turboDrive); //xbox_drive_B.whileHeld(driveControl); // xbox_drive_Y.whileHeld(hookOut); //xbox_drive_A.whileHeld(lowerWithPneumatics); diff --git a/src/main/java/frc/robot/commands/HatchGrab.java b/src/main/java/frc/robot/commands/HatchGrab.java index e6a46b4..245206e 100644 --- a/src/main/java/frc/robot/commands/HatchGrab.java +++ b/src/main/java/frc/robot/commands/HatchGrab.java @@ -51,8 +51,8 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - // System.out.println("Checking: " + - // canifierLimits.getGeneralInput(GeneralPin.LIMF)); + //System.out.println("Checking1: " + canifierLimits.getGeneralInput(GeneralPin.LIMF)); + // System.out.println("Checking2: " + canifierLimits.getGeneralInput(GeneralPin.LIMR)); // System.out.println("pigeon testing:"); // System.out.println("\t compasheading :" + pigeon.getCompassHeading()); // System.out.println("\t accelorometer data :"); diff --git a/src/main/java/frc/robot/commands/PneumaticsWithCANifier.java b/src/main/java/frc/robot/commands/PneumaticsWithCANifier.java index 9cca5cf..f5c6255 100644 --- a/src/main/java/frc/robot/commands/PneumaticsWithCANifier.java +++ b/src/main/java/frc/robot/commands/PneumaticsWithCANifier.java @@ -40,8 +40,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - // System.out.println("Checking: " + - // canifierLimits.getGeneralInput(GeneralPin.LIMF)); + // System.out.println("pigeon testing:"); // System.out.println("\t compasheading :" + pigeon.getCompassHeading()); // System.out.println("\t accelorometer data :"); @@ -51,6 +50,7 @@ protected void execute() { // accelerometer[1] + "\t z: " + accelerometer[2]); // System.out.println("Limit Switch Test"); + if (!canifierLimits.getGeneralInput(GeneralPin.LIMF) && !canifierLimits.getGeneralInput(GeneralPin.LIMR)) { Cylinder.pneumaticOut(); // System.out.println("One of the two limit switches are being pressed"); diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index 9b47b74..08913b1 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -195,15 +195,15 @@ public void drive(double LeftSpeed, double RightSpeed) { setArcadeDrive(arcade_chooser.getSelected()); if (slow) { //System.out.println("Changing Speeds to Slow Speeds"); - LeftSpeed = LeftSpeed / SLOW_DOWN_SCALAR; - RightSpeed = RightSpeed / SLOW_DOWN_SCALAR; + LeftSpeed = (LeftSpeed * .3) / SLOW_DOWN_SCALAR; + RightSpeed = (RightSpeed * .3) / SLOW_DOWN_SCALAR; }else if(turbo){ //System.out.println("Changing Speeds to turbo Speeds"); - LeftSpeed = LeftSpeed *1; - RightSpeed = RightSpeed *1; + LeftSpeed = LeftSpeed *.8; + RightSpeed = RightSpeed *.8; }else { - RightSpeed = RightSpeed * .8; - LeftSpeed = LeftSpeed * .8; + RightSpeed = RightSpeed * .3; + LeftSpeed = LeftSpeed * .3; } System.out.println("Right Speeds: " + RightSpeed + "\nLeft Speeds: " + LeftSpeed); rightLead.set(RightSpeed); From 2e24c65ffbf1a0d480c26c574df8fc2db3f5464e Mon Sep 17 00:00:00 2001 From: Elizabeth O'Neill Date: Sat, 19 Oct 2019 12:11:36 -0400 Subject: [PATCH 7/9] Added Slow/Turbo Indicators and Commented Levels --- src/main/java/frc/robot/OI.java | 8 +- .../frc/robot/subsystems/DriveSystem.java | 6 ++ vendordeps/REVRobotics.json | 78 +++++++++---------- 3 files changed, 49 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 9a2c896..d1c96b2 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -207,10 +207,10 @@ private OI() { - logitech_manipulator_A.whileHeld(liftToLow); - logitech_manipulator_B.whileHeld(liftToMiddle); - logitech_manipulator_Y.whileHeld(liftToTop); - logitech_manipulator_X.whileHeld(liftToHeightPIDLowHatch); + //logitech_manipulator_A.whileHeld(liftToLow); + //logitech_manipulator_B.whileHeld(liftToMiddle); + //logitech_manipulator_Y.whileHeld(liftToTop); + //logitech_manipulator_X.whileHeld(liftToHeightPIDLowHatch); logitech_manipulator_leftBumper.whenPressed(toggleSlowDrive); logitech_manipulator_rightBumper.whileHeld(HatchRelease); diff --git a/src/main/java/frc/robot/subsystems/DriveSystem.java b/src/main/java/frc/robot/subsystems/DriveSystem.java index 08913b1..d8764ab 100644 --- a/src/main/java/frc/robot/subsystems/DriveSystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSystem.java @@ -195,13 +195,19 @@ public void drive(double LeftSpeed, double RightSpeed) { setArcadeDrive(arcade_chooser.getSelected()); if (slow) { //System.out.println("Changing Speeds to Slow Speeds"); + SmartDashboard.putBoolean("Slow Mode: ", true); + SmartDashboard.putBoolean("Turbo: ", false); LeftSpeed = (LeftSpeed * .3) / SLOW_DOWN_SCALAR; RightSpeed = (RightSpeed * .3) / SLOW_DOWN_SCALAR; }else if(turbo){ //System.out.println("Changing Speeds to turbo Speeds"); + SmartDashboard.putBoolean("Slow Mode: ", false); + SmartDashboard.putBoolean("Turbo: ", true); LeftSpeed = LeftSpeed *.8; RightSpeed = RightSpeed *.8; }else { + SmartDashboard.putBoolean("Slow Mode: ", false); + SmartDashboard.putBoolean("Turbo: ", false); RightSpeed = RightSpeed * .3; LeftSpeed = LeftSpeed * .3; } diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json index 8e700ff..4ae8d70 100644 --- a/vendordeps/REVRobotics.json +++ b/vendordeps/REVRobotics.json @@ -1,64 +1,64 @@ { - "fileName": "REVRobotics.json", - "name": "REVRobotics", - "version": "1.4.1", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "http://www.revrobotics.com/content/sw/max/sdk/maven/" - ], - "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-java", - "version": "1.4.1" - } - ], - "jniDependencies": [ + "cppDependencies": [ { - "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-driver", - "version": "1.4.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ + "artifactId": "SparkMax-cpp", + "binaryPlatforms": [ "windowsx86-64", "windowsx86", "linuxathena", "linuxraspbian" - ] - } - ], - "cppDependencies": [ - { + ], "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-cpp", - "version": "1.4.1", - "libName": "libSparkMax", "headerClassifier": "headers", + "libName": "libSparkMax", "sharedLibrary": false, "skipInvalidPlatforms": true, + "version": "1.4.1" + }, + { + "artifactId": "SparkMax-driver", "binaryPlatforms": [ "windowsx86-64", "windowsx86", "linuxathena", "linuxraspbian" - ] - }, - { + ], "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-driver", - "version": "1.4.1", - "libName": "libSparkMaxDriver", "headerClassifier": "headers", + "libName": "libSparkMaxDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "binaryPlatforms": [ + "version": "1.4.1" + } + ], + "fileName": "REVRobotics.json", + "javaDependencies": [ + { + "artifactId": "SparkMax-java", + "groupId": "com.revrobotics.frc", + "version": "1.4.1" + } + ], + "jniDependencies": [ + { + "artifactId": "SparkMax-driver", + "groupId": "com.revrobotics.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ "windowsx86-64", "windowsx86", "linuxathena", "linuxraspbian" - ] + ], + "version": "1.4.1" } - ] + ], + "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", + "mavenUrls": [ + "http://www.revrobotics.com/content/sw/max/sdk/maven/" + ], + "name": "REVRobotics", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "version": "1.4.1" } \ No newline at end of file From d133da8638bc46a9833aab21c12afd221ca91653 Mon Sep 17 00:00:00 2001 From: connellsensational Date: Mon, 21 Oct 2019 17:13:55 -0400 Subject: [PATCH 8/9] Disabled lift to position buttons for SCRIW --- src/main/java/frc/robot/OI.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index d1c96b2..672cd05 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -44,7 +44,7 @@ import frc.robot.commands.EngageOveride; -import frc.robot.commands.TurboDrive;; +import frc.robot.commands.TurboDrive; /** * This class is the glue that binds the controls on the physical operator @@ -86,11 +86,11 @@ public class OI { private Command xboxRumble = new XboxRumble(); - -private Command liftToHeightPIDLowHatch = new LiftToHeightPID(LiftPosition.HatchLowRocket); - private Command liftToTop = new LiftToTop(); - private Command liftToMiddle = new LiftToMiddle(); - private Command liftToLow = new LiftToBottom(); + // Disabled lift to height buttons for SCRIW because of possibility of robot tipping + // private Command liftToHeightPIDLowHatch = new LiftToHeightPID(LiftPosition.HatchLowRocket); + // private Command liftToTop = new LiftToTop(); + // private Command liftToMiddle = new LiftToMiddle(); + // private Command liftToLow = new LiftToBottom(); private Command engageOveride = new EngageOveride(); From 6d101a9b524a79967402353969907a8978a0fa9c Mon Sep 17 00:00:00 2001 From: Aleric Date: Mon, 21 Oct 2019 19:14:59 -0400 Subject: [PATCH 9/9] Tuning PID loop for arm --- .../java/frc/robot/subsystems/LiftSystem.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LiftSystem.java b/src/main/java/frc/robot/subsystems/LiftSystem.java index c17e758..b121770 100644 --- a/src/main/java/frc/robot/subsystems/LiftSystem.java +++ b/src/main/java/frc/robot/subsystems/LiftSystem.java @@ -103,19 +103,19 @@ private void initializeLiftSystem() { liftWrist.enableCurrentLimit(true); // Setting the PID loop for the master controllers - liftMaster.config_kP(0,1, TIMEOUT_MS); - liftMaster.config_kI(0,0.002, TIMEOUT_MS); - liftMaster.config_kD(0,0.1, TIMEOUT_MS); - liftMaster.config_kF(0,0.06, TIMEOUT_MS); - liftMaster.configAllowableClosedloopError(1, 1, 10); - liftMaster.configAllowableClosedloopError(0, 1, 10); + liftMaster.config_kP(0, 0.1, TIMEOUT_MS); + liftMaster.config_kI(0, 0.001, TIMEOUT_MS); + liftMaster.config_kD(0, 0.1, TIMEOUT_MS); + liftMaster.config_kF(0, 0.06, TIMEOUT_MS); + liftMaster.configAllowableClosedloopError(1, 1, 2); + liftMaster.configAllowableClosedloopError(0, 1, 2); liftMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); - liftFollow.configAllowableClosedloopError(1, 1, 10); - liftFollow.configAllowableClosedloopError(0, 1, 10); - liftFollow.config_kP(0,1, TIMEOUT_MS); - liftFollow.config_kI(0,0.002, TIMEOUT_MS); - liftFollow.config_kD(0,0.1, TIMEOUT_MS); - liftFollow.config_kF(0,0.06, TIMEOUT_MS); + liftFollow.configAllowableClosedloopError(1, 1, 2); + liftFollow.configAllowableClosedloopError(0, 1, 2); + liftFollow.config_kP(0, 0.1, TIMEOUT_MS); + liftFollow.config_kI(0, 0.001, TIMEOUT_MS); + liftFollow.config_kD(0, 0.1, TIMEOUT_MS); + liftFollow.config_kF(0, 0.06, TIMEOUT_MS); }