TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 06:22:33 +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.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.TeleOp.states.PhoenixTeleOPState;
import org.timecrafters.TeleOp.states.TeleOPArmDriver;
import org.timecrafters.TeleOp.states.TeleOPTankDriver;
@TeleOp (name = "APhoenixTeleOP")
@@ -16,6 +18,8 @@ public class PhoenixTeleOP extends CyberarmEngine {
public void setup() {
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);
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE);
ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
ArmMotor.setTargetPosition(0);
ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
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 servoHighLow = 0.8; //Low servos, Y button
// private double servoHighHigh = 0.9; //High servos, Y button
private double ArmNeededPower;
private int armMotorCollect = 0;
private int armMotorLow = 100;
private int armMotorMed = 1000;
private int armMotorHigh = 1600;
private int armMotorLow = 240;
private int armMotorMed = 380;
private int armMotorHigh = 463;
private int ArmMotorStepSize = 2;
public TeleOPArmDriver(PhoenixBot1 robot) {
this.robot = robot;
@@ -36,24 +38,35 @@ public class TeleOPArmDriver extends CyberarmState {
public void telemetry() {
engine.telemetry.addLine("Arm Driver:");
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 Left Position", robot.LowRiserLeft.getPosition());
engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
engine.telemetry.addData("Target (Endeavour)", Endeavour);
}
@Override
public void init() {
robot.ArmMotor.setDirection(DcMotorSimple.Direction.REVERSE);
robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
robot.LowRiserLeft.setPosition(0.45);
robot.LowRiserRight.setPosition(0.45);
robot.ArmMotor.setPower(0);
robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setPower(0.25);
Opportunity = 0;
Endeavour = 0;
}
@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) {
Endeavour = 4;
@@ -68,20 +81,15 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
Endeavour = 1;
}
if (Endeavour == 4) {
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
}
if (Endeavour == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(armMotorHigh);
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ {
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
if (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
robot.LowRiserLeft.getPosition() >= servoHighLow) {
@@ -90,6 +98,7 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
}
if (Endeavour == 3) {
robot.ArmMotor.setTargetPosition(armMotorMed);
if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
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);
}
}
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) {
lastStepTime = System.currentTimeMillis();
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 &&
robot.ArmMotor.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ {
robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
}
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) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() > armMotorMed - 5) {
robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
Endeavour = 0;
}
}
if (Endeavour == 2) {
robot.ArmMotor.setTargetPosition(armMotorLow);
if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
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);
}
}
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) {
lastStepTime = System.currentTimeMillis();
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*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67));
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
}
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) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67));
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorLow - 5) {
robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
Endeavour = 0;
}
}
if (Endeavour == 1) {
robot.ArmMotor.setTargetPosition(armMotorCollect);
if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.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*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(45));
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
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) {
armPower = engine.gamepad2.right_trigger;
robot.ArmMotor.setPower(armPower);
} 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) {
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 MinimalPower = 0.2;
private int DeltaOdometerR, Endeavour, Spirit;
private boolean FreeSpirit;
private GamepadChecker gamepad1Checker;
public TeleOPTankDriver(PhoenixBot1 robot) {
@@ -33,37 +32,24 @@ public class TeleOPTankDriver extends CyberarmState {
@Override
public void init() {
FreeSpirit = false;
}
@Override
public void exec() {
if (drivePower > 0.2) {
if (System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR < 4096) {
lastStepTime = System.currentTimeMillis();
FreeSpirit = true;
}
}
if (FreeSpirit && System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR < 4096) {
getCurrentDriveCommand();
drivePower = -currentDriveCommand;
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
drivePower = engine.gamepad1.left_stick_y;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
if (FreeSpirit && System.currentTimeMillis() - lastStepTime >= 2000 && DeltaOdometerR >= 4096) {
FreeSpirit = false;
}
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && !FreeSpirit) {
drivePower = engine.gamepad1.left_stick_y;
if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) {
drivePower = engine.gamepad1.left_stick_x;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
@@ -76,6 +62,7 @@ public class TeleOPTankDriver extends CyberarmState {
}
}
public void CalculateDeltaRotation() {
@@ -107,7 +94,7 @@ public class TeleOPTankDriver extends CyberarmState {
currentDriveCommand = engine.gamepad1.right_stick_x;
} else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
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;
}