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

autnomous work added

Browse Source
This commit is contained in:
SpencerPiha
2022-10-29 12:03:31 -05:00
parent 4a698f2f07
commit 5b0643720b
8 changed files with 145 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java
View File

@@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.BottomArm;
@@ -31,21 +32,28 @@ public class TestAutonomousEngine extends CyberarmEngine {
//drive forward
addState(new DriverState(robot, "TestAutonomous", "05-0"));
//lower the bottom arm to get closer
addState(new BottomArm(robot, "TestAutonomous", "06-0"));
addState(new TopArm(robot, "TestAutonomous", "06-0"));
//make collector release the cone
addState(new CollectorState(robot, "TestAutonomous", "07-0"));
//lift the lower arm to clear the pole
addState(new BottomArm(robot, "TestAutonomous", "08-0"));
addState(new TopArm(robot, "TestAutonomous", "08-0"));
//Drive Backwards
addState(new DriverState(robot, "TestAutonomous", "09-0"));
// Rotate to either set up for cones to grab or park somewhere
addState(new RotationState(robot, "TestAutonomous", "10-0"));
// lower the bottom arm so we dont fall over
addState(new BottomArm(robot, "TestAutonomous", "11-0"));
addState(new BottomArm(robot, "TestAutonomous", "10-0"));
// lower the upper arm so we dont fall over
addState(new TopArm(robot, "TestAutonomous", "12-0"));
//either dont move if your in the right zone otherwise drive forward a specific variable if we aren't already in the right spot
addState(new DriverState(robot, "TestAutonomous", "13-0"));
addState(new TopArm(robot, "TestAutonomous", "11-0"));;
// Rotate to either set up for cones to grab or park somewhere
addState(new RotationState(robot, "TestAutonomous", "12-0"));
//adjust arm height to cone.
addState(new TopArm(robot, "TestAutonomous", "13-0"));
//drive towards stack of cones
addState(new CollectorDistanceState(robot, "", ""));
// DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0");
// addState(driverState);
// //collect next cone
// driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-0"));

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

@@ -5,6 +5,7 @@ import org.timecrafters.testing.states.PrototypeBot1;
public class BottomArm extends CyberarmState {
private final boolean stateDisabled;
PrototypeBot1 robot;
double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance;
long time;
@@ -18,6 +19,8 @@ public class BottomArm extends CyberarmState {
this.time = robot.configuration.variable(groupName, actionName, "time").value();
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@Override
@@ -27,6 +30,10 @@ public class BottomArm extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
setHasFinished(true);
return;
}
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();

57
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java Normal file
View File

@@ -0,0 +1,57 @@
package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.testing.states.PrototypeBot1;
public class CollectorDistanceState extends CyberarmState {
private final PrototypeBot1 robot;
@Override
public void start() {
robot.frontRightDrive.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.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_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);
}
@Override
public void exec() {
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
robot.frontRightDrive.setPower(0.15);
robot.frontLeftDrive.setPower(0.15);
robot.backRightDrive.setPower(0.15);
robot.backLeftDrive.setPower(0.15);
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
setHasFinished(true);
}
}
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
}
}

12
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java
View File

@@ -1,11 +1,13 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.testing.states.PrototypeBot1;
public class CollectorState extends CyberarmState {
private final PrototypeBot1 robot;
private final boolean stateDisabled;
private boolean collecting;
private long duration;
private long BeginningofActionTime;
@@ -16,6 +18,9 @@ public class CollectorState extends CyberarmState {
this.duration = robot.configuration.variable(groupName, actionName, "Duration").value();
this.collecting = robot.configuration.variable(groupName, actionName, "Collecting").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@@ -23,6 +28,7 @@ public class CollectorState extends CyberarmState {
public void telemetry() {
engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition());
engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition());
engine.telemetry.addData("Collector Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
}
@Override
@@ -32,12 +38,16 @@ public class CollectorState extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
setHasFinished(true);
return;
}
if (System.currentTimeMillis() - BeginningofActionTime < duration) {
if (collecting) {
robot.collectorRight.setPower(1);
robot.collectorLeft.setPower(1);
} else {
} else if (collecting != true){
robot.collectorRight.setPower(-1);
robot.collectorLeft.setPower(-1);

14
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java
View File

@@ -6,12 +6,14 @@ import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class DriverState extends CyberarmState {
private final boolean stateDisabled;
PrototypeBot1 robot;
public DriverState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower;
private int traveledDistance;
@@ -23,14 +25,18 @@ public class DriverState extends CyberarmState {
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_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.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);
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
robot.backLeftDrive.setPower(drivePower);

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

@@ -4,11 +4,14 @@ import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class RotationState extends CyberarmState {
private final boolean stateDisabled;
PrototypeBot1 robot;
public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@@ -18,7 +21,10 @@ public class RotationState extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
setHasFinished(true);
return;
}
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {

44
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java
View File

@@ -5,11 +5,13 @@ import org.timecrafters.testing.states.PrototypeBot1;
public class TopArm extends CyberarmState {
private final boolean stateDisabled;
PrototypeBot1 robot;
double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
long time;
long lastStepTime = 0;
boolean up;
boolean directPosition;
public TopArm(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
@@ -17,6 +19,10 @@ public class TopArm extends CyberarmState {
this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "UpperRiserRightPos").value();
this.time = robot.configuration.variable(groupName, actionName, "time").value();
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@Override
@@ -26,22 +32,36 @@ public class TopArm extends CyberarmState {
@Override
public void exec() {
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
if (stateDisabled){
setHasFinished(true);
return;
}
if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) {
if (directPosition) {
robot.HighRiserLeft.setPosition(UpperRiserLeftPos);
robot.HighRiserRight.setPosition(UpperRiserRightPos);
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
} else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
} else {
if (runTime() >= time){
setHasFinished(true);
}
} else {
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
} else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
} else {
setHasFinished(true);
}
}
}
}

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

@@ -1,6 +1,7 @@
package org.timecrafters.testing.states;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -35,6 +36,8 @@ public class PrototypeBot1 {
public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight;
private final CyberarmEngine engine;
public Rev2mDistanceSensor collectorDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
public CRServo collectorLeft, collectorRight;
@@ -53,6 +56,8 @@ public class PrototypeBot1 {
}
private void setupRobot () {
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU;