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

Debugging arm motor

Browse Source
This commit is contained in:
Sodi
2023-02-04 15:02:51 -06:00
parent b5117dc045
commit 555788cdf9
2 changed files with 52 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,6 +6,7 @@ import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
@@ -58,7 +59,9 @@ public class PhoenixBot1 {
public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance; public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor; public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal;
public DcMotorEx ArmMotor;
public CRServo collectorLeft, collectorRight; public CRServo collectorLeft, collectorRight;
@@ -175,7 +178,7 @@ public class PhoenixBot1 {
LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight"); LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight");
// HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft"); // HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft");
// HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight"); // HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight");
ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor"); ArmMotor = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor");
//motors direction and encoders //motors direction and encoders
@@ -227,7 +230,7 @@ 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.FORWARD);
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.FLOAT); ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);

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

@@ -26,7 +26,7 @@ public class TeleOPArmDriver extends CyberarmState {
private double servoHighLow = 0.8; //Low servos, Y button private double servoHighLow = 0.8; //Low servos, Y button
// private double servoHighHigh = 0.9; //High servos, Y button // private double servoHighHigh = 0.9; //High servos, Y button
private double ArmNeededPower; private double ArmNeededPower;
private int armMotorCollect = 0; private int armMotorCollect = -100;
private int armMotorLow = 240; private int armMotorLow = 240;
private int armMotorMed = 380; private int armMotorMed = 380;
private int armMotorHigh = 463; private int armMotorHigh = 463;
@@ -57,20 +57,53 @@ public class TeleOPArmDriver extends CyberarmState {
robot.ArmMotor.setTargetPosition(0); robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setPower(0.5); robot.ArmMotor.setPower(0.5);
robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.ArmMotor.setTargetPositionTolerance(5);
armPower = 0.5;
JunctionHeight = 0; JunctionHeight = 0;
} }
@Override @Override
public void exec() { public void exec() {
double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 400.0 + 0.1;
// ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
// armPower = 0.5;
robot.ArmMotor.setTargetPosition(TargetPosition);
robot.ArmMotor.setPower(armPower);
if (engine.gamepad2.y) {
JunctionHeight = 4;
TargetPosition = armMotorHigh;
}
if (engine.gamepad2.b) {
JunctionHeight = 3;
TargetPosition = armMotorMed;
}
if (engine.gamepad2.x) {
JunctionHeight = 2;
TargetPosition = armMotorLow;
armPower = -0.5;
}
if (engine.gamepad2.a) {
armPower = 1;
JunctionHeight = 1;
TargetPosition = armMotorCollect;
}
}
public void eexec() {
double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 400.0 + 0.1; double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - TargetPosition) / 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 = 0.5;
robot.ArmMotor.setTargetPosition(TargetPosition);
robot.ArmMotor.setPower(armPower); robot.ArmMotor.setPower(armPower);
if (engine.gamepad2.y) { if (engine.gamepad2.y) {
@@ -92,9 +125,9 @@ public void exec() {
if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) { if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
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.getCurrentPosition() + ArmMotorStepSize); // robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize);
} }
else 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);
@@ -107,7 +140,6 @@ public void exec() {
} }
if (JunctionHeight == 3) { if (JunctionHeight == 3) {
robot.ArmMotor.setTargetPosition(armMotorMed);
if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ { if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
@@ -126,14 +158,14 @@ public void exec() {
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.getCurrentPosition() - ArmMotorStepSize); // robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - ArmMotorStepSize);
} }
} }
else 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.getCurrentPosition() + ArmMotorStepSize); // robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize);
} }
} }
else if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && else if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
@@ -144,7 +176,7 @@ public void exec() {
} }
if (JunctionHeight == 2) { if (JunctionHeight == 2) {
robot.ArmMotor.setTargetPosition(armMotorLow);
if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ { if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
@@ -164,14 +196,14 @@ public void exec() {
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.getCurrentPosition() - ArmMotorStepSize); // robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - ArmMotorStepSize);
} }
} }
else 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.getCurrentPosition() + ArmMotorStepSize); // robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + ArmMotorStepSize);
} }
} }
else if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && else if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
@@ -182,7 +214,7 @@ public void exec() {
} }
if (JunctionHeight == 1) { if (JunctionHeight == 1) {
robot.ArmMotor.setTargetPosition(armMotorCollect); TargetPosition = armMotorCollect;
if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ { if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
@@ -193,7 +225,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.getCurrentPosition() - 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) {