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

Debugging arm motor

Browse Source
This commit is contained in:
Sodi
2023-02-04 14:05:10 -06:00
parent 365ed7a602
commit b5117dc045
3 changed files with 77 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java
View File

@@ -18,7 +18,6 @@ public class PhoenixTeleOP extends CyberarmEngine {
public void setup() { public void setup() {
robot = new PhoenixBot1(this); robot = new PhoenixBot1(this);
// addState(new PhoenixTeleOPState(robot));
addState(new TeleOPArmDriver(robot)); addState(new TeleOPArmDriver(robot));
addParallelStateToLastState(new TeleOPTankDriver(robot)); addParallelStateToLastState(new TeleOPTankDriver(robot));
} }

8
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -227,10 +227,10 @@ public class PhoenixBot1 {
// HighRiserRight.setDirection(Servo.Direction.FORWARD); // HighRiserRight.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE); LowRiserRight.setDirection(Servo.Direction.REVERSE);
// ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE); ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
// ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
CameraServo.setDirection(Servo.Direction.FORWARD); CameraServo.setDirection(Servo.Direction.FORWARD);

118
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java
View File

@@ -1,6 +1,8 @@
package org.timecrafters.TeleOp.states; package org.timecrafters.TeleOp.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.CyberarmState;
@@ -29,6 +31,7 @@ public class TeleOPArmDriver extends CyberarmState {
private int armMotorMed = 380; private int armMotorMed = 380;
private int armMotorHigh = 463; private int armMotorHigh = 463;
private int ArmMotorStepSize = 2; private int ArmMotorStepSize = 2;
private int TargetPosition = 0;
public TeleOPArmDriver(PhoenixBot1 robot) { public TeleOPArmDriver(PhoenixBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -53,7 +56,8 @@ public class TeleOPArmDriver extends CyberarmState {
robot.LowRiserRight.setPosition(0.45); robot.LowRiserRight.setPosition(0.45);
robot.ArmMotor.setTargetPosition(0); robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setPower(0.5); robot.ArmMotor.setPower(0.5);
Opportunity = 0; robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
JunctionHeight = 0; JunctionHeight = 0;
} }
@@ -62,8 +66,8 @@ public class TeleOPArmDriver extends CyberarmState {
@Override @Override
public void exec() { public void exec() {
Spirit = System.currentTimeMillis(); double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 400.0 + 0.1;
double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) / 400.0 + 0.1;
// ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25; // ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
armPower = ratio; armPower = ratio;
@@ -71,24 +75,28 @@ public void exec() {
if (engine.gamepad2.y) { if (engine.gamepad2.y) {
JunctionHeight = 4; JunctionHeight = 4;
TargetPosition = armMotorHigh;
} }
if (engine.gamepad2.b) { if (engine.gamepad2.b) {
JunctionHeight = 3; JunctionHeight = 3;
TargetPosition = armMotorMed;
} }
if (engine.gamepad2.x) { if (engine.gamepad2.x) {
JunctionHeight = 2; JunctionHeight = 2;
TargetPosition = armMotorLow;
} }
if (engine.gamepad2.a) { if (engine.gamepad2.a) {
JunctionHeight = 1; JunctionHeight = 1;
TargetPosition = armMotorCollect;
} }
if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) { if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(armMotorHigh); robot.ArmMotor.setTargetPosition(armMotorHigh);
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ { if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ {
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize);
} }
if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { else if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} }
@@ -107,28 +115,28 @@ public void exec() {
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { else if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() <= servoMedLow && else if (robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ { robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - ArmMotorStepSize);
} }
} }
if (robot.LowRiserLeft.getPosition() < servoMedLow && else if (robot.LowRiserLeft.getPosition() < servoMedLow &&
robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ { robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize);
} }
} }
if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && else if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow && robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorMed) { robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
JunctionHeight = 0; JunctionHeight = 0;
@@ -144,29 +152,29 @@ public void exec() {
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ { else if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() <= servoLowLow && else if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.LowRiserLeft.getPosition() > servoLowLow - 5 && robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - ArmMotorStepSize);
} }
} }
if (robot.LowRiserLeft.getPosition() <= servoLowLow && else if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ { robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize);
} }
} }
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && else if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow && robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorLow) { robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
JunctionHeight = 0; JunctionHeight = 0;
@@ -185,7 +193,7 @@ public void exec() {
robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - ArmMotorStepSize);
} }
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
@@ -193,45 +201,65 @@ public void exec() {
} }
} }
if (engine.gamepad2.dpad_left && Ingenuity != 1) { // if (engine.gamepad2.dpad_left && Ingenuity != 1) {
Ingenuity = 1; // Ingenuity = 1;
} // }
//
if (engine.gamepad2.dpad_right && Ingenuity != 2) { // if (engine.gamepad2.dpad_right && Ingenuity != 2) {
Ingenuity = 2; // Ingenuity = 2;
} // }
//
if (engine.gamepad2.dpad_left && Ingenuity == 1 || engine.gamepad2.dpad_right && Ingenuity == 2) { // if (Ingenuity == 1) {
robot.collectorLeft.setPower(0); // robot.collectorRight.setPower(-1);
robot.collectorRight.setPower(0); // robot.collectorLeft.setPower(-1);
Ingenuity = 0; // }
} //
// if (Ingenuity == 2) {
if (Ingenuity == 1) { // robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(1); // robot.collectorRight.setPower(1);
robot.collectorLeft.setPower(-1); // }
}
if (Ingenuity == 2) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(-1);
}
if (engine.gamepad2.right_trigger > 0.1) { if (engine.gamepad2.right_trigger > 0.1) {
JunctionHeight = 0; JunctionHeight = 0;
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100 && robot.ArmMotor.getCurrentPosition() < armMotorHigh + 5) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + 50); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + 1);
robot.ArmMotor.setPower(armPower); robot.ArmMotor.setPower(armPower);
} else if (engine.gamepad2.left_trigger > 0.1) { } else if (engine.gamepad2.left_trigger > 0.1) {
JunctionHeight = 0; JunctionHeight = 0;
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100 && robot.ArmMotor.getCurrentPosition() > armMotorCollect - 5) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - 50); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - 1);
robot.ArmMotor.setPower(armPower); robot.ArmMotor.setPower(armPower);
} }
} }
} }
} }
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (engine.gamepad2 == gamepad) {
if (button.equals("dpad_left")) {
robot.collectorRight.setPower(-1);
robot.collectorLeft.setPower(-1);
} else if (button.equals("dpad_right")) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(1);
}
}
}
public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad2 == gamepad) {
if (button.equals("dpad_left")) {
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
} else if (button.equals("dpad_right")) {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
}
}
}
} }