Compare commits

..

No commits in common. "339bf642a1aa7b8e1f30f2a2e4368b2dd047e99f" and "83db16794f0aab84892c69db5f743256a4ce40c5" have entirely different histories.

7 changed files with 40 additions and 40 deletions

View File

@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 1.2587090163934425, "x": 1.0549180327868852,
"y": 7.08186475409836 "y": 7.285655737704918
}, },
"prevControl": { "prevControl": {
"x": 2.115173057489333, "x": 1.9113820738827756,
"y": 6.135246603413428 "y": 6.339037587019986
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -34,7 +34,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 5.0, "maxVelocity": 5.0,
"maxAcceleration": 2.0, "maxAcceleration": 2.5,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

View File

@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.2587090163934425, "x": 1.0549180327868852,
"y": 7.08186475409836 "y": 7.285655737704918
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 2.550917920503032, "x": 2.3471269368964744,
"y": 6.856479480125757 "y": 7.060270463732315
}, },
"isLocked": false, "isLocked": false,
"linkedName": "HP Left Position" "linkedName": "HP Left Position"
}, },
{ {
"anchor": { "anchor": {
"x": 3.6442622950819668, "x": 3.5723360655737704,
"y": 5.031967213114754 "y": 4.996004098360656
}, },
"prevControl": { "prevControl": {
"x": 3.328722911520323, "x": 3.256796682012127,
"y": 5.557866185717494 "y": 5.521903070963395
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -46,7 +46,7 @@
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.5, "maxVelocity": 3.5,
"maxAcceleration": 1.5, "maxAcceleration": 1.25,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0, "maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,

View File

@ -3,13 +3,13 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.2587090163934425, "x": 1.0549180327868852,
"y": 7.08186475409836 "y": 7.285655737704918
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 1.7741803278688524, "x": 1.5703893442622952,
"y": 6.530430327868851 "y": 6.734221311475409
}, },
"isLocked": false, "isLocked": false,
"linkedName": "HP Left Position" "linkedName": "HP Left Position"

View File

@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 3.6442622950819668, "x": 3.5723360655737704,
"y": 5.031967213114754 "y": 4.996004098360656
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 3.5117625600811717, "x": 3.4398363305729753,
"y": 5.243966789116026 "y": 5.208003674361928
}, },
"isLocked": false, "isLocked": false,
"linkedName": "L" "linkedName": "L"
}, },
{ {
"anchor": { "anchor": {
"x": 1.2587090163934425, "x": 1.0549180327868852,
"y": 7.08186475409836 "y": 7.285655737704918
}, },
"prevControl": { "prevControl": {
"x": 1.4625000000000001, "x": 1.258709016393443,
"y": 6.8061475409836065 "y": 7.009938524590164
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,

View File

@ -7,7 +7,7 @@ package frc.robot;
import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger;
//import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.littletonrobotics.junction.wpilog.WPILOGWriter;
@ -35,7 +35,7 @@ public Robot() {
if (isReal()) { if (isReal()) {
Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
//Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
} else { } else {
setUseTiming(false); // Run as fast as possible setUseTiming(false); // Run as fast as possible

View File

@ -226,8 +226,8 @@ public class RobotContainer {
operator.back().onTrue(elevator.homeCommand()); operator.back().onTrue(elevator.homeCommand());
operator.start().toggleOnTrue( operator.start().toggleOnTrue(
climberPivot.runPivot(() -> -operator.getRightY()) climberPivot.runPivot(() -> -operator.getRightY()*0.5)
.alongWith(climberRollers.runRoller(() -> operator.getLeftY()))); .alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.a().onTrue( operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition) safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)

View File

@ -180,13 +180,13 @@ public class Drivetrain extends SubsystemBase {
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){ if(vision.getOrangeDist() < 60 && getVelocity() < 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){ }else if(vision.getOrangeDist() < 100 && getVelocity() < 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){ }else if(vision.getOrangeDist() < 60 && getVelocity() > 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360)));
}else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){ }else if(vision.getOrangeDist() < 100 && getVelocity() > 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
} }
@ -206,13 +206,13 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){ if(vision.getBlackDist() < 60 && getVelocity() < 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){ }else if(vision.getBlackDist() < 100 && getVelocity() < 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){ }else if(vision.getBlackDist() < 60 && getVelocity() > 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360)));
}else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){ }else if(vision.getBlackDist() < 100 && getVelocity() > 2){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
} }