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

autonomous work right side almost done

Browse Source
This commit is contained in:
SpencerPiha
2023-01-17 20:01:01 -06:00
parent a5a8260ae6
commit 7f7ebf6d20
11 changed files with 290 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
@@ -16,6 +17,7 @@ import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Left Side")
@Disabled
public class LeftSideAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;

186
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java Normal file
View File

@@ -0,0 +1,186 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "left 2 cone auto")
public class LeftTwoConeAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void setup() {
robot = new PhoenixBot1(this);
// 1 Rotate camera down at the signal
addState(new ServoCameraRotate(robot, "LeftTwoCone", "01-0"));
// 2 Scan custom image
addState(new ConeIdentification(robot, "LeftTwoCone", "02-0"));
// 3 Rotate Camera up, out of the way so it doesn't crash into stuff
addState(new ServoCameraRotate(robot, "LeftTwoCone", "03-0"));
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "04-0"));
// addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "04-1"));
addState(new TopArm(robot, "LeftTwoCone", "04-1"));
// 6 Raise lower arm while slowly driving at the junction (parallel state)
addState(new BottomArm(robot, "LeftTwoCone", "05-0"));
// 6 Turn Towards and look for junction with sensor
addState(new RotationState(robot, "LeftTwoCone", "06-0"));
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "06-1"));
// 6-3 align to junction with rotation or skip if it looks like it won't be able to
addState(new JunctionAllignmentDistanceState(robot, "LeftTwoCone", "06-3"));
addState(new JunctionAllignmentAngleState(robot, "LeftTwoCone", "06-4"));
//pause
addState(new BottomArm(robot, "LeftTwoCone", "06-2"));
// 7 Drop bottom arm down on the junction to place cone
addState(new BottomArm(robot, "LeftTwoCone", "07-0"));
addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "07-1"));
// 7-1 drive back slightly
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "07-2"));
// 8 Drop cone as soon as arm is in position
addState(new CollectorState(robot, "LeftTwoCone", "08-0"));
// 8-1 drive back to lose contact
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "08-1"));
// 8-1 realign to old angle
addState(new RotationState(robot, "LeftTwoCone", "08-2"));
// 9 Raise bottom arm to clear junction
addParallelStateToLastState(new BottomArm(robot, "LeftTwoCone", "09-0"));
addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "09-1"));
// // 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "LeftTwoCone", "10-1"));
// 11 Rotate towards stack
addState(new RotationState(robot, "LeftTwoCone", "11-0"));
//
// 12 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
addState(new TopArm(robot, "LeftTwoCone", "12-0"));
// drive forward at the stack without sensor
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "12-1"));
// turn and align at stack
addState(new RotationState(robot, "LeftTwoCone", "12-2"));
//
// 13 Drive at stack while collecting and check to see when we grab it
addState(new CollectorDistanceState(robot, "LeftTwoCone", "13-0"));
//
// // 14 Back up and raise arm
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "14-0"));
addState(new TopArm(robot, "LeftTwoCone", "14-1"));
// 14-2 align perpendicular too the wall
addState(new RotationState(robot, "LeftTwoCone", "14-2"));
//
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "15-0"));
//
// 16 Rotate and use sensor to find junction
addState(new RotationState(robot, "LeftTwoCone", "16-0"));
//
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "17-0"));
// // 18 Bring upper arm down
addState(new TopArm(robot, "LeftTwoCone", "18-0"));
//
// // 19 Drop cone
addState(new CollectorState(robot, "LeftTwoCone", "19-0"));
addParallelStateToLastState(new TopArm(robot, "LeftTwoCone", "19-1"));
//
// // 20 Bring upper arm up
// addState(new TopArm(robot, "LeftTwoCone", "20-0"));
//
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "21-0"));
//
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
// addState(new TopArm(robot, "LeftTwoCone", "22-0"));
//
// // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
// addState(new CollectorDistanceState(robot, "LeftTwoCone", "23-0"));
//
// // 24 Drive Back and lift up all the way to position for the low junction
// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "24-0"));
// addState(new TopArm(robot, "LeftTwoCone", "24-1"));
//
// // 25 Drive back faster after the cone is fully off of the stack
// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "25-0"));
//
// // 26 Turn and look for the low junction with the distance sensor and align
// addState(new RotationState(robot, "LeftTwoCone", "26-0"));
//
// // 27 Drive forward / backwards if you need to. (check with distance sensor)
// addState(new JunctionAllignmentState(robot, "LeftTwoCone", "26-1"));
//
// // 28 Bring Upper arm down on junction
// addState(new TopArm(robot, "LeftTwoCone", "28-0"));
//
// // 29 Let go of cone right after arm is in position
// addState(new CollectorState(robot, "LeftTwoCone", "29-0"));
//
// // 30 Raise arm as soon as the cone is dropped
// addState(new TopArm(robot, "LeftTwoCone", "30-0"));
//
// // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
// addState(new DriverStateWithOdometer(robot, "LeftTwoCone", "31-0"));
//
// // 32 Rotate towards Stack of cones
addState(new RotationState(robot, "LeftTwoCone", "32-0"));
//
// // 33 Decide which path after scanning image from earlier
addState(new PathDecision(robot, "LeftTwoCone", "33-0"));
//
// // 34 Drive backwards, forwards, or stay put
addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-1"));
addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-2"));
addState(new DriverParkPlaceState(robot, "LeftTwoCone", "34-3"));
//
// // 35 Rotate towards alliance terminal
addState(new RotationState(robot, "LeftTwoCone", "35-0"));
addState(new TopArm(robot, "RightTwoCone", "36-0"));
}
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
}
}

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
@@ -15,6 +16,7 @@ import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Right Side")
@Disabled
public class RightSideAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;

