TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 14:22:34 +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"));
// 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)
addState(new BottomArm(robot, "RightFourCone", "05-0"));
@@ -63,7 +64,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
addState(new BottomArm(robot, "RightFourCone", "09-0"));
//
// // 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
addState(new RotationState(robot, "RightFourCone", "11-0"));
@@ -83,7 +85,6 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
//
// // 16 Rotate and use sensor to find junction
// 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)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));

44
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 RampDownDistance;
private int maximumTolerance;
private float direction;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
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.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").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;
}
@@ -32,18 +34,19 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontLeftDrive.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.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
@@ -54,12 +57,15 @@ public class DriverStateWithOdometer extends CyberarmState {
if (Math.abs(CurrentPosition) <= RampUpDistance){
// 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
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
double ratio = (1.0 - ((traveledDistance - RampDownDistance) / RampDownDistance));
drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
} else {
// middle ground
drivePower = targetDrivePower;
}
@@ -92,24 +98,24 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
if (!getHasFinished()){
float angle = robot.imu.getAngularOrientation().firstAngle;
float angle = robot.imu.getAngularOrientation().firstAngle - direction;
if (angle < 0){
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
if (angle < -0.25){
robot.backLeftDrive.setPower(drivePower * 0.25);
robot.backRightDrive.setPower(drivePower * 1.25);
robot.frontLeftDrive.setPower(drivePower * 0.25);
robot.frontRightDrive.setPower(drivePower * 1.25);
}
if (angle > 0){
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
if (angle > 0.25){
robot.backLeftDrive.setPower(drivePower * 1.25);
robot.backRightDrive.setPower(drivePower * 0.25);
robot.frontLeftDrive.setPower(drivePower * 1.25);
robot.frontRightDrive.setPower(drivePower * 0.25);
}
}
}
@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);
addParallelState(new BottomArm(robot, "RightFourCone", "10-1"));
}