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

Browse Source
This commit is contained in:
Sodi
2023-01-31 20:44:59 -06:00
parent 236c291275
commit 3902bc3291
3 changed files with 35 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,6 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm;
public class PhoenixBot1 {
@@ -46,7 +47,7 @@ public class PhoenixBot1 {
public Rev2mDistanceSensor collectorDistance, /*downSensor, */leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor, armMotorEncoder;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoderRight, OdometerEncoderLeft, OdometerEncoderHorizontal, ArmMotor;
public CRServo collectorLeft, collectorRight;
@@ -148,7 +149,6 @@ public class PhoenixBot1 {
// HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft");
// HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight");
ArmMotor = engine.hardwareMap.dcMotor.get("ArmMotor");
armMotorEncoder = engine.hardwareMap.dcMotor.get("Arm Motor Encoder");
//motors direction and encoders
@@ -162,6 +162,7 @@ public class PhoenixBot1 {
frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -199,11 +200,11 @@ public class PhoenixBot1 {
// HighRiserRight.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE);
armMotorEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
armMotorEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
ArmMotor.setTargetPosition(0);
ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
CameraServo.setDirection(Servo.Direction.FORWARD);
@@ -216,6 +217,13 @@ public class PhoenixBot1 {
}
public int AngleToTicks(double angle) {
double d = (60 * 28) / 360.0;
// Casting to float so that the int version of Math.round is used.
return Math.round((float)d * (float)angle);
}
}

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

@@ -42,19 +42,14 @@ public class TeleOPArmDriver extends CyberarmState {
@Override
public void init() {
robot.ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
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.armMotorEncoder.setPower(0);
Opportunity = 0;
Endeavour = 0;
gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
}
@Override
@@ -74,10 +69,11 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
}
if (Endeavour == 4) {
if (robot.armMotorEncoder.getCurrentPosition() < armMotorHigh - 5)/* <-- high level too low*/ {
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 (robot.LowRiserLeft.getPosition() < servoHighLow - 5)/* <-- low level too low*/ {
@@ -87,7 +83,7 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.armMotorEncoder.getCurrentPosition() >= armMotorHigh &&
if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
robot.LowRiserLeft.getPosition() >= servoHighLow) {
Endeavour = 0;
}
@@ -109,22 +105,24 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
}
}
if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ {
robot.ArmMotor.getCurrentPosition() > armMotorMed + 5)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(-0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(90));
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() < armMotorMed - 5)/* <-- high level too low*/ {
robot.ArmMotor.getCurrentPosition() < armMotorMed - 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 (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.armMotorEncoder.getCurrentPosition() > armMotorMed - 5) {
robot.ArmMotor.getCurrentPosition() > armMotorMed - 5) {
Endeavour = 0;
}
}
@@ -146,22 +144,26 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.armMotorEncoder.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
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));
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ {
robot.ArmMotor.getCurrentPosition() < armMotorLow - 5)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setPower(0.5);
robot.ArmMotor.setTargetPosition(robot.AngleToTicks(67));
}
}
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() > armMotorLow - 5) {
robot.ArmMotor.getCurrentPosition() > armMotorLow - 5) {
Endeavour = 0;
}
}
@@ -174,13 +176,15 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.armMotorEncoder.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
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));
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 &&
robot.armMotorEncoder.getCurrentPosition() <= armMotorCollect) {
robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
Endeavour = 0;
}
}
@@ -214,6 +218,9 @@ public void exec() { //CHANGE ALL THE ENDEAVOUR TARGETS FROM SERVO POSITIONS TO
} 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);
}
}

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

@@ -33,7 +33,6 @@ public class TeleOPTankDriver extends CyberarmState {
@Override
public void init() {
gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
FreeSpirit = false;
}