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

driving straight added

Browse Source
This commit is contained in:
SpencerPiha
2023-01-07 16:06:40 -06:00
parent e3089884ea
commit c29ce160a7
5 changed files with 132 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -50,6 +50,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
//pause
addState(new BottomArm(robot, "RightFourCone", "06-2"));
// 7 Drop bottom arm down on the junction to place cone
addState(new BottomArm(robot, "RightFourCone", "07-0"));
@@ -70,18 +73,30 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// 11 Rotate towards stack
addState(new RotationState(robot, "RightFourCone", "11-0"));
//
// // 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
// addState(new TopArm(robot, "RightFourCone", "12-0"));
// 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
addState(new TopArm(robot, "RightFourCone", "12-0"));
// drive forward at the stack without sensor
addState(new DriverStateWithOdometer(robot, "RightFourCone", "12-1"));
// turn and align at stack
addState(new RotationState(robot, "RightFourCone", "12-2"));
//
// // 13 Drive at stack while collecting and check to see when we grab it
// addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
// 13 Drive at stack while collecting and check to see when we grab it
addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
//
// // 14 Back up and raise arm
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
// addState(new TopArm(robot, "RightFourCone", "14-1"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
addState(new TopArm(robot, "RightFourCone", "14-1"));
// 14-2 align perpendicular too the wall
addState(new RotationState(robot, "RightFourCone", "14-2"));
//
// // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
//
// // 16 Rotate and use sensor to find junction
// addState(new RotationState(robot, "RightFourCone", "16-0"));

39
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
View File

@@ -44,6 +44,7 @@ public class CollectorDistanceState extends CyberarmState {
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData("traveledDistance", traveledDistance);
@@ -74,12 +75,14 @@ public class CollectorDistanceState 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_WITHOUT_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(1);
@@ -96,6 +99,10 @@ public class CollectorDistanceState extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
return;
}
@@ -108,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState {
distanceDelta = LastDistanceRead - currentDistance;
if (distanceDelta >= 0.0 || currentDistance > 500) {
if (distanceDelta >= -50.0 || currentDistance > 500) {
// I am moving forward
// and im close too my target.
LastDistanceRead = currentDistance;
@@ -119,18 +126,24 @@ public class CollectorDistanceState extends CyberarmState {
debugStatus = "Nothing Collected";
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
return;
}
}
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
double delta = traveledDistance - Math.abs(robot.OdometerEncoder.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
} else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
drivePower = (Math.abs((float) robot.OdometerEncoder.getCurrentPosition()) / RampUpDistance) + 0.15;
} else if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.15);
} else {
@@ -147,7 +160,7 @@ public class CollectorDistanceState extends CyberarmState {
drivePower = drivePower * -1;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
@@ -167,6 +180,10 @@ public class CollectorDistanceState extends CyberarmState {
if (runTime() - inRangeTime >= collectTime){
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
}
}

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

@@ -10,10 +10,11 @@ import org.timecrafters.TeleOp.states.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState {
private final boolean stateDisabled;
PhoenixBot1 robot;
private int RampUpDistance;
private int RampDownDistance;
private double RampUpDistance;
private double RampDownDistance;
private int maximumTolerance;
private float direction;
private boolean targetAchieved = false;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -34,11 +35,13 @@ 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_WITHOUT_ENCODER);
@@ -52,18 +55,24 @@ public class DriverStateWithOdometer extends CyberarmState {
return;
}
double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
double delta = traveledDistance - Math.abs(CurrentPosition);
double CurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition());
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up
float ratio = (Math.abs((float)CurrentPosition) / RampUpDistance);
drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
// double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
if (targetDrivePower > 0) {
drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
} else {
drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
}
}
else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
// ramping down
double ratio = (1.0 - ((traveledDistance - RampDownDistance) / RampDownDistance));
drivePower = (targetDrivePower - 0.25) * ratio + 0.25;
if (targetDrivePower > 0){
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
} else {
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
}
} else {
// middle ground
@@ -75,11 +84,22 @@ public class DriverStateWithOdometer extends CyberarmState {
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
if (targetDrivePower < 0 && drivePower > 0) {
drivePower = drivePower * -1;
}
if (Math.abs(CurrentPosition) < traveledDistance - maximumTolerance){
if (targetAchieved) {
drivePower = drivePower * 0.25;
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
drivePower = 0.25;
}
}
}
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
@@ -87,10 +107,24 @@ public class DriverStateWithOdometer extends CyberarmState {
}
else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
robot.backLeftDrive.setPower(targetDrivePower * -0.25);
robot.backRightDrive.setPower(targetDrivePower * -0.25);
robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
robot.frontRightDrive.setPower(targetDrivePower * -0.25);
targetAchieved = true;
drivePower = targetDrivePower * -0.25;
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
drivePower = 0.25;
}
}
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
} else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
@@ -98,24 +132,25 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
if (!getHasFinished()){
if (!getHasFinished() && !targetAchieved){
float angle = robot.imu.getAngularOrientation().firstAngle - direction;
if (targetDrivePower < 0) { angle = angle * -1;}
if (angle < -0.25){
robot.backLeftDrive.setPower(drivePower * 0.25);
robot.backLeftDrive.setPower(drivePower * 0);
robot.backRightDrive.setPower(drivePower * 1.25);
robot.frontLeftDrive.setPower(drivePower * 0.25);
robot.frontLeftDrive.setPower(drivePower * 0);
robot.frontRightDrive.setPower(drivePower * 1.25);
}
if (angle > 0.25){
if (angle > 0.25) {
robot.backLeftDrive.setPower(drivePower * 1.25);
robot.backRightDrive.setPower(drivePower * 0.25);
robot.backRightDrive.setPower(drivePower * 0);
robot.frontLeftDrive.setPower(drivePower * 1.25);
robot.frontRightDrive.setPower(drivePower * 0.25);
robot.frontRightDrive.setPower(drivePower * 0);
}
}
}
@Override
@@ -133,6 +168,9 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
engine.telemetry.addData("Target Achieved", targetAchieved);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);

7
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
View File

@@ -36,6 +36,13 @@ public class RotationState extends CyberarmState {
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.3 * drivePower;
if (Math.abs(drivePowerVariable) < 0.3) {
if (drivePowerVariable < 0){
drivePowerVariable = -0.3;
} else {
drivePowerVariable = 0.3;
}
}
debugStatus = "Rotate Slow";
} // end of if
else {

15
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java Normal file
View File

@@ -0,0 +1,15 @@
package org.timecrafters.TeleOp.engine;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.PhoenixBot1;
public class DriveDebugEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void setup() {
robot = new PhoenixBot1(this);
}
}