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-11-03 20:32:35 -05:00
parent 495376212e
commit 863ef69690
5 changed files with 118 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -47,12 +47,27 @@ public class TestAutonomousEngine extends CyberarmEngine {
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"));
//drive towards stack of cones while collecting
addState(new CollectorDistanceState(robot, "TestAutonomous", "14-0"));
//drive slightly back
addState(new DriverState(robot, "TestAutonomous", "15-0"));
//lift arm up
addState(new TopArm(robot, "TestAutonomous", "16-0"));
// drive backwards too position
addState(new DriverState(robot, "TestAutonomous", "17-0"));
// rotate
addState(new RotationState(robot, "TestAutonomous", "18-0"));
//lift the upper arm
addState(new TopArm(robot, "TestAutonomous", "19-0"));
//lift the lower arm
addState(new BottomArm(robot, "TestAutonomous", "20-0"));
//drive forward to allign
addState(new DriverState(robot, "TestAutonomous", "21-0"));
// bring arm down.
addState(new TopArm(robot, "TestAutonomous", "22-0"));
// get rid of cone
addState(new CollectorState(robot, "TestAutonomous", "23-0"));

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

@@ -9,6 +9,37 @@ import org.timecrafters.testing.states.PrototypeBot1;
public class CollectorDistanceState extends CyberarmState {
private final PrototypeBot1 robot;
private int traveledDistance;
private int RampUpDistance;
private int RampDownDistance;
private double drivePower;
private double targetDrivePower;
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
}
@Override
public void telemetry() {
engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
}
@Override
public void start() {
@@ -33,10 +64,35 @@ public class CollectorDistanceState extends CyberarmState {
@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);
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
// ramping up
drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
}
else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){
// ramping down
drivePower = ((delta / RampDownDistance) + 0.15);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
@@ -49,9 +105,7 @@ public class CollectorDistanceState extends CyberarmState {
}
}
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
}
}

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

@@ -42,24 +42,22 @@ public class DriverState extends CyberarmState {
return;
}
double delta = traveledDistance - robot.frontRightDrive.getCurrentPosition();
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
drivePower = (((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
// ramping up
drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
}
else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){
drivePower = (((delta - RampDownDistance) / RampDownDistance) + 0.25) - 1;
if (drivePower < 0) {
drivePower = 0.25;
}
} else {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}

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

@@ -18,6 +18,24 @@ public class RotationState extends CyberarmState {
private double drivePower;
private float targetRotation;
float RobotRotation;
private double RotationTarget, DeltaRotation;
private double RotationDirectionMinimum;
public void CalculateDeltaRotation() {
if (RotationTarget >= 0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation);
}
else if (RotationTarget <= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation);
}
else if (RotationTarget >= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget + RobotRotation);
}
else if (RotationTarget <=0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RobotRotation + RotationTarget);
}
}
@Override
public void exec() {
@@ -27,6 +45,14 @@ public class RotationState extends CyberarmState {
}
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = targetRotation;
CalculateDeltaRotation();
if (drivePower < 0){
RotationDirectionMinimum = -0.3;
} else {
RotationDirectionMinimum = 0.3;
}
drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum;
if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);

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

@@ -117,8 +117,8 @@ public class PrototypeBot1 {
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE);
LowRiserLeft.setPosition(0.45);
LowRiserRight.setPosition(0.45);
LowRiserLeft.setPosition(0.35);
LowRiserRight.setPosition(0.35);
HighRiserLeft.setPosition(0.45);
HighRiserRight.setPosition(0.45);