auto align setpoints
This commit is contained in:
parent
d934cdf35b
commit
2990b917e7
@ -16,12 +16,12 @@
|
|||||||
},
|
},
|
||||||
{
|
{
|
||||||
"anchor": {
|
"anchor": {
|
||||||
"x": 3.4559075342465753,
|
"x": 12.272,
|
||||||
"y": 3.123458904109589
|
"y": 2.975
|
||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 2.4559075342465753,
|
"x": 10.940715672291898,
|
||||||
"y": 3.123458904109589
|
"y": 2.975
|
||||||
},
|
},
|
||||||
"nextControl": null,
|
"nextControl": null,
|
||||||
"isLocked": false,
|
"isLocked": false,
|
||||||
@ -30,7 +30,18 @@
|
|||||||
],
|
],
|
||||||
"rotationTargets": [],
|
"rotationTargets": [],
|
||||||
"constraintZones": [],
|
"constraintZones": [],
|
||||||
"pointTowardsZones": [],
|
"pointTowardsZones": [
|
||||||
|
{
|
||||||
|
"fieldPosition": {
|
||||||
|
"x": 0.4,
|
||||||
|
"y": 5.5
|
||||||
|
},
|
||||||
|
"rotationOffset": 0.0,
|
||||||
|
"minWaypointRelativePos": 0.15,
|
||||||
|
"maxWaypointRelativePos": 0.4,
|
||||||
|
"name": "Point Towards Zone"
|
||||||
|
}
|
||||||
|
],
|
||||||
"eventMarkers": [],
|
"eventMarkers": [],
|
||||||
"globalConstraints": {
|
"globalConstraints": {
|
||||||
"maxVelocity": 4.0,
|
"maxVelocity": 4.0,
|
||||||
@ -42,7 +53,7 @@
|
|||||||
},
|
},
|
||||||
"goalEndState": {
|
"goalEndState": {
|
||||||
"velocity": 0,
|
"velocity": 0,
|
||||||
"rotation": 57.80426606528677
|
"rotation": 59.99999999999999
|
||||||
},
|
},
|
||||||
"reversed": false,
|
"reversed": false,
|
||||||
"folder": null,
|
"folder": null,
|
||||||
|
@ -9,7 +9,7 @@
|
|||||||
"defaultMaxAngVel": 540.0,
|
"defaultMaxAngVel": 540.0,
|
||||||
"defaultMaxAngAccel": 720.0,
|
"defaultMaxAngAccel": 720.0,
|
||||||
"defaultNominalVoltage": 12.0,
|
"defaultNominalVoltage": 12.0,
|
||||||
"robotMass": 48.35,
|
"robotMass": 53.0,
|
||||||
"robotMOI": 6.883,
|
"robotMOI": 6.883,
|
||||||
"robotTrackwidth": 0.546,
|
"robotTrackwidth": 0.546,
|
||||||
"driveWheelRadius": 0.038,
|
"driveWheelRadius": 0.038,
|
||||||
|
@ -65,12 +65,6 @@ public class RobotContainer {
|
|||||||
|
|
||||||
manipulatorPivot = new ManipulatorPivot();
|
manipulatorPivot = new ManipulatorPivot();
|
||||||
|
|
||||||
//commands for pathplanner
|
|
||||||
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
|
|
||||||
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
|
|
||||||
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
|
||||||
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
|
|
||||||
|
|
||||||
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
||||||
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
||||||
|
|
||||||
@ -147,7 +141,7 @@ public class RobotContainer {
|
|||||||
);
|
);
|
||||||
|
|
||||||
driver.leftTrigger().whileTrue(
|
driver.leftTrigger().whileTrue(
|
||||||
manipulator.runUntilCollected(() -> 0.35)
|
manipulator.runUntilCollected(() -> 0.35).andThen(manipulator.retractCommand(() -> 0.35))
|
||||||
);
|
);
|
||||||
|
|
||||||
driver.start().and(driver.back()).onTrue(
|
driver.start().and(driver.back()).onTrue(
|
||||||
@ -210,6 +204,10 @@ public class RobotContainer {
|
|||||||
|
|
||||||
private void configureNamedCommands() {
|
private void configureNamedCommands() {
|
||||||
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
|
||||||
|
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
|
||||||
|
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
|
||||||
|
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
|
||||||
|
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
|
||||||
}
|
}
|
||||||
|
|
||||||
//creates tabs and transforms them on the shuffleboard
|
//creates tabs and transforms them on the shuffleboard
|
||||||
|
@ -1,6 +1,9 @@
|
|||||||
package frc.robot.constants;
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
import java.util.HashMap;
|
||||||
|
|
||||||
public class VisionConstants {
|
public class VisionConstants {
|
||||||
|
|
||||||
// global coordinate map of all tags. index is the tag id.
|
// global coordinate map of all tags. index is the tag id.
|
||||||
// Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
|
// Units: inches and degrees. {x, y, z, z-rotation, y-rotation}
|
||||||
// This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
|
// This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS.
|
||||||
@ -28,4 +31,26 @@ public class VisionConstants {
|
|||||||
{209.49, 158.3, 12.13, 0, 0},
|
{209.49, 158.3, 12.13, 0, 0},
|
||||||
{193.1, 129.97, 12.13, 300, 0},
|
{193.1, 129.97, 12.13, 300, 0},
|
||||||
};
|
};
|
||||||
|
|
||||||
|
//map of coral placing setpoints based on the tag that is on the same reef face
|
||||||
|
// and the on the left or right branch of that side of the reef
|
||||||
|
// <tag_number, {left_x, left_y, right_x, right_y}>
|
||||||
|
public HashMap<Integer, double[]> blueReefMap = new HashMap<>();
|
||||||
|
public HashMap<Integer, double[]> redReefMap = new HashMap<>();
|
||||||
|
|
||||||
|
VisionConstants(){
|
||||||
|
blueReefMap.put(17, new double[] {3.703, 3.975, 3.982, 2.806});
|
||||||
|
blueReefMap.put(18, new double[] {3.183, 4.191, 3.183, 3.857});
|
||||||
|
blueReefMap.put(19, new double[] {3.986, 5.24, 3.701, 5.076});
|
||||||
|
blueReefMap.put(20, new double[] {5.275, 5.075, 4.991, 5.246});
|
||||||
|
blueReefMap.put(21, new double[] {5.789, 3.862, 5.789, 4.194});
|
||||||
|
blueReefMap.put(22, new double[] {4.993, 2.816, 5.272, 2.996});
|
||||||
|
|
||||||
|
blueReefMap.put(11, new double[] {3.703+12.272, 3.975, 3.982+12.272, 2.806});
|
||||||
|
blueReefMap.put(10, new double[] {3.183+12.272, 4.191, 3.183, 3.857});
|
||||||
|
blueReefMap.put(9, new double[] {3.986+12.272, 5.24, 3.701+12.272, 5.076});
|
||||||
|
blueReefMap.put(8, new double[] {5.275+12.272, 5.075, 4.991+12.272, 5.246});
|
||||||
|
blueReefMap.put(7, new double[] {5.789+12.272, 3.862, 5.789+12.272, 4.194});
|
||||||
|
blueReefMap.put(6, new double[] {4.993+12.272, 2.816, 5.272+12.272, 2.996});
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
@ -74,6 +74,13 @@ public class Manipulator extends SubsystemBase {
|
|||||||
}).until(() -> !coralBeamBreak.get());
|
}).until(() -> !coralBeamBreak.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public Command retractCommand(DoubleSupplier speed){
|
||||||
|
return run(() -> {
|
||||||
|
manipulatorMotor.set(-speed.getAsDouble());
|
||||||
|
}
|
||||||
|
).until(() -> coralBeamBreak.get());
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Runs the manipulator in a way that will bring the coral to a reliable holding position
|
* Runs the manipulator in a way that will bring the coral to a reliable holding position
|
||||||
*
|
*
|
||||||
|
Loading…
Reference in New Issue
Block a user