TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 08:32:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Enable IMU reset from TeleOp, fix robot centric drive

Browse Source
This commit is contained in:
Cyberarm
2023-02-10 15:07:34 -06:00
parent 2d8ea6d431
commit fbb0645283
1 changed files with 14 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java
View File

@@ -10,6 +10,7 @@ import org.timecrafters.minibots.cyberarm.chiron.Robot;
public class DrivetrainDriverControl extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final double robotCentricRotation;
private Gamepad controller;
@@ -23,6 +24,8 @@ public class DrivetrainDriverControl extends CyberarmState {
this.actionName = actionName;
this.controller = engine.gamepad1;
this.robotCentricRotation = robot.tuningConfig("robot_centric_rotation").value();
}
@Override
@@ -50,6 +53,13 @@ public class DrivetrainDriverControl extends CyberarmState {
double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x);
if (!fieldCentricControl) {
Vector2d v = new Vector2d(x, y).rotated(robotCentricRotation);
x = v.getX();
y = v.getY();
}
// Improve control?
if (y < 0) {
y = -Math.sqrt(-y);
@@ -160,6 +170,10 @@ public class DrivetrainDriverControl extends CyberarmState {
if (button.equals("right_stick_button")) {
robotSlowMode = !robotSlowMode;
}
if (button.equals("left_stick_button") && robot.hardwareFault) {
robot.imu.resetYaw();
}
}
@Override