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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2023-10-07 12:31:26 -05:00
parent b8db588c27 c4df4f7795
commit b8ae610362
2 changed files with 61 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -17,11 +17,16 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import dev.cyberarm.engine.V2.CyberarmEngine;
public class PrototypeRobot extends Robot {
public double servoWaitTime;
public double servoSecPerDeg = 0.14/60;;
public float ELBOW_COLLECT;
public float ELBOW_DEPOSIT;
public float SHOULDER_COLLECT;
public float SHOULDER_DEPOSIT;
public float lastSetPosShoulder = SHOULDER_COLLECT;
public float lastSetPosElbow = ELBOW_COLLECT;
public float currentSetPosShoulder;
public float currentSetPosElbow;
private HardwareMap hardwareMap;
public MotorEx frontLeft, frontRight, backLeft, backRight, lift;
public IMU imu;
@@ -91,9 +96,24 @@ public class PrototypeRobot extends Robot {
xDrive = new HDrive(frontLeft, frontRight,
backLeft, backRight);
depositorShoulder.setPosition(SHOULDER_COLLECT);
depositorElbow.setPosition(ELBOW_COLLECT);
}
public void driveTrainTeleOp() {
xDrive.driveRobotCentric(-engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, -engine.gamepad1.right_stick_x);
}
}
public void ShoulderServoWaitTime(){
servoWaitTime = servoSecPerDeg * (Math.abs(lastSetPosShoulder - currentSetPosShoulder));
}
public void ElbowServoWaitTime(){
servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs(lastSetPosElbow - currentSetPosElbow)));
}
}

39
TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
View File

@@ -10,9 +10,48 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
private int maxExtension = 2000;
private int minExtension = 0;
private long lastCheckedTime;
public PrototypeRobotDrivetrainState(PrototypeRobot robot) {
this.robot = robot;
}
private void depositorAutomation(){
// TODO: 10/7/2023 Workout Logic to move each limb step by step
lastCheckedTime = System.currentTimeMillis();
if (engine.gamepad2.a){
// setting Servo Positions to do time Math
robot.currentSetPosShoulder = robot.SHOULDER_COLL
robot.depositorShoulder.setPosition(robot.currentSetPosShoulder);
// running math function to determine time
robot.ShoulderServoWaitTime();
// determining if the time is met to do the next action
if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){
robot.lastSetPosShoulder = robot.currentSetPosShoulder;
// setting Servo Positions to do time Math
robot.currentSetPosElbow = robot.ELBOW_COLLECT;
robot.depositorElbow.setPosition(robot.currentSetPosElbow);
robot.lastSetPosElbow = robot.currentSetPosElbow;
}
}
if (engine.gamepad2.y){
// setting Servo Positions to do time Math
robot.currentSetPosShoulder = robot.SHOULDER_DEPOSIT;
robot.depositorShoulder.setPosition(robot.currentSetPosShoulder);
// running math function to determine time
robot.ShoulderServoWaitTime();
// determining if the time is met to do the next action
if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){
robot.lastSetPosShoulder = robot.currentSetPosShoulder;
// setting Servo Positions to do time Math
robot.currentSetPosElbow = robot.ELBOW_COLLECT;
robot.depositorElbow.setPosition(robot.currentSetPosElbow);
robot.lastSetPosElbow = robot.currentSetPosElbow;
}
}
}
// --------------------------------------------------------------------------------------------------------- Depositor control function
private void depositorTeleOp(){