From f098d73645c7af4f72af13c23757db66b168e940 Mon Sep 17 00:00:00 2001 From: Mz Date: Mon, 26 Feb 2024 17:24:16 -0600 Subject: [PATCH] test auto and removed voltagecompensation --- .pathplanner/settings.json | 8 +-- .../pathplanner/autos/SimpleStraight.auto | 25 +++++++++ src/main/deploy/pathplanner/paths/1Meter.path | 52 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../armElevator/ArmElevatorIOSparkMax.java | 2 - .../subsystems/climber/ClimberIOSparkMax.java | 3 -- .../subsystems/intake/IntakeIOSparkMax.java | 2 - .../subsystems/sticks/SticksIOSparkMax.java | 2 - .../subsystems/wrist/WristIOSparkMax.java | 1 - 9 files changed, 83 insertions(+), 14 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/SimpleStraight.auto create mode 100644 src/main/deploy/pathplanner/paths/1Meter.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 3ccaece..bf50adf 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -3,17 +3,19 @@ "robotLength": 0.826, "holonomicMode": true, "pathFolders": [ - "SpinShoot", + "TestPaths", "Plans", "PointPaths", + "SpinShoot", "StartPaths" ], "autoFolders": [ - "Blue" + "Blue", + "TestAutos" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SimpleStraight.auto b/src/main/deploy/pathplanner/autos/SimpleStraight.auto new file mode 100644 index 0000000..169c829 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SimpleStraight.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1Meter" + } + } + ] + } + }, + "folder": "TestAutos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1Meter.path b/src/main/deploy/pathplanner/paths/1Meter.path new file mode 100644 index 0000000..cec66b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1Meter.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 7.0 + }, + "prevControl": { + "x": 2.0, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "TestPaths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ec472b..27b27d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -195,7 +195,7 @@ public RobotContainer() { SendableChooser chooser = new SendableChooser<>(); // ! Add new autos here. The autogenerated one is bad because it includes all of the ones in // the chache of the RoboRIO. - String[] autoNames = new String[] {"BlueLClose3" /*, "BlueLMid3", "BlueLClose3Far1"*/}; + String[] autoNames = new String[] {"BlueLClose3", "SimpleStraight" /*, "BlueLMid3", "BlueLClose3Far1"*/}; chooser.setDefaultOption("None", Commands.none()); List options = new ArrayList<>(); diff --git a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java index 8336593..93dda4a 100644 --- a/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/armElevator/ArmElevatorIOSparkMax.java @@ -45,8 +45,6 @@ public ArmElevatorIOSparkMax() { motor.setInverted(true); - // motor.enableVoltageCompensation(12.0); - motor.setSmartCurrentLimit(ArmElevatorConstants.kCurrentLimit); // pid.setOutputRange(0, ArmElevatorConstants.maxExtension); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java index 48dff7b..40475ab 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java @@ -53,9 +53,6 @@ public ClimberIOSparkMax() { leftMotor.setInverted(true); - leftMotor.enableVoltageCompensation(12.0); - rightMotor.enableVoltageCompensation(12.0); - leftMotor.setSmartCurrentLimit(ClimberConstants.kCurrentLimit); rightMotor.setSmartCurrentLimit(ClimberConstants.kCurrentLimit); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index c899ed3..e02e1e2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -46,9 +46,7 @@ public IntakeIOSparkMax() { intakeMotor.setInverted(false); - intakeMotor.enableVoltageCompensation(12.0); intakeMotor.setSmartCurrentLimit(30); - followerIntakeMotor.enableVoltageCompensation(12.0); followerIntakeMotor.setSmartCurrentLimit(30); followerIntakeMotor.follow(intakeMotor, false); diff --git a/src/main/java/frc/robot/subsystems/sticks/SticksIOSparkMax.java b/src/main/java/frc/robot/subsystems/sticks/SticksIOSparkMax.java index 260df70..b66d4f8 100644 --- a/src/main/java/frc/robot/subsystems/sticks/SticksIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/sticks/SticksIOSparkMax.java @@ -42,8 +42,6 @@ public SticksIOSparkMax() { // encoder.setPosition(SticksConstants.initialAngle); - // motor.enableVoltageCompensation(12.0); - motor.setSmartCurrentLimit(SticksConstants.kCurrentLimit); // motor.burnFlash(); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java index 54c3b6d..3551bbd 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSparkMax.java @@ -42,7 +42,6 @@ public WristIOSparkMax() { // encoder.setPosition(WristConstants.initialAngle); - // motor.enableVoltageCompensation(12.0); motor.setSmartCurrentLimit(WristConstants.kCurrentLimit);