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

Second drive state, tried to make ramping speed work better

Browse Source
This commit is contained in:
NerdyBirdy460
2023-12-04 21:31:51 -06:00
parent 84fd173082
commit db49950f30
4 changed files with 128 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -17,8 +17,9 @@ public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
@Override
public void setup() {
blackboardSet("readyToTurn", 0);
addState(new SodiPizzaAutoFirstDriveState());
addState(new SodiPizzaAutoTurnState());
addParallelStateToLastState(new SodiPizzaAutoFirstDriveState());
addState(new SodiPizzaAutoSecDriveState());
}
}

53
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java
View File

@@ -11,9 +11,8 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private int targetPos = 2500;
private double drivePower;
public int readyToTurn;
private double drivePower, drivePowerRaw;
public int readyToTurn, neededTicks, currentTicks, targetTicks = 1500;
public SodiPizzaAutoFirstDriveState() {
groupName = " ";
@@ -21,6 +20,25 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getDrivePower() {
if (Math.abs(neededTicks) > 1) {
drivePower = (drivePowerRaw * neededTicks) / 10;
}
return drivePower;
}
public void CalculateNeededTicks() {
if (targetTicks >= 0 && currentTicks >= 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks < 0 && currentTicks < 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks > 0 && currentTicks < 0) {
neededTicks = (targetTicks + Math.abs(currentTicks));
} else if (targetTicks < 0 && currentTicks > 0) {
neededTicks = (currentTicks + Math.abs(targetTicks));
}
}
@SuppressLint("SuspiciousIndentation")
@@ -34,10 +52,9 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
@Override
public void telemetry() {
engine.telemetry.addData("Target Ticks?", robot.leftFront.getTargetPosition());
engine.telemetry.addData("Current Ticks?", robot.leftFront.getCurrentPosition());
engine.telemetry.addData("Ticks Needed?", robot.leftFront.getTargetPosition() -
robot.leftFront.getCurrentPosition());
engine.telemetry.addData("Target Ticks?", targetTicks);
engine.telemetry.addData("Current Ticks?", currentTicks);
engine.telemetry.addData("Ticks Needed?", neededTicks);
engine.telemetry.addLine();
engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
engine.telemetry.addData("Distance Sensor Reading", robot.distSensor.getDistance(DistanceUnit.MM));
@@ -49,15 +66,20 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
readyToTurn = engine.blackboardGet("readyToTurn");
// Move forward from 0 to targetPos
currentTicks = robot.leftFront.getCurrentPosition();
CalculateNeededTicks();
// Move forward from 0 to targetTicks
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) {
robot.leftFront.setTargetPosition(targetPos);
robot.leftBack.setTargetPosition(targetPos);
robot.rightFront.setTargetPosition(targetPos);
robot.rightBack.setTargetPosition(targetPos);
robot.leftFront.setTargetPosition(targetTicks);
robot.leftBack.setTargetPosition(targetTicks);
robot.rightFront.setTargetPosition(targetTicks);
robot.rightBack.setTargetPosition(targetTicks);
drivePower = 0.5;
drivePowerRaw = 0.5;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
@@ -66,9 +88,10 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
}
//Stop and finish set after return to 0
else if (robot.leftFront.getCurrentPosition() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) {
else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
drivePower = 0;
drivePowerRaw = 0;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);

72
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
@@ -8,6 +10,9 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState {
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private double drivePower, drivePowerRaw;
public int readyToTurn, neededTicks, currentTicks, targetTicks = 500;
public SodiPizzaAutoSecDriveState() {
groupName = " ";
@@ -15,14 +20,81 @@ public class SodiPizzaAutoSecDriveState extends CyberarmState {
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getDrivePower() {
if (Math.abs(neededTicks) > 1) {
drivePower = (drivePowerRaw * neededTicks) / 10;
}
return drivePower;
}
public void CalculateNeededTicks() {
if (targetTicks >= 0 && currentTicks >= 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks < 0 && currentTicks < 0) {
neededTicks = Math.abs(targetTicks - currentTicks);
} else if (targetTicks > 0 && currentTicks < 0) {
neededTicks = (targetTicks + Math.abs(currentTicks));
} else if (targetTicks < 0 && currentTicks > 0) {
neededTicks = (currentTicks + Math.abs(targetTicks));
}
}
@Override
public void start() {
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
public void exec() {
readyToTurn = engine.blackboardGet("readyToTurn");
currentTicks = robot.leftFront.getCurrentPosition();
CalculateNeededTicks();
// Move forward from 0 to targetPos
if (robot.leftFront.getCurrentPosition() <= 10 && readyToTurn == 2) {
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
else if (robot.leftFront.getCurrentPosition() >= targetTicks - 10 && robot.leftFront.getCurrentPosition() <= targetTicks + 10) {
drivePowerRaw = 0;
getDrivePower();
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
engine.blackboardSet("readyToTurn", 3);
setHasFinished(true);
}
}
}

22
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.CenterStage.Autonomous.States;
import android.annotation.SuppressLint;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
@@ -9,7 +11,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaAutoTurnState extends CyberarmState {
final private SodiPizzaMinibotObject robot = new SodiPizzaMinibotObject();
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private double turnSpeedRaw, turnSpeed;
@@ -29,11 +31,12 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
public SodiPizzaAutoTurnState() {
groupName = " ";
actionName = " ";
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getTurnSpeed() {
if (Math.abs(neededRot) > 5) {
if (Math.abs(neededRot) > 1) {
turnSpeed = (turnSpeedRaw * neededRot) / 10;
}
return turnSpeed;
@@ -79,6 +82,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
}
@SuppressLint("SuspiciousIndentation")
@Override
public void exec() {
@@ -92,14 +96,20 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
if (readyToTurn == 0) {
targetRot = 0;
if (currentRot < targetRot - 1) {
if (currentRot <= targetRot - 1) {
turnSpeedRaw = 0.5;
getTurnSpeed();
robot.rightFront.setPower(robot.leftFront.getPower() + turnSpeed);
robot.rightBack.setPower(robot.leftBack.getPower() + turnSpeed);
}
} else if (currentRot >= targetRot + 1)
turnSpeedRaw = 0.5;
getTurnSpeed();
robot.leftFront.setPower(robot.rightFront.getPower() + turnSpeed);
}
@@ -130,7 +140,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(-turnSpeed);
robot.rightBack.setPower(-turnSpeed);
} else if (readyToTurn == 1 && Math.abs(neededRot) < 3) {
} else if (readyToTurn == 1 && Math.abs(neededRot) < 2) {
turnSpeedRaw = 0;
getTurnSpeed();
@@ -140,7 +150,7 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
engine.blackboardSet("readyToTurn", 0);
engine.blackboardSet("readyToTurn", 2);
}
}