TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 08:42: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-07 13:04:25 -06:00
parent d6ef24dbfe
commit c50b7d464b
5 changed files with 65 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java
View File

@@ -16,7 +16,9 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
blackboardSet("readyToTurn", 0);
addState(new SodiPizzaAutoFirstDriveState());
addState(new SodiPizzaAutoTurnState());
addState(new SodiPizzaAutoSecDriveState());
blackboardGetInt("readyToTurn");
addState(new SodiPizzaAutoFirstDriveState());
// addState(new SodiPizzaAutoSecDriveState());
}
}

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

@@ -2,6 +2,8 @@ package org.timecrafters.CenterStage.Autonomous.SodiStates;
import android.annotation.SuppressLint;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
@@ -12,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 = 1500;
public int readyToTurn, neededTicks, currentTicks, targetTicks = 1000;
public SodiPizzaAutoFirstDriveState() {
groupName = " ";
@@ -73,23 +75,43 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
CalculateNeededTicks();
// Move forward from 0 to targetTicks
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) {
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10/* && robot.distSensor.getDistance(DistanceUnit.MM) >= 100*/) {
if (readyToTurn == 0) {
targetTicks = 1000;
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);
robot.rightFront.setTargetPosition(targetTicks);
robot.rightBack.setTargetPosition(targetTicks);
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);
robot.rightFront.setTargetPosition(targetTicks);
robot.rightBack.setTargetPosition(targetTicks);
drivePowerRaw = 0.5;
getDrivePower();
drivePowerRaw = 0.5;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} else if (readyToTurn == 2) {
targetTicks = 500;
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);
robot.rightFront.setTargetPosition(targetTicks);
robot.rightBack.setTargetPosition(targetTicks);
drivePowerRaw = 0.5;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
}
}
//Stop and finish set after return to 0
//Stop and finish set after reached targetTicks within tolerance of 2
else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
drivePowerRaw = 0;
@@ -100,7 +122,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
engine.blackboardSet("readyToTurn", 1);
engine.blackboardSet("readyToTurn", readyToTurn + 1);
setHasFinished(true);
}

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java
View File

@@ -2,6 +2,7 @@ package org.timecrafters.CenterStage.Autonomous.SodiStates;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
@@ -65,7 +66,7 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState {
CalculateNeededTicks();
// Move forward from 0 to targetPos
if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2) {
if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2 && robot.distSensor.getDistance(DistanceUnit.MM) >= 100) {
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);

11
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoThirdDriveState.java Normal file
View File

@@ -0,0 +1,11 @@
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaAutoThirdDriveState extends CyberarmState {
@Override
public void exec() {
}
}

16
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java
View File

@@ -2,6 +2,8 @@ package org.timecrafters.CenterStage.Autonomous.SodiStates;
import android.annotation.SuppressLint;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
@@ -124,9 +126,10 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
}
if (currentRot >= -88) {
if (neededRot >= 2) {
turnSpeedRaw = 0.3;
turnSpeedRaw = 0.2;
CalculateNeededRot();
getTurnSpeed();
robot.leftFront.setPower(-turnSpeed);
@@ -134,9 +137,10 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
} else if (currentRot <= -92) {
} else if (neededRot <= -2) {
turnSpeedRaw = 0.2;
CalculateNeededRot();
getTurnSpeed();
robot.leftFront.setPower(turnSpeed);
@@ -147,6 +151,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
} else if (readyToTurn == 1 && Math.abs(neededRot) < 2) {
turnSpeedRaw = 0;
CalculateNeededRot();
getTurnSpeed();
robot.leftFront.setPower(turnSpeed);
@@ -154,6 +159,11 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
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);
engine.blackboardSet("readyToTurn", 2);
setHasFinished(true);