Skip to content

Commit

Permalink
After 1619
Browse files Browse the repository at this point in the history
  • Loading branch information
@zaneal
zaneal committed Oct 1, 2023
1 parent b3fb0f5 commit 54383d3
Show file tree
Hide file tree
Showing 12 changed files with 112 additions and 53 deletions.
7 changes: 4 additions & 3 deletions src/main/deploy/pathplanner/Blue Bottom 1 Balance.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,12 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidPlace",
"HighPlace",
"Base"
],
"executionBehavior": "sequential",
"waitBehavior": "after",
"waitTime": 0.01
"waitTime": 0.05
}
},
{
Expand Down Expand Up @@ -92,7 +92,8 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"Balance"
"Balance",
"Base"
],
"executionBehavior": "parallel",
"waitBehavior": "none",
Expand Down
9 changes: 5 additions & 4 deletions src/main/deploy/pathplanner/Blue Top 1 Balance.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,12 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidPlace",
"HighPlace",
"Base"
],
"executionBehavior": "sequential",
"waitBehavior": "after",
"waitTime": 0.01
"waitTime": 0.05
}
},
{
Expand Down Expand Up @@ -92,9 +92,10 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"Balance"
"Balance",
"Base"
],
"executionBehavior": "sequential",
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0.01
}
Expand Down
14 changes: 9 additions & 5 deletions src/main/deploy/pathplanner/Red Bottom 1 Balance.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,12 @@
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"names": [
"HighPlace",
"Base2"
],
"executionBehavior": "sequential",
"waitBehavior": "after",
"waitTime": 0.01
}
},
Expand Down Expand Up @@ -89,10 +92,11 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"Balance"
"Balance",
"Base"
],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitBehavior": "after",
"waitTime": 0.01
}
}
Expand Down
17 changes: 9 additions & 8 deletions src/main/deploy/pathplanner/Red Top 1 Balance.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidPlace",
"Base"
"HighPlace",
"Base2"
],
"executionBehavior": "sequential",
"waitBehavior": "after",
Expand Down Expand Up @@ -79,12 +79,12 @@
},
{
"anchorPoint": {
"x": 12.729878838229899,
"y": 2.7803488714616322
"x": 12.769252665241442,
"y": 2.7737962316202402
},
"prevControl": {
"x": 12.713787178442411,
"y": 2.7840311071247146
"x": 12.753161005453954,
"y": 2.7774784672833226
},
"nextControl": null,
"holonomicAngle": 180.0,
Expand All @@ -94,9 +94,10 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"Balance"
"Balance",
"Base2"
],
"executionBehavior": "sequential",
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/generatedJSON/New Path.wpilib.json
View file

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/generatedJSON/Red Top 1 Balance.wpilib.json
View file

Large diffs are not rendered by default.

