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

driving straight added

Browse Source
This commit is contained in:
SpencerPiha
2023-01-05 20:32:21 -06:00
parent de1fe0a01f
commit e3089884ea
3 changed files with 29 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java
View File

@@ -38,7 +38,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
addState(new ServoCameraRotate(robot, "RightFourCone", "03-0")); addState(new ServoCameraRotate(robot, "RightFourCone", "03-0"));
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel // 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
addState(new DriverStateWithOdometerUpperArmParallelState(robot, "RightFourCone", "04-0")); addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0"));
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
// 6 Raise lower arm while slowly driving at the junction (parallel state) // 6 Raise lower arm while slowly driving at the junction (parallel state)
addState(new BottomArm(robot, "RightFourCone", "05-0")); addState(new BottomArm(robot, "RightFourCone", "05-0"));
@@ -63,7 +64,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
addState(new BottomArm(robot, "RightFourCone", "09-0")); addState(new BottomArm(robot, "RightFourCone", "09-0"));
// //
// // 10 Back up and bring lower arm down (parallel state) // // 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0")); addState(new DriverStateWithOdometer(robot, "RightFourCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "10-1"));
// 11 Rotate towards stack // 11 Rotate towards stack
addState(new RotationState(robot, "RightFourCone", "11-0")); addState(new RotationState(robot, "RightFourCone", "11-0"));
@@ -83,7 +85,6 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// //
// // 16 Rotate and use sensor to find junction // // 16 Rotate and use sensor to find junction
// addState(new RotationState(robot, "RightFourCone", "16-0")); // addState(new RotationState(robot, "RightFourCone", "16-0"));
// addState(new JunctionAllignmentState(robot, "RightFourCone", "16-1"));
// //
// // 17 Drive Towards Junction (This is optional, idk if this is needed atm) // // 17 Drive Towards Junction (This is optional, idk if this is needed atm)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0")); // addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));

42
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -13,6 +13,7 @@ public class DriverStateWithOdometer extends CyberarmState {
private int RampUpDistance; private int RampUpDistance;
private int RampDownDistance; private int RampDownDistance;
private int maximumTolerance; private int maximumTolerance;
private float direction;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot; this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -20,6 +21,7 @@ public class DriverStateWithOdometer extends CyberarmState {
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value(); this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
} }
@@ -32,18 +34,19 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
} }
@Override @Override
public void exec() { public void exec() {
if (stateDisabled) { if (stateDisabled) {
setHasFinished(true); setHasFinished(true);
return; return;
@@ -54,11 +57,14 @@ public class DriverStateWithOdometer extends CyberarmState {
if (Math.abs(CurrentPosition) <= RampUpDistance){ if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up // ramping up
drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25; float ratio = (Math.abs((float)CurrentPosition) / RampUpDistance);
drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
} }
else if (Math.abs(CurrentPosition) >= delta){ else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
// ramping down // ramping down
drivePower = ((delta / RampDownDistance) + 0.25); double ratio = (1.0 - ((traveledDistance - RampDownDistance) / RampDownDistance));
drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
} else { } else {
// middle ground // middle ground
drivePower = targetDrivePower; drivePower = targetDrivePower;
@@ -92,24 +98,24 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontRightDrive.setPower(0); robot.frontRightDrive.setPower(0);
setHasFinished(true); setHasFinished(true);
} }
if (!getHasFinished()){ if (!getHasFinished()){
float angle = robot.imu.getAngularOrientation().firstAngle; float angle = robot.imu.getAngularOrientation().firstAngle - direction;
if (angle < 0){ if (angle < -0.25){
robot.backLeftDrive.setPower(-drivePower); robot.backLeftDrive.setPower(drivePower * 0.25);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower * 1.25);
robot.frontLeftDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower * 0.25);
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower * 1.25);
} }
if (angle > 0){ if (angle > 0.25){
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower * 1.25);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(drivePower * 0.25);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower * 1.25);
robot.frontRightDrive.setPower(-drivePower); robot.frontRightDrive.setPower(drivePower * 0.25);
} }
} }
} }
@Override @Override

1
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometerLowerArmParallelState2nd.java
View File

@@ -39,7 +39,6 @@ public class DriverStateWithOdometerLowerArmParallelState2nd extends CyberarmSta
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
addParallelState(new BottomArm(robot, "RightFourCone", "10-1"));
} }