119
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java
View File

@@ -7,6 +7,7 @@ import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
@@ -16,9 +17,9 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@Autonomous (name = "4 cone auto right")
@Autonomous (name = "Right 2 cone auto")
public class RightFourConeAutonomousEngine extends CyberarmEngine {
public class RightTwoConeAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
@@ -26,147 +27,153 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
robot = new PhoenixBot1(this);
// 1 Rotate camera down at the signal
addState(new ServoCameraRotate(robot, "RightFourCone", "01-0"));
addState(new ServoCameraRotate(robot, "RightTwoCone", "01-0"));
// 2 Scan custom image
addState(new ConeIdentification(robot, "RightFourCone", "02-0"));
addState(new ConeIdentification(robot, "RightTwoCone", "02-0"));
// 3 Rotate Camera up, out of the way so it doesn't crash into stuff
addState(new ServoCameraRotate(robot, "RightFourCone", "03-0"));
addState(new ServoCameraRotate(robot, "RightTwoCone", "03-0"));
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0"));
// addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
addState(new TopArm(robot, "RightFourCone", "04-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "04-0"));
// addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "04-1"));
addState(new TopArm(robot, "RightTwoCone", "04-1"));
// 6 Raise lower arm while slowly driving at the junction (parallel state)
addState(new BottomArm(robot, "RightFourCone", "05-0"));
addState(new BottomArm(robot, "RightTwoCone", "05-0"));
// 6 Turn Towards and look for junction with sensor
addState(new RotationState(robot, "RightFourCone", "06-0"));
addState(new RotationState(robot, "RightTwoCone", "06-0"));
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "06-1"));
// 6-3 align to junction with rotation or skip if it looks like it won't be able to
addState(new JunctionAllignmentDistanceState(robot, "RightFourCone", "06-3"));
addState(new JunctionAllignmentAngleState(robot, "RightFourCone", "06-4"));
addState(new JunctionAllignmentDistanceState(robot, "RightTwoCone", "06-3"));
addState(new JunctionAllignmentAngleState(robot, "RightTwoCone", "06-4"));
//pause
addState(new BottomArm(robot, "RightFourCone", "06-2"));
addState(new BottomArm(robot, "RightTwoCone", "06-2"));
// 7 Drop bottom arm down on the junction to place cone
addState(new BottomArm(robot, "RightFourCone", "07-0"));
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "07-1"));
addState(new BottomArm(robot, "RightTwoCone", "07-0"));
addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "07-1"));
// 7-1 drive back slightly
addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-2"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "07-2"));
// 8 Drop cone as soon as arm is in position
addState(new CollectorState(robot, "RightFourCone", "08-0"));
addState(new CollectorState(robot, "RightTwoCone", "08-0"));
// 8-1 drive back to lose contact
addState(new DriverStateWithOdometer(robot, "RightFourCone", "08-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "08-1"));
// 8-1 realign to old angle
addState(new RotationState(robot, "RightFourCone", "08-2"));
addState(new RotationState(robot, "RightTwoCone", "08-2"));
// 9 Raise bottom arm to clear junction
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "09-0"));
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "09-1"));
addParallelStateToLastState(new BottomArm(robot, "RightTwoCone", "09-0"));
addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "09-1"));
// // 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "10-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "10-0"));
addParallelStateToLastState(new BottomArm(robot, "RightTwoCone", "10-1"));
// 11 Rotate towards stack
addState(new RotationState(robot, "RightFourCone", "11-0"));
addState(new RotationState(robot, "RightTwoCone", "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"));
addState(new TopArm(robot, "RightTwoCone", "12-0"));
// drive forward at the stack without sensor
addState(new DriverStateWithOdometer(robot, "RightFourCone", "12-1"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "12-1"));
// turn and align at stack
addState(new RotationState(robot, "RightFourCone", "12-2"));
addState(new RotationState(robot, "RightTwoCone", "12-2"));
//
// 13 Drive at stack while collecting and check to see when we grab it
addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
addState(new CollectorDistanceState(robot, "RightTwoCone", "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, "RightTwoCone", "14-0"));
addState(new TopArm(robot, "RightTwoCone", "14-1"));
// 14-2 align perpendicular too the wall
addState(new RotationState(robot, "RightFourCone", "14-2"));
addState(new RotationState(robot, "RightTwoCone", "14-2"));
//
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "15-0"));
//
// 16 Rotate and use sensor to find junction
addState(new RotationState(robot, "RightFourCone", "16-0"));
addState(new RotationState(robot, "RightTwoCone", "16-0"));
//
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "17-0"));
// // 18 Bring upper arm down
addState(new TopArm(robot, "RightFourCone", "18-0"));
addState(new TopArm(robot, "RightTwoCone", "18-0"));
//
// // 19 Drop cone
addState(new CollectorState(robot, "RightFourCone", "19-0"));
addState(new CollectorState(robot, "RightTwoCone", "19-0"));
addParallelStateToLastState(new TopArm(robot, "RightTwoCone", "19-1"));
//
// // 20 Bring upper arm up
// addState(new TopArm(robot, "RightFourCone", "20-0"));
// addState(new TopArm(robot, "RightTwoCone", "20-0"));
//
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
addState(new DriverStateWithOdometer(robot, "RightTwoCone", "21-0"));
//
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
// addState(new TopArm(robot, "RightFourCone", "22-0"));
// addState(new TopArm(robot, "RightTwoCone", "22-0"));
//
// // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
// addState(new CollectorDistanceState(robot, "RightFourCone", "23-0"));
// addState(new CollectorDistanceState(robot, "RightTwoCone", "23-0"));
//
// // 24 Drive Back and lift up all the way to position for the low junction
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0"));
// addState(new TopArm(robot, "RightFourCone", "24-1"));
// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "24-0"));
// addState(new TopArm(robot, "RightTwoCone", "24-1"));
//
// // 25 Drive back faster after the cone is fully off of the stack
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0"));
// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "25-0"));
//
// // 26 Turn and look for the low junction with the distance sensor and align
// addState(new RotationState(robot, "RightFourCone", "26-0"));
// addState(new RotationState(robot, "RightTwoCone", "26-0"));
//
// // 27 Drive forward / backwards if you need to. (check with distance sensor)
// addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1"));
// addState(new JunctionAllignmentState(robot, "RightTwoCone", "26-1"));
//
// // 28 Bring Upper arm down on junction
// addState(new TopArm(robot, "RightFourCone", "28-0"));
// addState(new TopArm(robot, "RightTwoCone", "28-0"));
//
// // 29 Let go of cone right after arm is in position
// addState(new CollectorState(robot, "RightFourCone", "29-0"));
// addState(new CollectorState(robot, "RightTwoCone", "29-0"));
//
// // 30 Raise arm as soon as the cone is dropped
// addState(new TopArm(robot, "RightFourCone", "30-0"));
// addState(new TopArm(robot, "RightTwoCone", "30-0"));
//
// // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
// addState(new DriverStateWithOdometer(robot, "RightTwoCone", "31-0"));
//
// // 32 Rotate towards Stack of cones
addState(new RotationState(robot, "RightFourCone", "32-0"));
addState(new RotationState(robot, "RightTwoCone", "32-0"));
//
// // 33 Decide which path after scanning image from earlier
addState(new PathDecision(robot, "RightFourCone", "33-0"));
addState(new PathDecision(robot, "RightTwoCone", "33-0"));
//
// // 34 Drive backwards, forwards, or stay put
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3"));
addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-1"));
addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-2"));
addState(new DriverParkPlaceState(robot, "RightTwoCone", "34-3"));
//
// // 35 Rotate towards alliance terminal
// addState(new RotationState(robot, "RightFourCone", "35-0"));
addState(new RotationState(robot, "RightTwoCone", "35-0"));
addState(new TopArm(robot, "RightTwoCone", "36-0"));
}

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java
View File

