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-25 12:01:38 -06:00
parent 8bc7c69cb3 fe2c3de40b
commit 2862347f01
6 changed files with 358 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,10 +1,21 @@
package org.timecrafters.CenterStage.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState;
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState;
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState;
import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "")
public class SodiPizzaAutoRedRightEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new SodiPizzaAutoFirstDriveState());
addState(new SodiPizzaAutoTurnState());
// addState(new SodiPizzaAutoArmState());
// addState(new SodiPizzaWheelTest());
}
}

21
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java Normal file
View File

@@ -0,0 +1,21 @@
package org.timecrafters.CenterStage.Autonomous.States;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaAutoArmState extends CyberarmState {
private SodiPizzaMinibotObject robot;
private String groupName, actionName;
public void SodiPizzaAutoDriveState() {
groupName = " ";
actionName = " ";
robot = new SodiPizzaMinibotObject();
robot.setup();
}
@Override
public void exec() {
}
}

122
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java Normal file
View File

@@ -0,0 +1,122 @@
package org.timecrafters.CenterStage.Autonomous.States;
import android.annotation.SuppressLint;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaAutoFirstDriveState extends CyberarmState{
final private SodiPizzaMinibotObject robot;
final private String groupName, actionName;
private long lastMoveTime;
private int targetPos = 2500;
private double drivePower;
private boolean lastHalf;
public SodiPizzaAutoFirstDriveState() {
groupName = " ";
actionName = " ";
robot = new SodiPizzaMinibotObject();
robot.setup();
}
@SuppressLint("SuspiciousIndentation")
@Override
public void start() {
lastHalf = false;
lastMoveTime = System.currentTimeMillis();
robot.readyToTurn = 0;
}
@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());
}
@Override
public void exec() {
// Move forward from 0 to targetPos
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10
&& !lastHalf) {
robot.leftFront.setTargetPosition(targetPos);
robot.leftBack.setTargetPosition(targetPos);
robot.rightFront.setTargetPosition(targetPos);
robot.rightBack.setTargetPosition(targetPos);
drivePower = 0.5;
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() >= targetPos - 10 && robot.leftFront.getCurrentPosition() <= targetPos + 10) {
drivePower = 0;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
robot.readyToTurn = 1;
setHasFinished(true);
}
/* //Stop moving and update lastMoveTime
if (robot.leftFront.getCurrentPosition() >= targetPos && drivePower > 0) {
lastHalf = true;
drivePower = 0;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
lastMoveTime = System.currentTimeMillis();
}
//Move backwards from targetPos to 0
if (robot.leftFront.getCurrentPosition() >= targetPos && drivePower == 0
&& System.currentTimeMillis() - lastMoveTime >= 500 && lastHalf) {
robot.leftFront.setTargetPosition(0);
robot.leftBack.setTargetPosition(0);
robot.rightFront.setTargetPosition(0);
robot.rightBack.setTargetPosition(0);
drivePower = -0.5;
robot.leftFront.setPower(drivePower);
robot.leftBack.setPower(drivePower);
robot.rightFront.setPower(drivePower);
robot.rightBack.setPower(drivePower);
} */
/* if (robot.leftFront.getCurrentPosition() < 1250 && robot.leftFront.getCurrentPosition() >= targetPos &&
robot.rightBack.getCurrentPosition() > 750) {
robot.leftFront.setPower(0.5);
robot.leftBack.setPower(0.5);
robot.rightFront.setPower(-0.5);
robot.rightBack.setPower(-0.5);
robot.leftFront.setTargetPosition(1250);
robot.leftBack.setTargetPosition(1250);
robot.rightFront.setTargetPosition(750);
robot.rightBack.setTargetPosition(750);
}
*/
}
}

71
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java Normal file
View File

@@ -0,0 +1,71 @@
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 dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaAutoTurnState extends CyberarmState {
final private SodiPizzaMinibotObject robot = new SodiPizzaMinibotObject();
final private String groupName, actionName;
private long lastMoveTime;
private double turnSpeedRaw, turnSpeed;
private int startPos;
private double targetRot;
private double currentRot = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
private double neededRot = targetRot - currentRot;
/** Rot = rotation **/
public SodiPizzaAutoTurnState() {
groupName = " ";
actionName = " ";
robot.setup();
}
private double getTurnSpeed() {
if (Math.abs(neededRot) > 5) {
turnSpeed = turnSpeedRaw * neededRot / 10;
}
return turnSpeed;
}
@Override
public void start() {
startPos = robot.leftFront.getCurrentPosition();
turnSpeedRaw = 0;
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
robot.imu.resetYaw();
}
@Override
public void exec() {
if (robot.readyToTurn == 1 && robot.leftFront.getCurrentPosition() == startPos && Math.abs(neededRot) > 10) {
targetRot = currentRot + 90;
turnSpeedRaw = 0.3;
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(-turnSpeed);
robot.rightBack.setPower(-turnSpeed);
} else if (robot.readyToTurn == 1 && Math.abs(neededRot) < 10) {
turnSpeedRaw = 0;
robot.leftFront.setPower(turnSpeed);
robot.leftBack.setPower(turnSpeed);
robot.rightFront.setPower(turnSpeed);
robot.rightBack.setPower(turnSpeed);
}
}
}

