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

Compare commits

base: TimeCrafters:236c29127524a01f246d90ed00a34fccee5396f8
Branches Tags
TimeCrafters:master
...
compare: TimeCrafters:e666590d0dd62b238d5d186ab42c91eae2ab276d
Branches Tags
TimeCrafters:master

2 Commits
236c291275 ... e666590d0d

Author SHA1 Message Date
Cyberarm
e666590d0d Fixed Move not working correctly after first Move state 2023-01-31 21:38:21 -06:00
Sodi
3902bc3291 Switching the upper-arm servos to a motor 2023-01-31 20:44:59 -06:00
4 changed files with 54 additions and 28 deletions
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;
}

26
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
View File

@@ -15,9 +15,12 @@ public class Move extends CyberarmState {
private final double maxVelocity;
private double speed;
private int distanceAlreadyTravelled;
private int distanceAlreadyTravelled, travelledDistance;
private final boolean stateDisabled;
private double velocity;
private double ratio = 1.0;
public Move(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
@@ -41,7 +44,7 @@ public class Move extends CyberarmState {
@Override
public void start() {
// TODO: Use a dead wheel for this
distanceAlreadyTravelled = robot.frontRightDrive.getCurrentPosition();
distanceAlreadyTravelled = -robot.frontRightDrive.getCurrentPosition();
}
@Override
@@ -60,7 +63,7 @@ public class Move extends CyberarmState {
return;
}
int travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
travelledDistance = -robot.frontRightDrive.getCurrentPosition() - distanceAlreadyTravelled;
if (robot.isBetween(travelledDistance, targetDistance - tolerance, targetDistance + tolerance)) {
stop();
@@ -73,12 +76,21 @@ public class Move extends CyberarmState {
moveDirectional(travelledDistance);
}
private void moveDirectional(double travelledDistance) {
double velocity;
double ratio = 1.0;
@Override
public void telemetry() {
engine.telemetry.addLine("Move");
engine.telemetry.addData("Minimum Velocity", minimumVelocity);
engine.telemetry.addData("Target Velocity", targetVelocity);
engine.telemetry.addData("Velocity", velocity);
engine.telemetry.addData("Target Distance", targetDistance);
engine.telemetry.addData("Travelled Distance", travelledDistance);
engine.telemetry.addData("Distance Already Travelled", distanceAlreadyTravelled);
engine.telemetry.addData("Travel Forward", targetDistance > travelledDistance);
}
private void moveDirectional(double travelledDistance) {
if (Math.abs(travelledDistance) < easeInDistance) {
ratio = travelledDistance / easeInDistance;
ratio = Math.abs(travelledDistance) / easeInDistance;
} else if (Math.abs(travelledDistance) > Math.abs(targetDistance) - easeOutDistance) {
ratio = (Math.abs(targetDistance) - Math.abs(travelledDistance)) / easeOutDistance;
}