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

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;
private final CyberarmEngine engine;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, armMotor;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public CRServo collectorLeft, collectorRight;
public Servo collectorWrist;
public PrototypeBot1(CyberarmEngine engine) {
this.engine = engine;
@@ -27,16 +25,16 @@ public class PrototypeBot1 {
private void setupRobot () {
//motors configuration
frontLeftDrive = engine.hardwareMap.dcMotor.get("front left");
frontRightDrive = engine.hardwareMap.dcMotor.get("front right");
backRightDrive = engine.hardwareMap.dcMotor.get("back left");
backLeftDrive = engine.hardwareMap.dcMotor.get("back right");
armMotor = engine.hardwareMap.dcMotor.get("arm motor");
frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
frontRightDrive = engine.hardwareMap.dcMotor.get("Front Right");
backRightDrive = engine.hardwareMap.dcMotor.get("Back Left");
backLeftDrive = engine.hardwareMap.dcMotor.get("Back Right");
// armMotor = engine.hardwareMap.dcMotor.get("Arm Motor");
// servo configuration
collectorLeft = engine.hardwareMap.crservo.get("collector left");
collectorRight = engine.hardwareMap.crservo.get("collector right");
collectorWrist = engine.hardwareMap.servo.get("collector wrist");
collectorLeft = engine.hardwareMap.crservo.get("Collector Left");
collectorRight = engine.hardwareMap.crservo.get("Collector Right");
// collectorWrist = engine.hardwareMap.servo.get("Collector Wrist");
RackRiserLeft = engine.hardwareMap.servo.get("RackRiserLeft");
RackRiserRight = engine.hardwareMap.servo.get("RackRiserRight");
FrontRiserLeft = engine.hardwareMap.servo.get("FrontRiserLeft");
@@ -55,8 +53,8 @@ public class PrototypeBot1 {
backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armMotor.setDirection(DcMotorSimple.Direction.FORWARD);
armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// armMotor.setDirection(DcMotorSimple.Direction.FORWARD);
// 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 int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400;
private double collectorRiserPosition;
private boolean raiseFrontRiser = true;
private long lastStepTime = 0;
public PrototypeTeleOPState(PrototypeBot1 robot) {
this.robot = robot;
@@ -25,24 +27,28 @@ public class PrototypeTeleOPState extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("Arm Power", robot.armMotor.getPower());
engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition());
engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition());
engine.telemetry.addData("Wrist Current Position", robot.collectorWrist.getPosition());
// engine.telemetry.addData("Arm Power", robot.armMotor.getPower());
// engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition());
// engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition());
// 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
public void init() {
robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.armMotor.setTargetPosition(armTargetPosition);
robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.collectorWrist.setPosition(1);
robot.RackRiserLeft.setPosition(0);
robot.RackRiserRight.setPosition(1);
// robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// robot.armMotor.setTargetPosition(armTargetPosition);
// robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.collectorWrist.setPosition(1);
// robot.RackRiserLeft.setPosition(0);
// robot.RackRiserRight.setPosition(1);
robot.FrontRiserLeft.setPosition(1);
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.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) {
@@ -118,26 +124,26 @@ public class PrototypeTeleOPState extends CyberarmState {
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();
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);
}
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 (robot.FrontRiserLeft.getPosition() <= 0.5) {
raiseFrontRiser = true;
}
}
}
}
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.FrontRiserRight.setPosition(1);
}
if (engine.gamepad2.left_stick_y < -0.1) {
robot.FrontRiserLeft.setPosition(1);
robot.FrontRiserRight.setPosition(-1);
}
if (engine.gamepad2.right_bumper) {
robot.RackRiserRight.setPosition(1);
robot.RackRiserLeft.setPosition(0);
}
// if (engine.gamepad2.right_bumper) {
// robot.RackRiserRight.setPosition(1);
// robot.RackRiserLeft.setPosition(0);
// }
if (engine.gamepad2.left_bumper) {
robot.FrontRiserRight.setPosition(0);
robot.FrontRiserLeft.setPosition(1);
}
// For raising, front risers ALWAYS raise first, for lowering, rack risers ALWAYS lower first.