Skip to content
This repository has been archived by the owner on Jul 1, 2024. It is now read-only.

add orient command #52

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

add orient command #52

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fb0b5e2
add orient command
gabehowe Feb 7, 2024
bb19468
change starting heading to pi
gabehowe Feb 9, 2024
8eeb823
calculate minimum rotation for orientation; move orient to MecanumDri…
gabehowe Feb 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/helper/DriveManager.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,8 @@ class DriveManager(hardwareMap: HardwareMap, val keymap: Keymap, gamepad1: Gamep
getGamepad(this.keymap.loadDroneMap.second).getGamepadButton(this.keymap.loadDroneMap.first)
.whenHeld(InstantCommand({ loadDrone() }))

getGamepad(this.keymap.orientMap.second).getGamepadButton(this.keymap.orientMap.first)
.whenHeld(InstantCommand({ drive.orient() }))

}

Expand All @@ -118,6 +120,7 @@ class DriveManager(hardwareMap: HardwareMap, val keymap: Keymap, gamepad1: Gamep
val disableManualControlMap: Pair<GamepadKeys.Button, Int>,
val shootDroneMap: Pair<GamepadKeys.Button, Int>,
val loadDroneMap: Pair<GamepadKeys.Button, Int>,
val orientMap: Pair<GamepadKeys.Button, Int>
)


Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/helper/StorePos.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
import com.acmerobotics.roadrunner.geometry.Vector2d;

public class StorePos {
public static Pose2d OdoPose = new Pose2d(new Vector2d(0, 0), Math.toRadians(0));
public static Pose2d OdoPose = new Pose2d(new Vector2d(0, 0), Math.toRadians(Math.PI));
public void StorePos(Pose2d Pose) {
OdoPose = Pose;
}
Expand Down
3 changes: 2 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/Duo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,8 @@ public void initialize() {
new Pair<>(GamepadKeys.Button.X, 2), // Arm Manual Control
new Pair<>(GamepadKeys.Button.B, 2), // Arm Reset
new Pair<>(GamepadKeys.Button.Y, 1), // Shoot
new Pair<>(GamepadKeys.Button.X, 1) // Load
new Pair<>(GamepadKeys.Button.X, 1), // Load
new Pair<>(GamepadKeys.Button.RIGHT_STICK_BUTTON,1) // Align to Backdrop
);
driveManager = new DriveManager(hardwareMap, keymap, gamepad1, gamepad2);
}
Expand Down
5 changes: 3 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/Solo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,11 @@ public void initialize() {
new Pair<>(GamepadKeys.Button.DPAD_LEFT, 1), // Low Position
new Pair<>(GamepadKeys.Button.DPAD_UP, 1), // Top Position
new Pair<>(GamepadKeys.Button.DPAD_DOWN, 1), // Home Position
new Pair<>(GamepadKeys.Button.X, 1), // Arm Manual Control
new Pair<>(GamepadKeys.Button.A, 1), // Arm Manual Control
new Pair<>(GamepadKeys.Button.B, 1), // Arm Reset
new Pair<>(GamepadKeys.Button.Y, 1), // Shoot
new Pair<>(GamepadKeys.Button.X, 1) // Load
new Pair<>(GamepadKeys.Button.X, 1), // Load
new Pair<>(GamepadKeys.Button.RIGHT_STICK_BUTTON,1) // Align to Backdrop
);
driveManager = new DriveManager(hardwareMap, keymap, gamepad1, gamepad2);

Expand Down
8 changes: 8 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MecanumDriveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,4 +122,12 @@ public Localizer getLocalizer() {
return drive.getLocalizer();
}

public void orient() {
if (Math.abs(Math.PI * 2 - this.getPoseEstimate().getHeading()) < Math.abs(this.getPoseEstimate().getHeading())) {
this.turn(Math.PI * 2 - this.getPoseEstimate().getHeading());
} else {
this.turn(0 - this.getPoseEstimate().getHeading());
}
}

}
Loading