20 changes: 18 additions & 2 deletions src/main/kotlin/frc/robot/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ class RobotContainer {

private fun Discovery() {

JoystickButton(primaryController, XboxController.Button.kRightBumper.value).whileTrue(
JoystickButton(primaryController, XboxController.Button.kLeftBumper.value).whileTrue(
StandardDrive(swerveSubsystem,
{ primaryController.leftY * DrivetrainConstants.drivingSpeedScalar / 2.0 },
{ primaryController.leftX * DrivetrainConstants.drivingSpeedScalar / 2.0 },
Expand Down Expand Up @@ -359,7 +359,7 @@ class RobotContainer {
)
)
autoCommandChooser.addOption(
"Just Place Cube Moid",
"Just Place Cube Mid",
SequentialCommandGroup(
AutoPlaceCubeMid(pickAndPlace).withTimeout(4.0),
AutoBase(pickAndPlace)
Expand Down Expand Up @@ -388,6 +388,22 @@ class RobotContainer {
"Blue Right Place Cube High Leave",
testTrajectories.BlueBottom1()
)
autoCommandChooser.addOption(
"Red Left Cone Right Place Leave",
testTrajectories.RedLeftConeRightPlaceLeave()
)
autoCommandChooser.addOption(
"Red Left Cone Left Place Leave",
testTrajectories.RedLeftConeLeftPlaceLeave()
)
autoCommandChooser.addOption(
"Red Right Cone Right Place Leave",
testTrajectories.RedRightConeRightPlaceLeave()
)
autoCommandChooser.addOption(
"Red Right Cone Left Place Leave",
testTrajectories.RedRightConeLeftPlaceLeave()
)
autoCommandChooser.addOption(
"Red Left Place Cube High Leave and Balance",
testTrajectories.RedBottom1Balance()
Expand Down
37 changes: 31 additions & 6 deletions src/main/kotlin/frc/robot/commands/AutoTrajectories.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ class AutoTrajectories(val pnp: PickAndPlaceSubsystem, val swerveSubsystem: Swer
val pathGroup: List<PathPlannerTrajectory> = PathPlanner.loadPathGroup(
pathName, firstConstraint, *pathConstraints
)
println("Auto Running")
//println("Auto Running")
//It might be this for the line above: val pathGroup: ArrayList<PathPlannerTrajectory> = arrayListOf()
// PathPlanner.loadPathGroup("Red Top 1 Get Balance", PathConstraints(1.0, 0.5)).toCollection(pathGroup)

Expand All @@ -35,6 +35,7 @@ class AutoTrajectories(val pnp: PickAndPlaceSubsystem, val swerveSubsystem: Swer
"PickUpCube" to AutoPickCube(pnp),
"HighPlace" to AutoPlaceCubeHigh(pnp),
"MidCone" to AutoPlaceConeMid(pnp),
"Base2" to AutoBase2(pnp),
"Balance" to Balancer(swerveSubsystem)
)
// val eventMap = hashMapOf<String, Command>()
Expand Down Expand Up @@ -116,6 +117,30 @@ class AutoTrajectories(val pnp: PickAndPlaceSubsystem, val swerveSubsystem: Swer
PathConstraints(4.00, 3.00)
)
}
fun RedLeftConeRightPlaceLeave(): Command {
return base("Red Left Cone Right Place Leave",
PathConstraints(3.00,1.50),
PathConstraints(2.00, 1.00)
)
}
fun RedLeftConeLeftPlaceLeave(): Command {
return base("Red Left Cone Left Place Leave",
PathConstraints(3.00,1.50),
PathConstraints(2.00, 1.00)
)
}
fun RedRightConeLeftPlaceLeave(): Command {
return base("Red Right Cone Left Place Leave",
PathConstraints(3.00,1.50),
PathConstraints(3.00, 1.00)
)
}
fun RedRightConeRightPlaceLeave(): Command {
return base("Red Right Cone Right Place Leave",
PathConstraints(3.00,1.50),
PathConstraints(2.00, 1.00)
)
}
fun BlueCenter1Balance(): Command {
return base("Blue Center 1 Balance",
PathConstraints(1.00,0.50),
Expand Down Expand Up @@ -149,16 +174,16 @@ class AutoTrajectories(val pnp: PickAndPlaceSubsystem, val swerveSubsystem: Swer
fun RedTop1Balance(): Command {
return base("Red Top 1 Balance",
PathConstraints(3.00,1.50),
PathConstraints(2.00, 1.00),
PathConstraints(1.00, 0.50)
PathConstraints(3.00, 1.50),
PathConstraints(2.00, 1.00)
)
}
fun RedBottom1Balance(): Command {
println("Running The Path")
return base("Red Bottom 1 Balance",
PathConstraints(3.00,1.50),
PathConstraints(2.00, 1.00),
PathConstraints(1.00, 0.50)
PathConstraints(4.00,2.50),
PathConstraints(3.50, 1.75),
PathConstraints(1.20, 0.70)
)
}
fun BlueTop1GetBalance(): Command {
Expand Down
9 changes: 6 additions & 3 deletions src/main/kotlin/frc/robot/commands/LEDCommands.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,13 @@ class SetLEDValueConeCube(val subsystem: BlinkinLEDSubsystem): CommandBase() {
if (CommandValues.auto) {
if (CommandValues.balanced) {
subsystem.setLED(0.59)
println("Balanced")
} else if (CommandValues.balancing) {
subsystem.setLED(0.57)
subsystem.setLED(-0.31)
println("Balancing")
} else {
subsystem.setLED(0.73)
subsystem.setLED(-0.29)
println("Auto LEDs")
}
} else if (CommandValues.visionIsMovingRobot) {
subsystem.setLED(0.73)
Expand All @@ -27,7 +30,7 @@ class SetLEDValueConeCube(val subsystem: BlinkinLEDSubsystem): CommandBase() {
} else if (!CommandValues.cube) {
subsystem.setLED(0.69)
} else {
subsystem.setLED(-0.29)
subsystem.setLED(0.93)
}
}
}
Expand Down
40 changes: 26 additions & 14 deletions src/main/kotlin/frc/robot/commands/PickAndPlaceCommands.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,18 @@ fun AutoBase(pnp: PickAndPlaceSubsystem): Command {
)
)
}
fun AutoBase2(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(70.0), // elbow
Math.toRadians(80.0), // wrist
{ 1.5 } // intake
)
)
}
//TODO:Test
fun FloorPlace(pnp: PickAndPlaceSubsystem, controller: XboxController): Command {
return SequentialCommandGroup(
Expand All @@ -204,7 +216,7 @@ fun FloorPlace(pnp: PickAndPlaceSubsystem, controller: XboxController): Command
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(66.0), // elbow
Math.toRadians(-25.0), // wrist
{ if (controller.leftBumper) -5.0 else 1.5 } // intake
{ if (controller.rightBumper) -6.0 else 1.5 } // intake
)
)

Expand All @@ -226,7 +238,7 @@ fun MidPlaceCube(pnp: PickAndPlaceSubsystem, controller: XboxController): Comman
0.2, // elevator
Math.toRadians(60.0), // elbow
0.0, // wrist
{ if (controller.leftBumper) -3.0 else 2.0 } // intake
{ if (controller.rightBumper) -6.0 else 2.0 } // intake
)
)

Expand All @@ -249,7 +261,7 @@ fun HighPlaceCube(pnp: PickAndPlaceSubsystem, controller: XboxController): Comma
0.6, // elevator
Math.toRadians(60.0), // elbow
Math.toRadians(-5.0), // wrist
{ if (controller.leftBumper) -3.0 else 2.0 }
{ if (controller.rightBumper) -5.0 else 2.0 }
)
)
}
Expand All @@ -259,19 +271,19 @@ fun MidPlaceCone(pnp: PickAndPlaceSubsystem, controller: XboxController): Comman
SetPickAndPlacePosition(
false,
pnp,
0.4, // elevator
0.47, // elevator
Math.toRadians(53.0), // elbow
Math.toRadians(-25.0), // wrist
Math.toRadians(-33.0), // wrist
{ 1.0 } // intake
),

SetPickAndPlacePosition(
true,
pnp,
0.4, // elevator
0.47, // elevator
Math.toRadians(53.0), // elbow
Math.toRadians(-25.0), // wrist
{ if (controller.leftBumper) -6.0 else 1.0 } // intake
Math.toRadians(-33.0), // wrist
{ if (controller.rightBumper) -6.0 else 1.0 } // intake
)
)
}
Expand Down Expand Up @@ -301,7 +313,7 @@ fun HighPlaceCone(pnp: PickAndPlaceSubsystem, controller: XboxController): Comma
0.945, // elevator
Math.toRadians(20.0), // elbow
Math.toRadians(-29.0), // wrist
{ if (controller.leftBumper) -6.0 else 1.0 }
{ if (controller.rightBumper) -6.0 else 1.0 }
)
)
}
Expand Down Expand Up @@ -357,8 +369,8 @@ fun ChutePick(pnp: PickAndPlaceSubsystem): Command {
pnp,
ArmConstants.elevatorMinHeight
, // elevator
1.03475, // elbow
0.99999, // wrist
1.13475, // elbow
0.83999, // wrist
{ 2.5 } // intake
)
)
Expand Down Expand Up @@ -392,7 +404,7 @@ fun AutoPlaceCubeMid(pnp: PickAndPlaceSubsystem): Command {
0.2, // elevator
Math.toRadians(60.0), // elbow
0.0, // wrist
{ -3.0 } // intake
{ -8.0 } // intake
)
)
}
Expand All @@ -414,7 +426,7 @@ fun AutoPlaceCubeHigh(pnp: PickAndPlaceSubsystem): Command {
0.6, // elevator
Math.toRadians(60.0), // elbow
Math.toRadians(-5.0), // wrist
{ -5.0 }
{ -9.0 }
)
)
}
Expand All @@ -435,7 +447,7 @@ fun AutoPlaceConeMid(pnp: PickAndPlaceSubsystem): Command {
0.4, // elevator
Math.toRadians(53.0), // elbow
Math.toRadians(-25.0), // wrist
{ -6.0 } // intake
{ -9.0 } // intake
)
)
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/frc/robot/constants/ArmConstants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,5 +64,5 @@ object ArmConstants {
val elbowEncoderPosOffset = 2.088
val wristEncoderPosOffset = 2.767

val elevatorZeroingVoltage = 0.0 //-1.0
val elevatorZeroingVoltage = -1.0 //-1.0
}
Loading

0 comments on commit 54383d3

Please sign in to comment.