TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-16 01:02:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

worked on righting the correct structure for the arm sequences and positions. Started to fix and simplify some functions so my code can be more modular.

Browse Source
This commit is contained in:
SpencerPiha
2023-10-24 18:41:30 -05:00
parent 2d48b57e4f
commit c32ab723af
4 changed files with 204 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java Normal file
View File

@@ -0,0 +1,26 @@
package org.timecrafters.CenterStage.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.timecrafters.CenterStage.Autonomous.States.DepositorArmPosState;
import org.timecrafters.CenterStage.Common.PrototypeRobot;
import org.timecrafters.CenterStage.Common.ServoTestRobot;
import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Servo Test", group = "Testing")
public class ServoTestEngine extends CyberarmEngine {
ServoTestRobot robot;
@Override
public void setup() {
this.robot = new ServoTestRobot("Hello World");
this.robot.setup();
addState(new DepositorArmPosState(robot, "ServoPositions", "01-0"));
}
}

91
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java
View File

@@ -1,50 +1,91 @@
package org.timecrafters.CenterStage.Autonomous.States;
import org.timecrafters.CenterStage.Common.PrototypeRobot;
import org.timecrafters.CenterStage.Common.ServoTestRobot;
import dev.cyberarm.engine.V2.CyberarmState;
public class DepositorArmPosState extends CyberarmState {
private final boolean stateDisabled;
PrototypeRobot robot;
ServoTestRobot robot;
private long startOfSequencerTime;
private long totalWaitedTime;
private long lastMeasuredTime;
private boolean actionsFinished = false;
public DepositorArmPosState(PrototypeRobot robot, String groupName, String actionName) {
private int armPosition;
public DepositorArmPosState(ServoTestRobot robot, String groupName, String actionName) {
this.robot = robot;
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
armPosition = robot.configuration.variable(groupName, actionName, "armPosition").value();
}
@Override
public void start() {
//add variables that need to be reinitillized
startOfSequencerTime = System.currentTimeMillis();
}
@Override
public void exec() {
if (robot.armPosition == 0) { // transfer mode
if (robot.oldArmPosititon == 0){
setHasFinished(true);
} else if (robot.oldArmPosititon == 1){
robot.depositorElbow.setPosition(robot.ELBOW_COLLECT);
} else if (robot.oldArmPosititon == 2){
if (actionsFinished) {
setHasFinished(true);
} else {
lastMeasuredTime = System.currentTimeMillis();
if (armPosition == robot.oldArmPosititon){
actionsFinished = true;
} else if (robot.oldArmPosititon == 1){
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN);
robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN);
totalWaitedTime = robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime){
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN);
totalWaitedTime += robot.servoWaitTime;
if (System.currentTimeMillis() - lastMeasuredTime >= totalWaitedTime){
actionsFinished = true;
}
}
}
// } else if (robot.armPosition == 1) { // drive mode
// if (robot.oldArmPosititon == 1){
// setHasFinished(true);
// } else if (robot.oldArmPosititon == 0){
//
// } else if (robot.oldArmPosititon == 2){
//
// }
// } else if (robot.armPosition == 2) { // deposit mode
// if (robot.oldArmPosititon == 2){
// setHasFinished(true);
// } else if (robot.oldArmPosititon == 0){
//
// } else if (robot.oldArmPosititon == 1){
//
// }
// -----------------------------------------------------------------------------------------------------0, drive to transfer
// if already at 0, actions have finished = true
// else if at 1,
// drive collector shoulder to position
// do math and wait till wait time is met
// once met, drive the elbow
// set has finished = true
// else if at 2,
// drive collector shoulder to position
// do math and wait till wait time is met
// once met, drive the elbow
// set has finished = true
// else if at 3,
// drive collector shoulder to position
// do math and wait till wait time is met
// once met, drive the elbow
// set has finished = true
// -----------------------------------------------------------------------------------------------------1, drive to driving mode
// -----------------------------------------------------------------------------------------------------2, drive to collect
// -----------------------------------------------------------------------------------------------------3, drive to deposit
}
}
}

