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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2023-11-30 18:45:18 -06:00
parent faa9d9ed92 79e06c3c10
commit 77fc170ca7
4 changed files with 42 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,18 +9,16 @@ import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState;
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.CyberarmState;
@Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "")
public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new SodiPizzaAutoFirstDriveState());
blackboardSet("readyToTurn", 0);
addState(new SodiPizzaAutoTurnState());
addParallelStateToLastState(new SodiPizzaAutoFirstDriveState());
// addState(new SodiPizzaAutoSecDriveState());
// addState(new SodiPizzaAutoArmState());
// addState(new SodiPizzaWheelTest());
}
}

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

@@ -12,6 +12,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
private long lastMoveTime;
private int targetPos = 2500;
private double drivePower;
public int readyToTurn;
public SodiPizzaAutoFirstDriveState() {
groupName = " ";
@@ -26,7 +27,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
public void start() {
lastMoveTime = System.currentTimeMillis();
robot.readyToTurn = 0;
readyToTurn = engine.blackboardGetInt("readyToTurn");
}
@@ -36,12 +37,18 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
engine.telemetry.addData("Current Ticks?", robot.leftFront.getCurrentPosition());
engine.telemetry.addData("Ticks Needed?", robot.leftFront.getTargetPosition() -
robot.leftFront.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Internal Ready To Turn Value", readyToTurn);
}
@Override
public void exec() {
readyToTurn = engine.blackboardGet("readyToTurn");
// Move forward from 0 to targetPos
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10) {
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && readyToTurn == 0) {
robot.leftFront.setTargetPosition(targetPos);
robot.leftBack.setTargetPosition(targetPos);
@@ -66,7 +73,7 @@ public class SodiPizzaAutoFirstDriveState extends CyberarmState{
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
robot.readyToTurn = 1;
engine.blackboardSet("readyToTurn", 1);
setHasFinished(true);
}

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

@@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.States;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmState;
@@ -14,7 +15,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
private int startPos;
private double targetRot;
private double currentRot;
private double neededRot = targetRot - currentRot;
private double neededRot;
public int readyToTurn;
// private double rightTurnCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) + 90;
// private double rightTurnCCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - 90;
// private double backTurnCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) + 180;
// private double backTurnCCW = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - 180;
/** Rot = rotation **/
public SodiPizzaAutoTurnState() {
@@ -23,20 +31,19 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.setup();
}
public SodiPizzaAutoTurnState(int readyToTurnParm) {
groupName = " ";
actionName = " ";
robot.setup();
robot.readyToTurn = readyToTurnParm;
}
private double getTurnSpeed() {
if (Math.abs(neededRot) > 5) {
turnSpeed = turnSpeedRaw * neededRot / 10;
turnSpeed = (turnSpeedRaw * neededRot) / 10;
}
return turnSpeed;
}
@Override
public void telemetry() {
engine.telemetry.addData("Target Pos", robot.leftFront.getTargetPosition());
engine.telemetry.addData("Power", robot.leftFront.getPower());
}
@Override
public void start() {
@@ -48,18 +55,21 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
robot.imu.resetYaw();
neededRot = (targetRot - robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
// robot.imu.resetYaw();
}
@Override
public void exec() {
readyToTurn = engine.blackboardGet("readyToTurn");
currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (robot.readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) {
if (readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) {
targetRot = currentRot + 90;
targetRot = -90;
turnSpeedRaw = 0.3;
@@ -68,14 +78,14 @@ public class SodiPizzaAutoTurnState extends CyberarmState {
robot.rightFront.setPower(-turnSpeed);
robot.rightBack.setPower(-turnSpeed);
} else if (robot.readyToTurn == 1 && Math.abs(neededRot) < 5) {
} else if (readyToTurn == 1 && Math.abs(neededRot) < 5) {
turnSpeedRaw = 0;
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
robot.readyToTurn = 0;
engine.blackboardSet("readyToTurn", 0);
}
}

6
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
View File

@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.Library.Robot;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
@@ -19,8 +20,6 @@ public class SodiPizzaMinibotObject extends Robot {
public IMU imu;
private String string;
public int readyToTurn;
public static double GRIPPER_CLOSED = 0.333; // ~90 degrees
public static double GRIPPER_OPEN = 0.75; // ~205 degrees
@@ -43,6 +42,7 @@ public class SodiPizzaMinibotObject extends Robot {
this.engine = CyberarmEngine.instance;
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
this.imu = CyberarmEngine.instance.hardwareMap.get(IMU.class, "imu");
//Motor defining
leftFront = engine.hardwareMap.dcMotor.get("leftFront");
@@ -70,5 +70,7 @@ public class SodiPizzaMinibotObject extends Robot {
gripper = engine.hardwareMap.servo.get("gripper");
//readyToTurn
}
}