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

almost finished with arm sequences, got about 90% done, plan is program arm positions next meeting

Browse Source
This commit is contained in:
SpencerPiha
2023-11-02 20:29:42 -05:00
parent a22b36142d
commit 28bb7ab674
4 changed files with 351 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,10 +11,11 @@ public class DepositorArmPosState extends CyberarmState {
ServoTestRobot robot;
private long startOfSequencerTime;
private long totalWaitedTime = 0;
private long lastMeasuredTime = System.currentTimeMillis();
private boolean actionsFinished = false;
private boolean enteredLoop = false;
private int armPosition;
private int armPosition = 0;
private int oldArmPosition;
public DepositorArmPosState(ServoTestRobot robot, String groupName, String actionName) {
this.robot = robot;
@@ -30,83 +31,256 @@ public class DepositorArmPosState extends CyberarmState {
@Override
public void exec() {
lastMeasuredTime = System.currentTimeMillis();
if (armPosition == robot.oldArmPosititon) {
actionsFinished = true;
} else if (robot.oldArmPosititon == 1) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
totalWaitedTime = robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - lastMeasuredTime >= totalWaitedTime) {
setHasFinished(true);
switch (armPosition) {
case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos
switch (oldArmPosition) {
case 0:
// transfer
break;
case 10:
// driving
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= totalWaitedTime) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
enteredLoop = true;
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
totalWaitedTime += (long) robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= totalWaitedTime) {
// setHasFinished(true);
}
}
break;
case 20:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 30:
// deposit
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
setHasFinished(true);
}
}
}
}
break;
}
}
} else if (robot.oldArmPosititon == 2) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
totalWaitedTime = robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - lastMeasuredTime >= totalWaitedTime) {
setHasFinished(true);
}
}
} else if (robot.oldArmPosititon == 3) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
totalWaitedTime = robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= robot.servoWaitTime) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - lastMeasuredTime >= totalWaitedTime) {
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait }
if (System.currentTimeMillis() - lastMeasuredTime >= totalWaitedTime) {
break;
case 10:// ----------------------------------------------------------------------------------------------- drive to driving pos
switch (oldArmPosition) {
case 0:
// transfer
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 10:
// drive pos
setHasFinished(true);
}
break;
case 20:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2100) {
setHasFinished(true);
}
}
break;
case 30:
// deposit
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_PASSIVE, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1600) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 3100) {
setHasFinished(true);
}
}
}
}
break;
}
break;
// -----------------------------------------------------------------------------------------------------0, drive to transfer
// if already at 0, actions have finished = true
case 20:// ----------------------------------------------------------------------------------------------- drive to collect pos
switch (oldArmPosition) {
case 0:
// transfer
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_OUT);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 10:
// driving
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_OUT);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 20:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 30:
// deposit
robot.depositorShoulder.setPosition(robot.DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position
if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
robot.depositorElbow.setPosition(robot.DEPOSITOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_PASSIVE, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_OUT); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_OUT);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 100) {
setHasFinished(true);
}
}
}
}
break;
}
break;
// 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
case 30:// ----------------------------------------------------------------------------------------------- drive to deposit pos
switch (oldArmPosition) {
case 0:
// transfer
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 10:
// driving
robot.collectorShoulder.setPosition(robot.COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_SHOULDER_PASSIVE);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 2000) {
setHasFinished(true);
}
}
break;
case 20:
// collect
robot.collectorShoulder.setPosition(robot.COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position
// robot.ServoWaitTime(robot.COLLECTOR_SHOULDER_OUT, robot.COLLECTOR_SHOULDER_IN); // calculate time to move
// totalWaitedTime = (long) robot.servoWaitTime;
if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met
robot.collectorElbow.setPosition(robot.COLLECTOR_ELBOW_IN);
// robot.ServoWaitTime(robot.COLLECTOR_ELBOW_OUT, robot.COLLECTOR_ELBOW_IN); // calculate time to move
// totalWaitedTime += robot.servoWaitTime; // add the time to the total time to wait
if (System.currentTimeMillis() - startOfSequencerTime >= 1500) {
setHasFinished(true);
}
}
break;
case 30:
// deposit
break;
}
break;
// 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
}
}
}
}
}
@Override
public void telemetry(){
engine.telemetry.addData("Current Time",System.currentTimeMillis()-startOfSequencerTime);
engine.telemetry.addData("servo wait time",robot.servoWaitTime);
engine.telemetry.addData("servo wait time (long)", (long)robot.servoWaitTime);
engine.telemetry.addData("entered loop",enteredLoop);
}
}

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