@@ -89,9 +89,9 @@ public class ConeIdentification extends CyberarmState {
if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) {
bestConfidence = recognition.getConfidence();
if (recognition.getLabel().equals("2 Bulb")) {
if (recognition.getLabel().equals("#2")) {
engine.blackboardSet("parkPlace", "2");
} else if (recognition.getLabel().equals("3 Panel")) {
} else if (recognition.getLabel().equals("#3")) {
engine.blackboardSet("parkPlace", "3");
} else {

11
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java
View File

@@ -49,12 +49,12 @@ public class DriverParkPlaceState extends CyberarmState {
setHasFinished(true);
}
if (placement.equals(intendedPlacement)) {
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
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.25;
} else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
drivePower = (Math.abs((float) robot.OdometerEncoder.getCurrentPosition()) / RampUpDistance) + 0.25;
} else if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
@@ -71,7 +71,7 @@ public class DriverParkPlaceState 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);
@@ -96,6 +96,7 @@ public class DriverParkPlaceState 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.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);

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

@@ -15,6 +15,7 @@ public class DriverStateWithOdometer extends CyberarmState {
private int maximumTolerance;
private float direction;
private boolean targetAchieved = false;
private double CurrentPosition;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -36,12 +37,14 @@ public class DriverStateWithOdometer extends CyberarmState {
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.OdometerEncoderLeft.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.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -55,7 +58,12 @@ public class DriverStateWithOdometer extends CyberarmState {
return;
}
double CurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition());
double RightCurrentPosition = Math.abs(robot.OdometerEncoder.getCurrentPosition());
double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up

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

@@ -35,12 +35,12 @@ public class RotationState extends CyberarmState {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.3 * drivePower;
if (Math.abs(drivePowerVariable) < 0.3) {
drivePowerVariable = 0.4 * drivePower;
if (Math.abs(drivePowerVariable) < 0.4) {
if (drivePowerVariable < 0){
drivePowerVariable = -0.3;
drivePowerVariable = -0.4;
} else {
drivePowerVariable = 0.3;
drivePowerVariable = 0.4;
}
}
debugStatus = "Rotate Slow";

8
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -44,7 +44,7 @@ public class PhoenixBot1 {
public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo;
private final CyberarmEngine engine;
public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance;
public Rev2mDistanceSensor collectorDistance, /*downSensor,*/ leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, OdometerEncoder, OdometerEncoderLeft;
@@ -104,7 +104,7 @@ public class PhoenixBot1 {
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
// downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance");
@@ -195,8 +195,8 @@ public class PhoenixBot1 {
LowRiserLeft.setPosition(0.35);
LowRiserRight.setPosition(0.35);
HighRiserLeft.setPosition(0.45);
HighRiserRight.setPosition(0.45);
HighRiserLeft.setPosition(0.40);
HighRiserRight.setPosition(0.40);
CameraServo.setPosition(0.775);

22
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -58,7 +58,7 @@ public class PhoenixTeleOPState extends CyberarmState {
engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation);
engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM));
// engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Odometer Encoder, Right", robot.OdometerEncoder.getCurrentPosition());
@@ -483,15 +483,15 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.imu.initialize(parameters);
}
}
public double downSensor() {
double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
return Distance;
// public double downSensor() {
// double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
// Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
// return Distance;
}
}