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

Smoothing the sweep of the High Risers and adding a low raise

Browse Source
This commit is contained in:
Sodi
2022-10-08 12:30:26 -05:00
parent 6a2c14e60b
commit 9efbc78013
2 changed files with 75 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,14 +9,14 @@ import org.cyberarm.engine.V2.CyberarmEngine;
public class PrototypeBot1 { public class PrototypeBot1 {
public Servo RackRiserLeft, RackRiserRight, FrontRiserLeft, FrontRiserRight; public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight;
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 Servo collectorWrist;
public PrototypeBot1(CyberarmEngine engine) { public PrototypeBot1(CyberarmEngine engine) {
this.engine = engine; this.engine = engine;
@@ -27,20 +27,20 @@ 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"); LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft");
RackRiserRight = engine.hardwareMap.servo.get("RackRiserRight"); LowRiserRight = engine.hardwareMap.servo.get("LowRiserRight");
FrontRiserLeft = engine.hardwareMap.servo.get("FrontRiserLeft"); HighRiserLeft = engine.hardwareMap.servo.get("HighRiserLeft");
FrontRiserRight = engine.hardwareMap.servo.get("FrontRiserRight"); HighRiserRight = engine.hardwareMap.servo.get("HighRiserRight");
//motors direction and encoders //motors direction and encoders
frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD); frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
@@ -55,8 +55,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);
} }
} }

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

