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

Allow Rotate states created after robot has moved and rotated to work correctly by using the robot's initial facing angle.

Browse Source
This commit is contained in:
Cyberarm
2023-02-03 09:51:11 -06:00
parent 7e667b154b
commit 02a066d2b6
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
View File

@@ -43,7 +43,7 @@ public class Robot {
public final ColorSensor indicatorA, indicatorB; public final ColorSensor indicatorA, indicatorB;
public LynxModule expansionHub; public LynxModule expansionHub;
public final double imuAngleOffset; public final double imuAngleOffset, initialFacing;
public boolean wristManuallyControlled = false, armManuallyControlled = false; public boolean wristManuallyControlled = false, armManuallyControlled = false;
public boolean automaticAntiTipActive = false; public boolean automaticAntiTipActive = false;
public boolean hardwareFault = false; public boolean hardwareFault = false;
@@ -191,6 +191,9 @@ public class Robot {
imu.initialize(parameters); imu.initialize(parameters);
// Preserve our "initial" facing, since we transform it from zero.
initialFacing = facing();
// BulkRead from Hubs // BulkRead from Hubs
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) { for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
@@ -725,6 +728,10 @@ public class Robot {
arm.setPower(tuningConfig("arm_automatic_power").value()); arm.setPower(tuningConfig("arm_automatic_power").value());
} }
public double initialFacing() {
return initialFacing;
}
public double facing() { public double facing() {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);

2
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java
View File

@@ -32,7 +32,7 @@ public class Rotate extends CyberarmState {
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
facing = (robot.facing() + targetFacing + 360.0) % 360.0; facing = (robot.initialFacing() + targetFacing + 360.0) % 360.0;
velocity = targetVelocity; velocity = targetVelocity;
} }