TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Started to format drive state similar to turn state, tests seem to work well for now

Browse Source
This commit is contained in:
NerdyBirdy460
2023-12-09 13:00:34 -06:00
parent b537d04c52
commit 47883710ba
1 changed files with 15 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java
View File

@@ -14,7 +14,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
final private String groupName, actionName;
private long lastMoveTime;
private double drivePower, drivePowerRaw;
public int readyToTurn, neededTicks, currentTicks, targetTicks = 1000;
public int readyToTurn, neededTicks, currentTicks, targetTicks;
public SodiPizzaAutoFirstDriveState() {
groupName = " ";
@@ -52,6 +52,16 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot.imu.resetYaw();
robot.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
@Override
@@ -111,8 +121,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
}
//Stop and finish set after reached targetTicks within tolerance of 2
else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
//Stop and finish set after reached targetTicks within tolerance of 10
else if (Math.abs(neededTicks) < 10) {
drivePowerRaw = 0;
getDrivePower();
@@ -122,6 +132,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
engine.blackboardSet("readyToTurn", readyToTurn + 1);
setHasFinished(true);