7
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -19,11 +19,14 @@ public class PrototypeRobot extends Robot {
public int armPosition = 0;
public int oldArmPosititon;
public long waitTime;
public double servoWaitTime;
public double servoSecPerDeg = 0.14/60;
public float ELBOW_COLLECT;
public float ELBOW_DRIVING;
public float ELBOW_DEPOSIT;
public float SHOULDER_COLLECT;
public float SHOULDER_DRIVING;
public float SHOULDER_DEPOSIT;
public float lastSetPosShoulder = SHOULDER_COLLECT;
public float lastSetPosElbow = ELBOW_COLLECT;
@@ -82,6 +85,8 @@ public class PrototypeRobot extends Robot {
ELBOW_COLLECT = configuration.variable("Robot", "Tuning", "ELBOW_COLLECT").value();
SHOULDER_DEPOSIT = configuration.variable("Robot", "Tuning", "SHOULDER_DEPOSIT").value();
SHOULDER_COLLECT = configuration.variable("Robot", "Tuning", "SHOULDER_COLLECT").value();
SHOULDER_DRIVING = configuration.variable("Robot", "Tuning", "SHOULDER_DRIVING").value();
ELBOW_DRIVING = configuration.variable("Robot", "Tuning", "ELBOW_DRIVING").value();
}
@Override
@@ -99,7 +104,7 @@ public class PrototypeRobot extends Robot {
backLeft = new MotorEx(hardwareMap, "backLeft");
lift = new MotorEx(hardwareMap, "lift");
configuration = new TimeCraftersConfiguration("Robbie");
configuration = new TimeCraftersConfiguration("Blue Crab");
initConstants();

106
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java Normal file
View File

@@ -0,0 +1,106 @@
package org.timecrafters.CenterStage.Common;
import com.arcrobotics.ftclib.drivebase.HDrive;
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 com.qualcomm.robotcore.hardware.Servo;
import org.timecrafters.Library.Robot;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
public class ServoTestRobot extends Robot {
public int oldArmPosititon;
public long servoWaitTime;
public double servoSecPerDeg = 0.14/60;
public long waitTime;
public float DEPOSITOR_SHOULDER_IN;
public float DEPOSITOR_SHOULDER_OUT;
public float DEPOSITOR_ELBOW_IN;
public float DEPOSITOR_ELBOW_OUT;
public float COLLECTOR_SHOULDER_IN;
public float COLLECTOR_SHOULDER_PASSIVE;
public float COLLECTOR_SHOULDER_OUT;
public float COLLECTOR_ELBOW_IN;
public float COLLECTOR_ELBOW_PASSIVE;
public float COLLECTOR_ELBOW_OUT;
public float currentSetPosShoulder;
public float currentSetPosElbow;
private HardwareMap hardwareMap;
public IMU imu;
public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow;
private String string;
private CyberarmEngine engine;
public TimeCraftersConfiguration configuration;
public ServoTestRobot(String string) {
this.engine = engine;
setup();
this.string = string;
}
public void initConstants(){
DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value();
DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value();
DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value();
DEPOSITOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_OUT").value();
COLLECTOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_IN").value();
COLLECTOR_SHOULDER_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_PASSIVE").value();
COLLECTOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_OUT").value();
COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value();
COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value();
COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value();
}
@Override
public void setup() {
System.out.println("Bacon: " + this.string);
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
this.engine = CyberarmEngine.instance;
imu = engine.hardwareMap.get(IMU.class, "imu");
configuration = new TimeCraftersConfiguration("Blue Crab");
initConstants();
//IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
//SERVO
depositorShoulder = hardwareMap.servo.get("depositor_shoulder");
depositorElbow = hardwareMap.servo.get("depositor_elbow");
collectorShoulder = hardwareMap.servo.get("collector_shoulder");
collectorElbow = hardwareMap.servo.get("collector_elbow");
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN);
depositorElbow.setPosition(DEPOSITOR_ELBOW_IN);
collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
collectorElbow.setPosition(COLLECTOR_ELBOW_IN);
}
public void ServoWaitTime(Float lastSetPos, Float currentSetPos){
servoWaitTime = (long) (servoSecPerDeg * (Math.abs(lastSetPos - currentSetPosShoulder)));
}
}