@@ -22,21 +22,25 @@ public class PrototypeRobot extends Robot {
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;
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 lastSetPosShoulder;
public float lastSetPosElbow;
public float currentSetPosShoulder;
public float currentSetPosElbow;
private HardwareMap hardwareMap;
public MotorEx frontLeft, frontRight, backLeft, backRight, lift;
public DcMotor odometerR, odometerL, odometerA;
public IMU imu;
public Servo depositorShoulder, depositorElbow, depositor;
// public Servo depositorShoulder, depositorElbow, depositor;
private HDrive xDrive;
private String string;
public double xMultiplier = 1;
@@ -81,12 +85,16 @@ public class PrototypeRobot extends Robot {
}
public void initConstants(){
ELBOW_DEPOSIT = configuration.variable("Robot", "Tuning", "ELBOW_DEPOSIT").value();
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();
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
@@ -98,11 +106,11 @@ public class PrototypeRobot extends Robot {
imu = engine.hardwareMap.get(IMU.class, "imu");
//MOTORS
frontRight = new MotorEx(hardwareMap, "frontRight");
frontLeft = new MotorEx(hardwareMap, "frontLeft");
backRight = new MotorEx(hardwareMap, "backRight");
backLeft = new MotorEx(hardwareMap, "backLeft");
lift = new MotorEx(hardwareMap, "lift");
frontRight = new MotorEx(hardwareMap, "FrontRight");
frontLeft = new MotorEx(hardwareMap, "FrontLeft");
backRight = new MotorEx(hardwareMap, "BackRight");
backLeft = new MotorEx(hardwareMap, "BackLeft");
lift = new MotorEx(hardwareMap, "Lift");
configuration = new TimeCraftersConfiguration("Blue Crab");
@@ -128,17 +136,17 @@ public class PrototypeRobot extends Robot {
imu.initialize(parameters);
//SERVO
depositorShoulder = hardwareMap.servo.get("depositor_shoulder");
depositorElbow = hardwareMap.servo.get("depositor_elbow");
depositor = hardwareMap.servo.get("depositor");
// //SERVO
// depositorShoulder = hardwareMap.servo.get("depositor_shoulder");
// depositorElbow = hardwareMap.servo.get("depositor_elbow");
// depositor = hardwareMap.servo.get("depositor");
// input motors exactly as shown below
xDrive = new HDrive(frontLeft, frontRight,
backLeft, backRight);
depositorShoulder.setPosition(SHOULDER_COLLECT);
depositorElbow.setPosition(ELBOW_COLLECT);
// depositorShoulder.setPosition(COLLECTOR_SHOULDER_IN);
// depositorElbow.setPosition(COLLECTOR_ELBOW_IN);
}

14
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ServoTestRobot.java
View File

@@ -16,8 +16,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
public class ServoTestRobot extends Robot {
public int oldArmPosititon;
public long servoWaitTime;
public double servoSecPerDeg = 0.14/60;
public double servoWaitTime;
public double servoSecPerDeg = 0.14/(60/270);
public long waitTime;
public float DEPOSITOR_SHOULDER_IN;
@@ -89,16 +89,16 @@ public class ServoTestRobot extends Robot {
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);
depositorShoulder.setPosition(DEPOSITOR_SHOULDER_OUT);
depositorElbow.setPosition(DEPOSITOR_ELBOW_OUT);
collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE);
collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE);
}
public void ServoWaitTime(Float lastSetPos, Float currentSetPos){
servoWaitTime = (long) (servoSecPerDeg * (Math.abs(lastSetPos - currentSetPosShoulder)));
servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs((double)lastSetPos - (double)currentSetPos)));
}

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

@@ -18,66 +18,66 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
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.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;
}
}
}
// lastCheckedTime = System.currentTimeMillis();
//
// if (engine.gamepad2.a){
// // setting Servo Positions to do time Math
// 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(){
// flip arms
if (engine.gamepad1.x) {
// shoulder deposit
robot.depositorShoulder.setPosition(robot.SHOULDER_DEPOSIT);
// shoulder collect
} else if (engine.gamepad1.a) {
robot.depositorShoulder.setPosition(robot.SHOULDER_COLLECT);
}
if (engine.gamepad1.y){
// elbow deposit
robot.depositorElbow.setPosition(robot.ELBOW_DEPOSIT);
// elbow collect
} else if (engine.gamepad1.b){
robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); // Collect / transfer = 0
// private void depositorTeleOp(){
// // flip arms
// if (engine.gamepad1.x) {
// // shoulder deposit
// robot.depositorShoulder.setPosition(robot.SHOULDER_DEPOSIT);
// // shoulder collect
// } else if (engine.gamepad1.a) {
// robot.depositorShoulder.setPosition(robot.SHOULDER_COLLECT);
// }
// if (engine.gamepad1.y){
// // elbow deposit
// robot.depositorElbow.setPosition(robot.ELBOW_DEPOSIT);
// // elbow collect
// } else if (engine.gamepad1.b){
// robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); // Collect / transfer = 0
// }
// // depositor
// if (engine.gamepad1.right_bumper) {
// robot.depositor.setPosition(0.8);
// } else if (engine.gamepad1.left_bumper) {
// robot.depositor.setPosition(0.2);
}
// depositor
if (engine.gamepad1.right_bumper) {
robot.depositor.setPosition(0.8);
} else if (engine.gamepad1.left_bumper) {
robot.depositor.setPosition(0.2);
}
}
// }
// --------------------------------------------------------------------------------------------------------- Slider control function
private void sliderTeleOp(){
if (engine.gamepad1.right_trigger != 0){
@@ -108,7 +108,7 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
robot.driveTrainTeleOp();
// depositor
depositorTeleOp();
// depositorTeleOp();
// lift
sliderTeleOp();
@@ -120,9 +120,9 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
public void telemetry() {
engine.telemetry.addData("Lift Encoder Pos", robot.lift.motor.getCurrentPosition());
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Elbow Collect", robot.ELBOW_COLLECT);
engine.telemetry.addData("Elbow Deposit", robot.ELBOW_DEPOSIT);
engine.telemetry.addData("Shoulder Collect", robot.SHOULDER_COLLECT);
engine.telemetry.addData("Shoulder Deposit", robot.SHOULDER_DEPOSIT);
// engine.telemetry.addData("Elbow Collect", robot.ELBOW_COLLECT);
// engine.telemetry.addData("Elbow Deposit", robot.ELBOW_DEPOSIT);
// engine.telemetry.addData("Shoulder Collect", robot.SHOULDER_COLLECT);
// engine.telemetry.addData("Shoulder Deposit", robot.SHOULDER_DEPOSIT);
}
}