93
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java Normal file
View File

@@ -0,0 +1,93 @@
package org.timecrafters.CenterStage.Autonomous.States;
import android.annotation.SuppressLint;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;
import dev.cyberarm.engine.V2.CyberarmState;
public class SodiPizzaWheelTest extends CyberarmState {
private SodiPizzaMinibotObject robot;
private String groupName, actionName;
private long lastMoveTime;
private double lDrivePower, rDrivePower;
public SodiPizzaWheelTest() {
groupName = " ";
actionName = " ";
robot = new SodiPizzaMinibotObject();
robot.setup();
}
private double getrDrivePower() {
rDrivePower = lDrivePower * 0.75;
return rDrivePower;
}
@Override
public void telemetry() {
engine.telemetry.addData("Current Pos? ", robot.leftFront.getCurrentPosition());
engine.telemetry.addData("Last Moved Time? ", lastMoveTime);
engine.telemetry.addData("System Current Time? ", System.currentTimeMillis());
engine.telemetry.addData("Diff in Right/Left Front Wheel Power? ", robot.rightFront.getPower() - robot.leftFront.getPower());
}
@Override
public void start() {
lastMoveTime = System.currentTimeMillis();
lDrivePower = 0;
rDrivePower = 0;
robot.leftFront.setPower(lDrivePower);
robot.leftBack.setPower(lDrivePower);
robot.rightFront.setPower(rDrivePower);
robot.rightBack.setPower(rDrivePower);
}
@SuppressLint("SuspiciousIndentation")
@Override
public void exec() {
/* lDrivePower = 0.3;
robot.leftFront.setPower(lDrivePower);
robot.leftBack.setPower(lDrivePower);
robot.rightFront.setPower(lDrivePower);
robot.rightBack.setPower(lDrivePower);
*/
if (robot.leftFront.getCurrentPosition() <= 10 && robot.leftFront.getCurrentPosition() >= -10 && System.currentTimeMillis() - lastMoveTime >= 3000) {
robot.leftFront.setTargetPosition(500);
robot.leftBack.setTargetPosition(500);
robot.rightFront.setTargetPosition(500);
robot.rightBack.setTargetPosition(500);
lDrivePower = 0.3;
rDrivePower = lDrivePower * 0.75;
robot.leftFront.setPower(lDrivePower);
robot.leftBack.setPower(lDrivePower);
robot.rightFront.setPower(rDrivePower);
robot.rightBack.setPower(rDrivePower);
lastMoveTime = System.currentTimeMillis();
} else if (robot.leftFront.getCurrentPosition() >= 500 && System.currentTimeMillis() - lastMoveTime >= 3000) {
robot.leftFront.setTargetPosition(0);
robot.leftBack.setTargetPosition(0);
robot.rightFront.setTargetPosition(0);
robot.rightBack.setTargetPosition(0);
lDrivePower = -0.3;
rDrivePower = lDrivePower * 0.75;
robot.leftFront.setPower(lDrivePower);
robot.leftBack.setPower(lDrivePower);
robot.rightFront.setPower(rDrivePower);
robot.rightBack.setPower(rDrivePower);
lastMoveTime = System.currentTimeMillis();
}
}
}

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

@@ -1,6 +1,7 @@
package org.timecrafters.CenterStage.Common;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
@@ -13,11 +14,25 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
public class SodiPizzaMinibotObject extends Robot {
public HardwareMap hardwareMap;
public DcMotor flDrive, frDrive, blDrive, brDrive;
public Servo shoulder, hand;
public DcMotor leftFront, rightFront, leftBack, rightBack;
public Servo shoulder, gripper;
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
public static double ARM_COLLECT = 0.0; // ~? degrees
public static double ARM_PRECOLLECT = 0.05; // ~? degrees
public static double ARM_DELIVER = 0.28; // ~? degrees
public static double ARM_STOW = 0.72; // ~? degrees
public static double ARM_HOVER_5_STACK = 0.10;
public static double ARM_HOVER_4_STACK = 0.08;
public static double ARM_HOVER_3_STACK = 0.07;
public static double ARM_HOVER_2_STACK = 0.06;
private CyberarmEngine engine;
public TimeCraftersConfiguration configuration;
@@ -30,11 +45,29 @@ public class SodiPizzaMinibotObject extends Robot {
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
//Motor defining
flDrive = engine.hardwareMap.dcMotor.get("FL Drive");
frDrive = engine.hardwareMap.dcMotor.get("FR Drive");
blDrive = engine.hardwareMap.dcMotor.get("BL Drive");
brDrive = engine.hardwareMap.dcMotor.get("BR Drive");
leftFront = engine.hardwareMap.dcMotor.get("leftFront");
rightFront = engine.hardwareMap.dcMotor.get("rightFront");
leftBack = engine.hardwareMap.dcMotor.get("leftBack");
rightBack = engine.hardwareMap.dcMotor.get("rightBack");
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
rightBack.setDirection(DcMotorSimple.Direction.FORWARD);
leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//Servo Defining
shoulder = engine.hardwareMap.servo.get("arm");
gripper = engine.hardwareMap.servo.get("gripper");
}
}