Skip to content

Commit

Permalink
Before load in KCMT
Browse files Browse the repository at this point in the history
  • Loading branch information
@zaneal
zaneal committed Oct 13, 2023
1 parent 58578f6 commit 33d6b65
Show file tree
Hide file tree
Showing 14 changed files with 81 additions and 128 deletions.
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/Blue Left Cone Left Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/Blue Left Cone Right Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/Blue Right Cone Left Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/Blue Right Cone Right Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/Red Left Cone Left Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand Down
18 changes: 9 additions & 9 deletions src/main/deploy/pathplanner/Red Left Cone Right Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
},
"prevControl": null,
"nextControl": {
"x": 14.676998666497335,
"y": 0.5399839133011589
"x": 14.752093078838936,
"y": 0.16341008029238285
},
"holonomicAngle": 0,
"isReversal": false,
Expand All @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand All @@ -27,16 +27,16 @@
},
{
"anchorPoint": {
"x": 12.637098097112862,
"y": 0.6695290230858237
"x": 11.50801446568171,
"y": 0.7589464623373685
},
"prevControl": {
"x": 12.902335040781193,
"y": 0.6554959359918717
"x": 11.770694951874724,
"y": 0.7196205676456271
},
"nextControl": {
"x": 11.35495407439076,
"y": 0.7373643649089
"x": 10.658200865012839,
"y": 0.8861720535941066
},
"holonomicAngle": 0,
"isReversal": false,
Expand Down
18 changes: 9 additions & 9 deletions src/main/deploy/pathplanner/Red Right Cone Left Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
},
"prevControl": null,
"nextControl": {
"x": 14.655717084003614,
"y": 4.671162387463664
"x": 14.655543120114018,
"y": 4.934601704272853
},
"holonomicAngle": 0,
"isReversal": false,
Expand All @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand All @@ -27,16 +27,16 @@
},
{
"anchorPoint": {
"x": 12.832691496980217,
"y": 4.727816627508255
"x": 12.123266755214068,
"y": 4.8020992083650835
},
"prevControl": {
"x": 13.805398751544114,
"y": 4.6203616092287545
"x": 13.095974009777965,
"y": 4.694644190085583
},
"nextControl": {
"x": 11.807522295600148,
"y": 4.841067119634239
"x": 11.098097553833998,
"y": 4.915349700491068
},
"holonomicAngle": 0,
"isReversal": false,
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/Red Right Cone Right Place Leave.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
"isStopPoint": false,
"stopEvent": {
"names": [
"MidCone",
"HighCone",
"Base"
],
"executionBehavior": "sequential",
Expand Down
2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/generatedJSON/Red Left Cone Right Place Leave.wpilib.json
View file

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion src/main/deploy/pathplanner/generatedJSON/Red Right Cone Left Place Leave.wpilib.json
View file

Large diffs are not rendered by default.

14 changes: 0 additions & 14 deletions src/main/kotlin/frc/robot/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,6 @@ class RobotContainer {
enum class CommandSelector {
LOWPICKCUBE,
UPRIGHTPICKCONE,
CHUTEPICK,
FLOORPLACE,
MIDPLACECUBE,
HIGHPLACECUBE,
Expand Down Expand Up @@ -194,10 +193,6 @@ class RobotContainer {

)

// JoystickButton(primaryController, XboxController.Button.kRightBumper.value).whileTrue(
// RetroreflectiveVision(swerveSubsystem, primaryController)
//)

Trigger { primaryController.rightTriggerAxis > 0.2 }.whileTrue(


Expand Down Expand Up @@ -494,13 +489,4 @@ class RobotContainer {
SmartDashboard.putData("Auto Mode", autoCommandChooser)
}
val autonomousCommand: Command get() = autoCommandChooser.selected

// SequentialCommandGroup(
// AutoPlaceConeMid(pickAndPlace).withTimeout(4.0),
// RunCommand({swerveSubsystem.drive(-0.5,0.0,0.0,false, true)}, swerveSubsystem).withTimeout(1.0),
// ParallelCommandGroup(
// RunCommand({swerveSubsystem.drive(0.0,0.0,0.0,false, true)}, swerveSubsystem),
// Base(pickAndPlace)
// )
// )
}
1 change: 1 addition & 0 deletions src/main/kotlin/frc/robot/commands/AutoTrajectories.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ class AutoTrajectories(val pnp: PickAndPlaceSubsystem, val swerveSubsystem: Swer
"MidConePlace" to AutoPlaceConeMidPlace(pnp).withTimeout(2.0),
"HighCone" to AutoPlaceConeHigh(pnp).withTimeout(4.0),
"Base2" to AutoBase2(pnp),
"Base3" to AutoBase3(pnp),
"Balance" to Balancer(swerveSubsystem),
"PickUpConeUp" to AutoPickCone(pnp)
)
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/frc/robot/commands/LEDCommands.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ class SetLEDValueConeCube(val subsystem: BlinkinLEDSubsystem): CommandBase() {
println("Auto LEDs")
}
} else if (CommandValues.visionIsMovingRobot) {
subsystem.setLED(-0.91)
subsystem.setLED(-0.93)
} else {
if (CommandValues.cube) {
subsystem.setLED(0.91)//0.91
Expand Down
140 changes: 53 additions & 87 deletions src/main/kotlin/frc/robot/commands/PickAndPlaceCommands.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ fun Base(pnp: PickAndPlaceSubsystem): Command {
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(73.0), // elbow 60
Math.toRadians(74.0), // elbow 60
Math.toRadians(-10.0), // wrist -100
{ if (!CommandValues.cube) 1.0 else 0.0 }, //in
{ if (!CommandValues.cube) -1.2 else 0.0 } //in
Expand All @@ -175,59 +175,23 @@ fun Base(pnp: PickAndPlaceSubsystem): Command {
)
)
}
fun AutoBase(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(73.0), // elbow
Math.toRadians(-10.0), // wrist
{ 0.5 }, //in
{ -0.5 } //in
),
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(120.0), // elbow
Math.toRadians(-20.0), // wrist
{ 0.5 }, //in
{ -0.5 } //in
)
)
}
fun AutoBase2(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(73.0), // elbow
Math.toRadians(-10.0), // wrist
{ 0.5 }, //in
{ -0.5 }// in
)
)
}
//TODO:Test
fun FloorPlace(pnp: PickAndPlaceSubsystem, controller: XboxController): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(68.0), // elbow
Math.toRadians(-65.0), // wrist
Math.toRadians(73.0), // elbow
Math.toRadians(-70.0), // wrist
{ if (!CommandValues.cube) 1.0 else 0.0 }, //in
{ if (!CommandValues.cube) -1.2 else 0.0 }// in
),
SetPickAndPlacePosition(
true,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(68.0), // elbow
Math.toRadians(-65.0), // wrist
Math.toRadians(73.0), // elbow
Math.toRadians(-70.0), // wrist
{ if (controller.rightBumper) -6.0 else if (!CommandValues.cube) 1.0 else 0.0 }, //out then in
{ if (controller.rightBumper) 6.0 else if (!CommandValues.cube) -1.2 else 0.0 }// out then in
)
Expand Down Expand Up @@ -261,15 +225,6 @@ fun MidPlaceCube(pnp: PickAndPlaceSubsystem, controller: XboxController): Comman
}
fun HighPlaceCube(pnp: PickAndPlaceSubsystem, controller: XboxController): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(80.0), // elbow
Math.toRadians(-30.0), // wrist
{ 0.0 },
{ 0.0 }
),
SetPickAndPlacePosition(
false,
pnp,
Expand Down Expand Up @@ -317,15 +272,6 @@ fun MidPlaceCone(pnp: PickAndPlaceSubsystem, controller: XboxController): Comman
//TODO:Tune
fun HighPlaceCone(pnp: PickAndPlaceSubsystem, controller: XboxController): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
0.15, // elevator
Math.toRadians(70.0), // elbow
Math.toRadians(10.0), // wrist
{ 3.0 },
{ -4.0 }
),
SetPickAndPlacePosition(
false,
pnp,
Expand Down Expand Up @@ -456,7 +402,54 @@ fun ShelfPickCone(pnp: PickAndPlaceSubsystem): Command {




fun AutoBase(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(73.0), // elbow
Math.toRadians(-10.0), // wrist
{ 0.5 }, //in
{ -0.5 } //in
),
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(120.0), // elbow
Math.toRadians(-20.0), // wrist
{ 0.5 }, //in
{ -0.5 } //in
)
)
}
fun AutoBase2(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(73.0), // elbow
Math.toRadians(-10.0), // wrist
{ 0.5 }, //in
{ -0.5 }// in
)
)
}
fun AutoBase3(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(110.0), // elbow
Math.toRadians(-20.0), // wrist
{ 0.5 }, //in
{ -0.5 }// in
)
)
}
fun AutoPickCube(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
Expand Down Expand Up @@ -518,15 +511,6 @@ fun AutoPlaceCubeMid(pnp: PickAndPlaceSubsystem): Command {
//TODO:Test
fun AutoPlaceCubeHigh(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
ArmConstants.elevatorMinHeight, // elevator
Math.toRadians(80.0), // elbow
Math.toRadians(-30.0), // wrist
{ 0.0 },
{ 0.0 }
),
SetPickAndPlacePosition(
false,
pnp,
Expand Down Expand Up @@ -557,15 +541,6 @@ fun AutoPlaceConeMidGetThere(pnp: PickAndPlaceSubsystem): Command {
Math.toRadians(-30.0), // wrist
{ 5.0 },
{ -6.0 }// intake
),
SetPickAndPlacePosition(
false,
pnp,
0.37, // elevator
Math.toRadians(60.0), // elbow
Math.toRadians(-50.0), // wrist
{ 5.0 },
{ -6.0 }// intake
)
)
}
Expand All @@ -585,15 +560,6 @@ fun AutoPlaceConeMidPlace(pnp: PickAndPlaceSubsystem): Command {

fun AutoPlaceConeHigh(pnp: PickAndPlaceSubsystem): Command {
return SequentialCommandGroup(
SetPickAndPlacePosition(
false,
pnp,
0.15, // elevator
Math.toRadians(70.0), // elbow
Math.toRadians(10.0), // wrist
{ 3.0 },
{ -4.0 }
),
SetPickAndPlacePosition(
false,
pnp,
Expand Down

0 comments on commit 33d6b65

Please sign in to comment.