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

Smoothing and triangulating the locking limit, adding a choppy walk.

Browse Source
This commit is contained in:
Sodi
2022-10-06 20:31:11 -05:00
parent cbc1e85722
commit a2adf72747
2 changed files with 105 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
View File

@@ -12,12 +12,10 @@ public class PrototypeBot1 {
public Servo RackRiserLeft, RackRiserRight, FrontRiserLeft, FrontRiserRight; public Servo RackRiserLeft, RackRiserRight, FrontRiserLeft, FrontRiserRight;
private final CyberarmEngine engine; private final CyberarmEngine engine;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, armMotor; public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public CRServo collectorLeft, collectorRight; public CRServo collectorLeft, collectorRight;
public Servo collectorWrist;
public PrototypeBot1(CyberarmEngine engine) { public PrototypeBot1(CyberarmEngine engine) {
this.engine = engine; this.engine = engine;
@@ -27,16 +25,16 @@ public class PrototypeBot1 {
private void setupRobot () { private void setupRobot () {
//motors configuration //motors configuration
frontLeftDrive = engine.hardwareMap.dcMotor.get("front left"); frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
frontRightDrive = engine.hardwareMap.dcMotor.get("front right"); frontRightDrive = engine.hardwareMap.dcMotor.get("Front Right");
backRightDrive = engine.hardwareMap.dcMotor.get("back left"); backRightDrive = engine.hardwareMap.dcMotor.get("Back Left");
backLeftDrive = engine.hardwareMap.dcMotor.get("back right"); backLeftDrive = engine.hardwareMap.dcMotor.get("Back Right");
armMotor = engine.hardwareMap.dcMotor.get("arm motor"); // armMotor = engine.hardwareMap.dcMotor.get("Arm Motor");
// servo configuration // servo configuration
collectorLeft = engine.hardwareMap.crservo.get("collector left"); collectorLeft = engine.hardwareMap.crservo.get("Collector Left");
collectorRight = engine.hardwareMap.crservo.get("collector right"); collectorRight = engine.hardwareMap.crservo.get("Collector Right");
collectorWrist = engine.hardwareMap.servo.get("collector wrist"); // collectorWrist = engine.hardwareMap.servo.get("Collector Wrist");
RackRiserLeft = engine.hardwareMap.servo.get("RackRiserLeft"); RackRiserLeft = engine.hardwareMap.servo.get("RackRiserLeft");
RackRiserRight = engine.hardwareMap.servo.get("RackRiserRight"); RackRiserRight = engine.hardwareMap.servo.get("RackRiserRight");
FrontRiserLeft = engine.hardwareMap.servo.get("FrontRiserLeft"); FrontRiserLeft = engine.hardwareMap.servo.get("FrontRiserLeft");
@@ -55,8 +53,8 @@ public class PrototypeBot1 {
backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE); backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armMotor.setDirection(DcMotorSimple.Direction.FORWARD); // armMotor.setDirection(DcMotorSimple.Direction.FORWARD);
armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
} }
} }

159
TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
View File

@@ -18,6 +18,8 @@ public class PrototypeTeleOPState extends CyberarmState {
private boolean UpDPad; private boolean UpDPad;
private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400; private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400;
private double collectorRiserPosition; private double collectorRiserPosition;
private boolean raiseFrontRiser = true;
private long lastStepTime = 0;
public PrototypeTeleOPState(PrototypeBot1 robot) { public PrototypeTeleOPState(PrototypeBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -25,24 +27,28 @@ public class PrototypeTeleOPState extends CyberarmState {
@Override @Override
public void telemetry() { public void telemetry() {
engine.telemetry.addData("Arm Power", robot.armMotor.getPower()); // engine.telemetry.addData("Arm Power", robot.armMotor.getPower());
engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition()); // engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition());
engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition()); // engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition());
engine.telemetry.addData("Wrist Current Position", robot.collectorWrist.getPosition()); // engine.telemetry.addData("Wrist Current Position", robot.collectorWrist.getPosition());
engine.telemetry.addData("Front Riser Right Position", robot.FrontRiserRight.getPosition());
engine.telemetry.addData("Front Riser Left Position", robot.FrontRiserLeft.getPosition());
// engine.telemetry.addData("Rack Riser Right Position", robot.RackRiserRight.getPosition());
// engine.telemetry.addData("Rack Riser Left Position", robot.RackRiserLeft.getPosition());
} }
@Override @Override
public void init() { public void init() {
robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.armMotor.setTargetPosition(armTargetPosition); // robot.armMotor.setTargetPosition(armTargetPosition);
robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); // robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.collectorWrist.setPosition(1); // robot.collectorWrist.setPosition(1);
robot.RackRiserLeft.setPosition(0); // robot.RackRiserLeft.setPosition(0);
robot.RackRiserRight.setPosition(1); // robot.RackRiserRight.setPosition(1);
robot.FrontRiserLeft.setPosition(1); robot.FrontRiserLeft.setPosition(1);
robot.FrontRiserRight.setPosition(0); robot.FrontRiserRight.setPosition(0);
// ~RightRiser's always are setPosition'ed(1), ~LeftRisers always are setPosition'ed(0) or vice versa if wrong.
} }
@@ -95,7 +101,7 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-frontRightPower); robot.frontRightDrive.setPower(-frontRightPower);
robot.backRightDrive.setPower(backRightPower); robot.backRightDrive.setPower(backRightPower);
robot.armMotor.setPower(engine.gamepad2.left_stick_y * 0.5); // robot.armMotor.setPower(engine.gamepad2.left_stick_y * 0.5);
if (engine.gamepad2.y) { if (engine.gamepad2.y) {
@@ -118,26 +124,26 @@ public class PrototypeTeleOPState extends CyberarmState {
robot.collectorRight.setPower(1); robot.collectorRight.setPower(1);
} }
//
// if (engine.gamepad2.dpad_left) {
//
// robot.collectorWrist.setPosition(0.4);
if (engine.gamepad2.dpad_left) { // }
//
// if (engine.gamepad2.dpad_right) {
//
// robot.collectorWrist.setPosition(1);
//
// }
robot.collectorWrist.setPosition(0.4); // if (engine.gamepad2.dpad_up) {
} // robot.armMotor.setPower(1);
// } else if (engine.gamepad2.dpad_down) {
// robot.armMotor.setPower(0);
if (engine.gamepad2.dpad_right) { // }
robot.collectorWrist.setPosition(1);
}
if (engine.gamepad2.dpad_up) {
robot.armMotor.setPower(1);
} else if (engine.gamepad2.dpad_down) {
robot.armMotor.setPower(0);
}
// int armTargetPosition = robot.armMotor.getCurrentPosition(); // int armTargetPosition = robot.armMotor.getCurrentPosition();
if (engine.gamepad2.left_trigger > 0.1) { if (engine.gamepad2.left_trigger > 0.1) {
@@ -156,59 +162,82 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.left_bumper) { // if (engine.gamepad1.left_bumper) {
//
// armTargetPosition = armDeliverPosition;
//
// }
//
// if (engine.gamepad2.right_bumper) {
//
// armTargetPosition = armCollectPosition;
//
// }
//
// robot.armMotor.setTargetPosition(armTargetPosition);
//
// if (engine.gamepad1.a) {
// robot.RackRiserLeft.setPosition(0);
// robot.RackRiserRight.setPosition(1);
// }
armTargetPosition = armDeliverPosition; // if (engine.gamepad1.x) {
// robot.RackRiserLeft.setPosition(1);
// robot.RackRiserRight.setPosition(0);
// }
//
// if (engine.gamepad2.right_stick_y < -0.1) {
// robot.RackRiserRight.setPosition(1);
// robot.RackRiserLeft.setPosition(-1);
// }
//
// if (engine.gamepad2.right_stick_y > 0.1) {
// robot.RackRiserRight.setPosition(-1);
// robot.RackRiserLeft.setPosition(1);
// }
} if (engine.gamepad2.start) {
if (System.currentTimeMillis() - lastStepTime >= 1_000) {
lastStepTime = System.currentTimeMillis();
if (engine.gamepad2.right_bumper) { if (raiseFrontRiser) {
robot.FrontRiserLeft.setPosition(robot.FrontRiserLeft.getPosition() + 0.05);
robot.FrontRiserRight.setPosition(robot.FrontRiserRight.getPosition() - 0.05);
armTargetPosition = armCollectPosition; if (robot.FrontRiserLeft.getPosition() >= 1.0) {
raiseFrontRiser = false;
}
} else {
robot.FrontRiserLeft.setPosition(robot.FrontRiserLeft.getPosition() - 0.05);
robot.FrontRiserRight.setPosition(robot.FrontRiserRight.getPosition() + 0.05);
} if (robot.FrontRiserLeft.getPosition() <= 0.5) {
raiseFrontRiser = true;
robot.armMotor.setTargetPosition(armTargetPosition); }
}
if (engine.gamepad1.a) { }
robot.RackRiserLeft.setPosition(0);
robot.RackRiserRight.setPosition(1);
}
if (engine.gamepad1.x) {
robot.RackRiserLeft.setPosition(1);
robot.RackRiserRight.setPosition(0);
}
if (engine.gamepad2.right_stick_y > 0.1) {
robot.RackRiserRight.setPosition(1);
robot.RackRiserLeft.setPosition(-1);
}
if (engine.gamepad2.right_stick_y < -0.1) {
robot.RackRiserRight.setPosition(-1);
robot.RackRiserLeft.setPosition(1);
} }
if (engine.gamepad2.left_stick_y > 0.1) { if (engine.gamepad2.left_stick_y > 0.1) {
robot.FrontRiserLeft.setPosition(0.5);
robot.FrontRiserRight.setPosition(-0.5);
}
if (engine.gamepad2.left_stick_y < -0.1) {
robot.FrontRiserLeft.setPosition(-1); robot.FrontRiserLeft.setPosition(-1);
robot.FrontRiserRight.setPosition(1); robot.FrontRiserRight.setPosition(1);
} }
if (engine.gamepad2.left_stick_y < -0.1) { // if (engine.gamepad2.right_bumper) {
robot.FrontRiserLeft.setPosition(1); // robot.RackRiserRight.setPosition(1);
robot.FrontRiserRight.setPosition(-1); // robot.RackRiserLeft.setPosition(0);
} // }
if (engine.gamepad2.right_bumper) {
robot.RackRiserRight.setPosition(1);
robot.RackRiserLeft.setPosition(0);
}
if (engine.gamepad2.left_bumper) { if (engine.gamepad2.left_bumper) {
robot.FrontRiserRight.setPosition(0); robot.FrontRiserRight.setPosition(0);
robot.FrontRiserLeft.setPosition(1); robot.FrontRiserLeft.setPosition(1);
} }
// For raising, front risers ALWAYS raise first, for lowering, rack risers ALWAYS lower first.