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-02 20:30:55 -05:00
parent 28bb7ab674 09280688fb
commit c7accb3a4d
7 changed files with 294 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,12 +7,12 @@ import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Autonomous- Sodi", group = "Simple Test")
@Autonomous(name = "Autonomous- Sodi", group = "Arm Sequence")
public class ProtoBotEngineSodi extends CyberarmEngine {
private ProtoBotSodi robot;
@Override
public void setup() {
this.robot = new ProtoBotSodi("Autonomous - Sodi");
this.robot = new ProtoBotSodi("Autonomous- Sodi");
this.robot.setup();
addState(new ProtoBotStateSodi(robot));

54
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java
View File

@@ -1,5 +1,9 @@
package org.timecrafters.CenterStage.Autonomous.States;
/**BEGAN WITH 43 LINES**/
import com.arcrobotics.ftclib.hardware.motors.Motor;
import org.timecrafters.CenterStage.Common.ProtoBotSodi;
import org.timecrafters.CenterStage.Common.PrototypeRobot;
@@ -10,12 +14,24 @@ public class ArmStateSodi extends CyberarmState {
private final boolean stateDisabled;
ProtoBotSodi robot;
private int testSequence;
private int targetPos;
private int currentPos;
private int totalDist;
public ArmStateSodi(ProtoBotSodi robot, String groupName, String actionName) {
this.robot = robot;
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private int getTotalDist() {
int totalDist = targetPos - currentPos;
return totalDist;
}
@Override
public void start() {
//add variables that need to be reinitialized
@@ -23,18 +39,34 @@ public class ArmStateSodi extends CyberarmState {
}
@Override
public void exec() {
if (robot.liftMotor.motor.getCurrentPosition() < 0) {
robot.liftMotor.motor.setPower(0);
}
if (robot.liftMotor.motor.getCurrentPosition() >= 0 && robot.liftMotor.motor.getCurrentPosition() <= 49) {
}
if (robot.liftMotor.motor.getCurrentPosition() >= 50 && robot.liftMotor.motor.getCurrentPosition() <= 250) {
}
public void init() {
}
@Override
public void exec() {
// if (robot.liftMotor.motor.getCurrentPosition() >= 2800 &&
// Math.abs(robot.liftMotor.motor.getPower()) != robot.liftMotor.motor.getPower()) {
// robot.liftMotor.motor.setPower(0);
// }
//
// if (robot.liftMotor.motor.getCurrentPosition() < 0 &&
// Math.abs(robot.liftMotor.motor.getPower()) != robot.liftMotor.motor.getPower()) {
// robot.liftMotor.motor.setPower(0);
// }
//
// if (robot.liftMotor.motor.getCurrentPosition() >= 0 &&
// robot.liftMotor.motor.getCurrentPosition() <= 50) {
//
// }
//
// if (robot.liftMotor.motor.getCurrentPosition() >= 50 &&
// robot.liftMotor.motor.getCurrentPosition() <= 250) {
//
// }
//
//
}
}

106
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java
View File

@@ -1,5 +1,6 @@
package org.timecrafters.CenterStage.Autonomous.States;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
@@ -10,15 +11,16 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi;
public class ProtoBotStateSodi extends CyberarmState {
ProtoBotSodi robot;
private double avgVelocity, avgDrivePower;
private long lastTimeChecked;
private int testSequence;
private int targetPos;
private int currentPos;
private int totalDist;
public ProtoBotStateSodi(ProtoBotSodi robot) {
this.robot = robot;
}
public void telemetry() {
// engine.telemetry.addData("Avg Drive Velocity", avgVelocity);
// engine.telemetry.addData("Avg Drive Power", avgDrivePower);
engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity());
engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity());
engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity());
@@ -27,18 +29,15 @@ public class ProtoBotStateSodi extends CyberarmState {
engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower());
engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower());
engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower());
engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition());
engine.telemetry.addData("Test Sequence", testSequence);
}
// public double getAvgDrivePower() {
// avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4;
// return avgDrivePower;
// }
// public double getAvgVelocity() {
// avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4;
// return avgVelocity;
// }
@Override
public void start() {
testSequence = 0;
}
@Override
public void init() {
@@ -46,69 +45,54 @@ public class ProtoBotStateSodi extends CyberarmState {
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
robot.liftMotor.motor.setPower(0);
robot.grabJaw.setPosition(0);
robot.grabElbow.setPosition(0);
robot.grabShoulder.setPosition(0);
robot.dropShoulder.setPosition(0);
robot.dropElbow.setPosition(0);
robot.dropJaw.setPosition(0);
// robot.liftMotor.motor.setPower(0);
//
// robot.grabJaw.setPosition(0);
// robot.grabElbow.setPosition(0);
// robot.grabShoulder.setPosition(0);
// robot.dropShoulder.setPosition(0);
// robot.dropElbow.setPosition(0);
// robot.dropJaw.setPosition(0);
lastTimeChecked = System.currentTimeMillis();
testSequence = 0;
}
@Override
public void exec() {
//
// if (System.currentTimeMillis() - lastTimeChecked >= 500 && System.currentTimeMillis() - lastTimeChecked < 2500) {
// robot.flDrive.motor.setPower(0.5);
// robot.frDrive.motor.setPower(0.5);
// robot.blDrive.motor.setPower(0.5);
// robot.brDrive.motor.setPower(0.5);
// robot.liftMotor.motor.setPower(0.5);
// } else if (System.currentTimeMillis() - lastTimeChecked >= 2500 && System.currentTimeMillis() - lastTimeChecked < 4500) {
// robot.flDrive.motor.setPower(-0.5);
// robot.frDrive.motor.setPower(-0.5);
// robot.blDrive.motor.setPower(-0.5);
// robot.brDrive.motor.setPower(-0.5);
// robot.liftMotor.motor.setPower(-0.5);
// } else if (System.currentTimeMillis() - lastTimeChecked >= 4500 && System.currentTimeMillis() - lastTimeChecked < 6500) {
// robot.flDrive.motor.setPower(0.5);
// robot.frDrive.motor.setPower(0.5);
// robot.blDrive.motor.setPower(-0.5);
// robot.brDrive.motor.setPower(-0.5);
// robot.liftMotor.motor.setPower(0);
// } else if (System.currentTimeMillis() - lastTimeChecked >= 6500 && System.currentTimeMillis() - lastTimeChecked < 8500) {
// robot.flDrive.motor.setPower(-0.5);
// robot.frDrive.motor.setPower(-0.5);
// robot.blDrive.motor.setPower(0.5);
// robot.brDrive.motor.setPower(0.5);
// robot.liftMotor.motor.setPower(0);
// } else if (System.currentTimeMillis() - lastTimeChecked >= 8600){
// robot.flDrive.motor.setPower(0);
// robot.frDrive.motor.setPower(0);
// robot.blDrive.motor.setPower(0);
// robot.brDrive.motor.setPower(0);
// robot.liftMotor.motor.setPower(0);
// setHasFinished(true);
// }
switch (testSequence) {
case 1:
currentPos = robot.flDrive.motor.getCurrentPosition();
//lift motor go up for some way
//wait for about 0.25
if (testSequence < 1) {
testSequence = 1;
}
case 2:
if (testSequence == 1) {
robot.flDrive.motor.setTargetPosition(1000);
robot.frDrive.motor.setTargetPosition(1000);
robot.blDrive.motor.setTargetPosition(1000);
robot.brDrive.motor.setTargetPosition(1000);
//lift motor go down
//repeat
//wait for about 0.25
if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) {
robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition()));
robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition()));
robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition()));
robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition()));
} else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 ||
robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) {
robot.flDrive.motor.setPower(0);
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
testSequence = 2;
}
}

