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

Autonomous has been wrote to acsend and descend the arms, and camera has been commented out

Browse Source
This commit is contained in:
SpencerPiha
2022-10-22 14:26:44 -05:00
parent 54f95918d5
commit e04b3cb2f2
8 changed files with 160 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,9 +5,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.LowerArm;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.UpperArm;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.testing.states.PrototypeBot1;
@Autonomous (name = "Autonomous Test")
@@ -24,25 +24,25 @@ public class TestAutonomousEngine extends CyberarmEngine {
//turn towards high pole
addState(new RotationState(robot, "TestAutonomous", "02-0"));
//lift the upper arm
addState(new UpperArm(robot, "TestAutonomous", "03-0"));
addState(new TopArm(robot, "TestAutonomous", "03-0"));
//lift the lower arm
addState(new LowerArm(robot, "TestAutonomous", "04-0"));
addState(new BottomArm(robot, "TestAutonomous", "04-0"));
//drive forward
addState(new DriverState(robot, "TestAutonomous", "05-0"));
//lower the bottom arm to get closer
addState(new LowerArm(robot, "TestAutonomous", "06-0"));
addState(new BottomArm(robot, "TestAutonomous", "06-0"));
//make collector release the cone
addState(new CollectorState(robot, "TestAutnomous", "07-0"));
addState(new CollectorState(robot, "TestAutonomous", "07-0"));
//lift the lower arm to clear the pole
addState(new LowerArm(robot, "TestAutonomous", "08-0"));
addState(new BottomArm(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 LowerArm(robot, "TestAutonomous", "11-0"));
addState(new BottomArm(robot, "TestAutonomous", "11-0"));
// lower the upper arm so we dont fall over
addState(new UpperArm(robot, "TestAutonomous", "12-0"));
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"));

59
TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java Normal file
View File

@@ -0,0 +1,59 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class BottomArm extends CyberarmState {
PrototypeBot1 robot;
double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance;
long time;
long lastStepTime = 0;
boolean up;
public BottomArm(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
this.time = robot.configuration.variable(groupName, actionName, "time").value();
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
}
@Override
public void start() {
up = robot.LowRiserLeft.getPosition() < LowerRiserLeftPos;
}
@Override
public void exec() {
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance);
} else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
} else {
setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("LowerRiserRightPos",LowerRiserRightPos);
engine.telemetry.addData("LowerRiserLeftPos",LowerRiserLeftPos);
engine.telemetry.addData("AddedDistance",AddedDistance);
engine.telemetry.addData("left servo", robot.LowRiserLeft.getPosition());
engine.telemetry.addData("right servo", robot.LowRiserRight.getPosition());
engine.telemetry.addData("time",time);
}
}

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

@@ -8,16 +8,18 @@ public class DriverState extends CyberarmState {
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();
}
private float RobotRotation;
private double drivePower;
private int RobotPosition,RobotStartingPosition;
private int RobotPosition,RobotStartingPosition,traveledDistance;
@Override
public void exec() {
if (RobotPosition - RobotStartingPosition < 2500){
if (RobotPosition - RobotStartingPosition < traveledDistance){
drivePower = 1;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
@@ -28,6 +30,7 @@ public class DriverState extends CyberarmState {
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
}

32
TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java
View File

@@ -1,32 +0,0 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class LowerArm extends CyberarmState {
PrototypeBot1 robot;
double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance;
long time;
long lastStepTime = 0;
public LowerArm(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
this.time = robot.configuration.variable(groupName, actionName, "time").value();
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
}
@Override
public void exec() {
if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos) {
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
}
}
}
}

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

@@ -8,18 +8,20 @@ public class RotationState extends CyberarmState {
public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
// this.RobotRotation = robot.configuration.variable(groupName, actionName, "RobotRotation").value();
this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
}
private float RobotRotation;
private double drivePower;
private float targetRotation;
float RobotRotation;
@Override
public void exec() {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation <= 45) {
if (RobotRotation - 3 <= targetRotation || RobotRotation + 3 <= targetRotation) {
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
@@ -30,7 +32,15 @@ public class RotationState extends CyberarmState {
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Robot Rotation", RobotRotation);
engine.telemetry.addData("Drive Power", drivePower);
}
}

60
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java Normal file
View File

@@ -0,0 +1,60 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class TopArm extends CyberarmState {
PrototypeBot1 robot;
double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
long time;
long lastStepTime = 0;
boolean up;
public TopArm(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "UpperRiserLeftPos").value();
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();
}
@Override
public void start() {
up = robot.HighRiserLeft.getPosition() < UpperRiserLeftPos;
}
@Override
public void exec() {
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);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("UpperRiserRightPos",UpperRiserRightPos);
engine.telemetry.addData("UpperRiserLeftPos",UpperRiserLeftPos);
engine.telemetry.addData("AddedDistance",AddedDistance);
engine.telemetry.addData("time",time);
engine.telemetry.addData("servo position left", robot.HighRiserLeft.getPosition());
engine.telemetry.addData("servo position Right", robot.HighRiserRight.getPosition());
}
}

34
TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java
View File

@@ -1,34 +0,0 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class UpperArm extends CyberarmState {
PrototypeBot1 robot;
double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
long time;
long lastStepTime = 0;
public UpperArm(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
this.time = robot.configuration.variable(groupName, actionName, "time").value();
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
}
@Override
public void exec() {
if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos) {
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
} else {
setHasFinished(true);
}
}
}
}

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

@@ -48,8 +48,8 @@ public class PrototypeBot1 {
public PrototypeBot1(CyberarmEngine engine) {
this.engine = engine;
initVuforia();
initTfod();
// initVuforia();
// initTfod();
setupRobot();
}
@@ -66,7 +66,7 @@ public class PrototypeBot1 {
imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
///configuration = new TimeCraftersConfiguration("Phoenix");
configuration = new TimeCraftersConfiguration("Phoenix");
//motors configuration
frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
@@ -99,6 +99,17 @@ public class PrototypeBot1 {
backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
HighRiserLeft.setDirection(Servo.Direction.REVERSE);
HighRiserRight.setDirection(Servo.Direction.FORWARD);
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE);
LowRiserLeft.setPosition(0.45);
LowRiserRight.setPosition(0.45);
HighRiserLeft.setPosition(0.45);
HighRiserRight.setPosition(0.45);
}
private void initVuforia(){