@@ -1,6 +1,7 @@
package org.timecrafters.testing.states; package org.timecrafters.testing.states;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.CyberarmState;
@@ -18,8 +19,9 @@ 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 boolean raiseHighRiser = true;
private long lastStepTime = 0; private long lastStepTime = 0;
private boolean raiseLowRiser = true;
public PrototypeTeleOPState(PrototypeBot1 robot) { public PrototypeTeleOPState(PrototypeBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -31,10 +33,10 @@ public class PrototypeTeleOPState extends CyberarmState {
// 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("High Riser Right Position", robot.HighRiserRight.getPosition());
engine.telemetry.addData("Front Riser Left Position", robot.FrontRiserLeft.getPosition()); engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition());
// engine.telemetry.addData("Rack Riser Right Position", robot.RackRiserRight.getPosition()); engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
// engine.telemetry.addData("Rack Riser Left Position", robot.RackRiserLeft.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
} }
@@ -45,10 +47,14 @@ public class PrototypeTeleOPState extends CyberarmState {
// 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.HighRiserLeft.setDirection(Servo.Direction.REVERSE);
// robot.RackRiserRight.setPosition(1); robot.HighRiserRight.setDirection(Servo.Direction.FORWARD);
robot.FrontRiserLeft.setPosition(1); robot.LowRiserLeft.setDirection(Servo.Direction.FORWARD);
robot.FrontRiserRight.setPosition(0); robot.LowRiserRight.setDirection(Servo.Direction.REVERSE);
robot.LowRiserLeft.setPosition(0.45);
robot.LowRiserRight.setPosition(0.45);
robot.HighRiserLeft.setPosition(0.45);
robot.HighRiserRight.setPosition(0.45);
} }
@@ -77,9 +83,9 @@ public class PrototypeTeleOPState extends CyberarmState {
} }
//Drivetrain Variables //Drivetrain Variables
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative double y = -engine.gamepad1.left_stick_y * 0.5; // Remember, this is reversed! because of the negative
double x = engine.gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing double x = engine.gamepad1.right_stick_x * 1.1 * 0.5; // Counteract imperfect strafing
double rx = engine.gamepad1.left_stick_x; double rx = engine.gamepad1.left_stick_x * 0.5;
// Denominator is the largest motor power (absolute value) or 1 // Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when // This ensures all the powers maintain the same ratio, but only when
@@ -177,67 +183,69 @@ public class PrototypeTeleOPState extends CyberarmState {
// robot.armMotor.setTargetPosition(armTargetPosition); // robot.armMotor.setTargetPosition(armTargetPosition);
// //
// if (engine.gamepad1.a) { // if (engine.gamepad1.a) {
// robot.RackRiserLeft.setPosition(0); // robot.LowRiserLeft.setPosition(0);
// robot.RackRiserRight.setPosition(1); // robot.LowRiserRight.setPosition(1);
// } // }
// if (engine.gamepad1.x) { // if (engine.gamepad1.x) {
// robot.RackRiserLeft.setPosition(1); // robot.LowRiserLeft.setPosition(1);
// robot.RackRiserRight.setPosition(0); // robot.LowRiserRight.setPosition(0);
// } // }
// //
// if (engine.gamepad2.right_stick_y < -0.1) { if (engine.gamepad2.right_stick_y < -0.1) {
// robot.RackRiserRight.setPosition(1); robot.LowRiserRight.setPosition(0.6);
// robot.RackRiserLeft.setPosition(-1); robot.LowRiserLeft.setPosition(0.6);
// } }
//
// if (engine.gamepad2.right_stick_y > 0.1) { if (engine.gamepad2.right_stick_y > 0.1) {
// robot.RackRiserRight.setPosition(-1); robot.LowRiserRight.setPosition(0.45);
// robot.RackRiserLeft.setPosition(1); robot.LowRiserLeft.setPosition(0.45);
// } }
if (engine.gamepad2.start) { if (engine.gamepad2.start) {
if (System.currentTimeMillis() - lastStepTime >= 1_000) { if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
if (raiseFrontRiser) { if (raiseHighRiser) {
robot.FrontRiserLeft.setPosition(robot.FrontRiserLeft.getPosition() + 0.05); if (robot.HighRiserLeft.getPosition() >= 1) {
robot.FrontRiserRight.setPosition(robot.FrontRiserRight.getPosition() - 0.05); if (raiseLowRiser) {
raiseHighRiser = false;
if (robot.FrontRiserLeft.getPosition() >= 1.0) { }
raiseFrontRiser = false; } else {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
} }
} else { } else {
robot.FrontRiserLeft.setPosition(robot.FrontRiserLeft.getPosition() - 0.05); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.035);
robot.FrontRiserRight.setPosition(robot.FrontRiserRight.getPosition() + 0.05); robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.035);
if (robot.FrontRiserLeft.getPosition() <= 0.5) { if (robot.HighRiserLeft.getPosition() <= 0.45) {
raiseFrontRiser = true; raiseHighRiser = true;
} }
} }
} }
} }
if (engine.gamepad2.left_stick_y > 0.1) { // if (engine.gamepad2.left_stick_y > 0.1) {
robot.FrontRiserLeft.setPosition(0.5); // robot.HighRiserLeft.setPosition(0.5);
robot.FrontRiserRight.setPosition(-0.5); // robot.HighRiserRight.setPosition(-0.5);
} // }
//
if (engine.gamepad2.left_stick_y < -0.1) { // if (engine.gamepad2.left_stick_y < -0.1) {
robot.FrontRiserLeft.setPosition(-1); // robot.HighRiserLeft.setPosition(-1);
robot.FrontRiserRight.setPosition(1); // robot.HighRiserRight.setPosition(1);
} // }
// if (engine.gamepad2.right_bumper) { // if (engine.gamepad2.right_bumper) {
// robot.RackRiserRight.setPosition(1); // robot.LowRiserRight.setPosition(1);
// robot.RackRiserLeft.setPosition(0); // robot.LowRiserLeft.setPosition(0);
// } // }
if (engine.gamepad2.left_bumper) { if (engine.gamepad2.left_bumper) {
robot.FrontRiserRight.setPosition(0); robot.HighRiserRight.setPosition(0);
robot.FrontRiserLeft.setPosition(1); robot.HighRiserLeft.setPosition(1);
} }
// For raising, front risers ALWAYS raise first, for lowering, rack risers ALWAYS lower first. // For raising, high risers ALWAYS raise first, for lowering, low risers ALWAYS lower first.