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

Switching the upper-arm servos to a motor, trying to debug low arm servos

Browse Source
This commit is contained in:
Sodi
2023-02-01 16:48:45 -06:00
parent e666590d0d
commit a826059286
4 changed files with 62 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,6 +5,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine; import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.PhoenixBot1; import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.TeleOp.states.PhoenixTeleOPState; import org.timecrafters.TeleOp.states.PhoenixTeleOPState;
import org.timecrafters.TeleOp.states.TeleOPArmDriver;
import org.timecrafters.TeleOp.states.TeleOPTankDriver;
@TeleOp (name = "APhoenixTeleOP") @TeleOp (name = "APhoenixTeleOP")
@@ -16,6 +18,8 @@ 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 PhoenixTeleOPState(robot));
addState(new TeleOPArmDriver(robot));
addParallelStateToLastState(new TeleOPTankDriver(robot));
} }
} }

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

@@ -200,11 +200,11 @@ 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.FORWARD); ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ArmMotor.setTargetPosition(0);
ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
CameraServo.setDirection(Servo.Direction.FORWARD); CameraServo.setDirection(Servo.Direction.FORWARD);

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

@@ -23,10 +23,12 @@ public class TeleOPArmDriver extends CyberarmState {
// private double servoMedHigh = 0.9; //High servos, B button // private double servoMedHigh = 0.9; //High servos, B button
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 int armMotorCollect = 0; private int armMotorCollect = 0;
private int armMotorLow = 100; private int armMotorLow = 240;
private int armMotorMed = 1000; private int armMotorMed = 380;
private int armMotorHigh = 1600; private int armMotorHigh = 463;
private int ArmMotorStepSize = 2;
public TeleOPArmDriver(PhoenixBot1 robot) { public TeleOPArmDriver(PhoenixBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -36,24 +38,35 @@ public class TeleOPArmDriver extends CyberarmState {
public void telemetry() { public void telemetry() {
engine.telemetry.addLine("Arm Driver:"); engine.telemetry.addLine("Arm Driver:");
engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower()); engine.telemetry.addData("Arm Motor Power", robot.ArmMotor.getPower());
engine.telemetry.addData("Arm Motor Encoder Position", robot.ArmMotor.getCurrentPosition());
engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition()); engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
engine.telemetry.addData("Target (Endeavour)", Endeavour);
} }
@Override @Override
public void init() { public void init() {
robot.ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD); robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
robot.LowRiserRight.setDirection(Servo.Direction.REVERSE); robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
robot.LowRiserLeft.setPosition(0.45); robot.LowRiserLeft.setPosition(0.45);
robot.LowRiserRight.setPosition(0.45); robot.LowRiserRight.setPosition(0.45);
robot.ArmMotor.setPower(0); robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setPower(0.25);
Opportunity = 0; Opportunity = 0;
Endeavour = 0; Endeavour = 0;
} }
@Override @Override
public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO MM HEIGHTS!! public void exec() {
if (Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) > 46) {
ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
armPower = ArmNeededPower;
robot.ArmMotor.setPower(armPower);
}
if (engine.gamepad2.y) { if (engine.gamepad2.y) {
Endeavour = 4; Endeavour = 4;
@@ -68,20 +81,15 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
Endeavour = 1; Endeavour = 1;
} }
if (Endeavour == 4) { if (Endeavour == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ { lastStepTime = System.currentTimeMillis();
if (System.currentTimeMillis() - lastStepTime >= 100) { robot.ArmMotor.setTargetPosition(armMotorHigh);
lastStepTime = System.currentTimeMillis(); if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ {
robot.ArmMotor.setPower(0.5); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
}
} }
if (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ { if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
lastStepTime = System.currentTimeMillis(); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
} }
if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh && if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
robot.LowRiserLeft.getPosition() >= servoHighLow) { robot.LowRiserLeft.getPosition() >= servoHighLow) {
@@ -90,6 +98,7 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
} }
if (Endeavour == 3) { if (Endeavour == 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();
@@ -97,7 +106,7 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() < servoMedLow - 5)/* <-- low level too low*/ { 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);
@@ -105,29 +114,28 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
} }
} }
if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 && if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorMed + 5)/* <-- 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.setPower(-0.5); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
} }
} }
if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 && if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 &&
robot.ArmMotor.getCurrentPosition() < armMotorMed - 5)/* <-- 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.setPower(0.5); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
} }
} }
if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 && if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow && robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() > armMotorMed - 5) { robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
Endeavour = 0; Endeavour = 0;
} }
} }
if (Endeavour == 2) { if (Endeavour == 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();
@@ -135,7 +143,7 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} }
} }
if (robot.LowRiserLeft.getPosition() < servoLowLow - 5)/* <-- low level too low*/ { 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);
@@ -147,41 +155,36 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
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.setPower(-0.5); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67));
} }
} }
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.ArmMotor.getCurrentPosition() < armMotorLow - 5)/* <-- 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.setPower(0.5); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67));
} }
} }
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorLow - 5) { robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
Endeavour = 0; Endeavour = 0;
} }
} }
if (Endeavour == 1) { if (Endeavour == 1) {
robot.ArmMotor.setTargetPosition(armMotorCollect);
if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ { if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ {
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);
} }
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
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.setPower(-0.5); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(45));
} }
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 && } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
@@ -213,14 +216,11 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
} }
if (engine.gamepad2.right_trigger > 0.1) { if (engine.gamepad2.right_trigger > 0.1) {
armPower = engine.gamepad2.right_trigger;
robot.ArmMotor.setPower(armPower);
} else if (engine.gamepad2.left_trigger > 0.1) { } else if (engine.gamepad2.left_trigger > 0.1) {
armPower = -(engine.gamepad2.left_trigger);
robot.ArmMotor.setPower(armPower);
} else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 0) { } else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 0) {
armPower = 0;
robot.ArmMotor.setPower(armPower);
} }
} }

29
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java
View File

@@ -16,7 +16,6 @@ public class TeleOPTankDriver extends CyberarmState {
private double RotationTarget, DeltaRotation; private double RotationTarget, DeltaRotation;
private double MinimalPower = 0.2; private double MinimalPower = 0.2;
private int DeltaOdometerR, Endeavour, Spirit; private int DeltaOdometerR, Endeavour, Spirit;
private boolean FreeSpirit;
private GamepadChecker gamepad1Checker; private GamepadChecker gamepad1Checker;
public TeleOPTankDriver(PhoenixBot1 robot) { public TeleOPTankDriver(PhoenixBot1 robot) {
@@ -33,37 +32,24 @@ public class TeleOPTankDriver extends CyberarmState {
@Override @Override
public void init() { public void init() {
FreeSpirit = false;
} }
@Override @Override
public void exec() { public void exec() {
if (drivePower > 0.2) { if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
if (System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR < 4096) { drivePower = engine.gamepad1.left_stick_y;
lastStepTime = System.currentTimeMillis();
FreeSpirit = true;
}
}
if (FreeSpirit && System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR < 4096) {
getCurrentDriveCommand();
drivePower = -currentDriveCommand;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower);
} }
if (FreeSpirit && System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR >= 4096) { if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) {
FreeSpirit = false; drivePower = engine.gamepad1.left_stick_x;
}
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && !FreeSpirit) {
drivePower = engine.gamepad1.left_stick_y;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower);
} }
@@ -76,6 +62,7 @@ public class TeleOPTankDriver extends CyberarmState {
} }
} }
public void CalculateDeltaRotation() { public void CalculateDeltaRotation() {
@@ -107,7 +94,7 @@ public class TeleOPTankDriver extends CyberarmState {
currentDriveCommand = engine.gamepad1.right_stick_x; currentDriveCommand = engine.gamepad1.right_stick_x;
} else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { } else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
currentDriveCommand = engine.gamepad1.right_stick_y; currentDriveCommand = engine.gamepad1.right_stick_y;
} else if ((Math.abs(engine.gamepad1.left_stick_y)) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_y) > 0.1 && !FreeSpirit) { } else if ((Math.abs(engine.gamepad1.left_stick_y)) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
currentDriveCommand = 0; currentDriveCommand = 0;
} }