63
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java Normal file
View File

@@ -0,0 +1,63 @@
package org.timecrafters.CenterStage.Common;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
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 org.timecrafters.Library.Robot;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
public class MiniYTeleOPBot extends Robot {
public HardwareMap hardwareMap;
public MotorEx flDrive, frDrive, blDrive, brDrive;
public IMU imu;
private String string;
public TimeCraftersConfiguration configuration;
public MiniYTeleOPBot() {
}
@Override
public void setup() {
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
CyberarmEngine engine = CyberarmEngine.instance;
imu = engine.hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
//Motors
frDrive = new MotorEx(hardwareMap, "FrontRight");
flDrive = new MotorEx(hardwareMap, "FrontLeft");
brDrive = new MotorEx(hardwareMap, "BackRight");
blDrive = new MotorEx(hardwareMap, "BackLeft");
flDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD);
frDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD);
blDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD);
brDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD);
flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}

28
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java
View File

@@ -28,22 +28,20 @@ public class ProtoBotSodi extends Robot {
public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor;
public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw;
private String string;
private CyberarmEngine engine;
public TimeCraftersConfiguration configuration;
public ProtoBotSodi(String string) {
this.engine = engine;
this.string = string;
}
@Override
public void setup() {System.out.println("Bacon: " + this.string);
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
this.engine = CyberarmEngine.instance;
CyberarmEngine engine = CyberarmEngine.instance;
TimeCraftersConfiguration configuration = new TimeCraftersConfiguration("Robbie");
// TimeCraftersConfiguration configuration = new TimeCraftersConfiguration();
//Motors
frDrive = new MotorEx(hardwareMap, "FrontRight");
@@ -52,16 +50,22 @@ public class ProtoBotSodi extends Robot {
blDrive = new MotorEx(hardwareMap, "BackLeft");
liftMotor = new MotorEx(hardwareMap, "Lift");
flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftMotor.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flDrive.motor.setDirection(FORWARD);
frDrive.motor.setDirection(REVERSE);
// frDrive.motor.setDirection(REVERSE);
blDrive.motor.setDirection(FORWARD);
brDrive.motor.setDirection(REVERSE);
// brDrive.motor.setDirection(REVERSE);
//Servos
grabJaw = hardwareMap.servo.get("GrabJaw");
@@ -70,7 +74,7 @@ public class ProtoBotSodi extends Robot {
dropShoulder = hardwareMap.servo.get("DropShoulder");
dropElbow = hardwareMap.servo.get("DropElbow");
dropJaw = hardwareMap.servo.get("DropJaw");
grabElbow.setDirection(Servo.Direction.FORWARD);
grabJaw.setDirection(Servo.Direction.FORWARD);
grabShoulder.setDirection(Servo.Direction.FORWARD);

21
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java Normal file
View File

@@ -0,0 +1,21 @@
package org.timecrafters.CenterStage.TeleOp.Engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.timecrafters.CenterStage.Common.MiniYTeleOPBot;
import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP;
import dev.cyberarm.engine.V2.CyberarmEngine;
@TeleOp(name = "A Yellow Minibot")
public class MiniYTeleOPEngine extends CyberarmEngine {
private MiniYTeleOPBot robot;
@Override
public void setup() {
this.robot = new MiniYTeleOPBot();
this.robot.setup();
addState(new YellowMinibotTeleOP(robot));
}
}

104
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java Normal file
View File

@@ -0,0 +1,104 @@
package org.timecrafters.CenterStage.TeleOp.States;
import dev.cyberarm.engine.V2.CyberarmState;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import dev.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.CenterStage.Common.MiniYTeleOPBot;
public class YellowMinibotTeleOP extends CyberarmState {
private final MiniYTeleOPBot robot;
public float angleDelta, drivePower;
YawPitchRollAngles imuInitAngle;
public YellowMinibotTeleOP(MiniYTeleOPBot robot) {
this.robot = robot;
}
public float getAngleDelta() {
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return angleDelta;
}
@Override
public void telemetry() {
engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity());
engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity());
engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity());
engine.telemetry.addData("Back Right Velocity", robot.brDrive.getVelocity());
engine.telemetry.addData("Front Left Power", robot.flDrive.motor.getPower());
engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower());
engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower());
engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower());
engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition());
engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
}
@Override
public void init() {
drivePower = 0;
robot.flDrive.motor.setPower(0);
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
robot.imu.resetYaw();
imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
}
@Override
public void exec() {
if (Math.abs(engine.gamepad1.right_stick_y) < 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
drivePower = 0;
robot.flDrive.motor.setPower(0);
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
}
if (engine.gamepad1.start && !engine.gamepad1.a) {
robot.flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.imu.resetYaw();
}
if (Math.abs(engine.gamepad1.right_stick_y) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1) {
drivePower = engine.gamepad1.right_stick_y;
robot.flDrive.motor.setPower(drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(drivePower);
robot.brDrive.motor.setPower(drivePower);
}
if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
drivePower = engine.gamepad1.right_stick_x;
robot.flDrive.motor.setPower(-drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(drivePower);
robot.brDrive.motor.setPower(-drivePower);
}
if (Math.abs(engine.gamepad1.left_stick_x) > 0.1){
drivePower = engine.gamepad1.left_stick_x;
robot.flDrive.motor.setPower(-drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(-drivePower);
robot.brDrive.motor.setPower(drivePower);
}
}
}