mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 15:32:35 +00:00
RightSideAutonomous and leftsideAutonomous work added
This commit is contained in:
SpencerPiha
@@ -0,0 +1,96 @@
|
||||
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.DriverState;
|
||||
import org.timecrafters.Autonomous.States.PathDecision;
|
||||
import org.timecrafters.Autonomous.States.RotationState;
|
||||
import org.timecrafters.Autonomous.States.TopArm;
|
||||
import org.timecrafters.testing.states.PrototypeBot1;
|
||||
|
||||
@Autonomous (name = "Left Side")
|
||||
|
||||
public class LeftSideAutonomousEngine extends CyberarmEngine {
|
||||
PrototypeBot1 robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
robot = new PrototypeBot1(this);
|
||||
addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0"));
|
||||
//drive to high pole
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "01-0"));
|
||||
//turn towards high pole
|
||||
addState(new RotationState(robot, "LeftSideAutonomous", "02-0"));
|
||||
//lift the upper arm
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "03-0"));
|
||||
//lift the lower arm
|
||||
addState(new BottomArm(robot, "LeftSideAutonomous", "04-0"));
|
||||
//drive forward
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "05-0"));
|
||||
//lower the bottom arm to get closer
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "06-0"));
|
||||
//make collector release the cone
|
||||
addState(new CollectorState(robot, "LeftSideAutonomous", "07-0"));
|
||||
//lift the lower arm to clear the pole
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "08-0"));
|
||||
//Drive Backwards
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "09-0"));
|
||||
// Rotate to either set up for cones to grab or park somewhere
|
||||
addState(new RotationState(robot, "LeftSideAutonomous", "12-0"));
|
||||
// lower the bottom arm so we dont fall over
|
||||
addState(new BottomArm(robot, "LeftSideAutonomous", "10-0"));
|
||||
// lower the upper arm so we dont fall over
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "11-0"));;
|
||||
//adjust arm height to cone.
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "13-0"));
|
||||
//drive towards stack of cones while collecting
|
||||
addState(new CollectorDistanceState(robot, "LeftSideAutonomous", "14-0"));
|
||||
//drive slightly back
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "15-0"));
|
||||
// face to -90
|
||||
addState(new RotationState(robot, "LeftSideAutonomous", "15-1"));
|
||||
//lift arm up
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "16-0"));
|
||||
// drive backwards too position
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "17-0"));
|
||||
// rotate
|
||||
addState(new RotationState(robot, "LeftSideAutonomous", "18-0"));
|
||||
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "21-0"));
|
||||
// bring arm down.
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "22-0"));
|
||||
// get rid of cone
|
||||
addState(new CollectorState(robot, "LeftSideAutonomous", "23-0"));
|
||||
// lift arm up to clear
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "24-0"));
|
||||
// rotate towards stack of cones
|
||||
addState(new RotationState(robot, "LeftSideAutonomous", "28-0"));
|
||||
// Choose Parking Spot
|
||||
addState(new PathDecision(robot, "LeftSideAutonomous", "29-0"));
|
||||
// case 1 drive forward
|
||||
addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-1"));
|
||||
// case 2 drive forward
|
||||
addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-2"));
|
||||
// case 3 drive forward
|
||||
addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "29-3"));
|
||||
// zero out
|
||||
addState(new RotationState(robot, "LeftSideAutonomous", "30-0"));
|
||||
// Top Arm Down
|
||||
addState(new TopArm(robot, "LeftSideAutonomous", "28-1"));
|
||||
//Drive Backwards
|
||||
addState(new DriverState(robot, "LeftSideAutonomous", "30-1"));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
super.loop();
|
||||
|
||||
telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
package org.timecrafters.Autonomous.Engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
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.DriverState;
|
||||
import org.timecrafters.Autonomous.States.BottomArm;
|
||||
import org.timecrafters.Autonomous.States.PathDecision;
|
||||
import org.timecrafters.Autonomous.States.RotationState;
|
||||
import org.timecrafters.Autonomous.States.TopArm;
|
||||
import org.timecrafters.testing.states.PrototypeBot1;
|
||||
|
||||
@Autonomous (name = "Right Side")
|
||||
|
||||
public class RightSideAutonomousEngine extends CyberarmEngine {
|
||||
PrototypeBot1 robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
robot = new PrototypeBot1(this);
|
||||
addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0"));
|
||||
//drive to high pole
|
||||
addState(new DriverState(robot, "RightSideAutonomous", "01-0"));
|
||||
//turn towards high pole
|
||||
addState(new RotationState(robot, "RightSideAutonomous", "02-0"));
|
||||
//lift the upper arm
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "03-0"));
|
||||
//lift the lower arm
|
||||
addState(new BottomArm(robot, "RightSideAutonomous", "04-0"));
|
||||
//drive forward
|
||||
addState(new DriverState(robot, "RightSideAutonomous", "05-0"));
|
||||
//lower the bottom arm to get closer
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "06-0"));
|
||||
//make collector release the cone
|
||||
addState(new CollectorState(robot, "RightSideAutonomous", "07-0"));
|
||||
//lift the lower arm to clear the pole
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "08-0"));
|
||||
//Drive Backwards
|
||||
addState(new DriverState(robot, "RightSideAutonomous", "09-0"));
|
||||
// lower the bottom arm so we dont fall over
|
||||
addState(new BottomArm(robot, "RightSideAutonomous", "10-0"));
|
||||
// lower the upper arm so we dont fall over
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "11-0"));;
|
||||
// Rotate to either set up for cones to grab or park somewhere
|
||||
addState(new RotationState(robot, "RightSideAutonomous", "12-0"));
|
||||
//adjust arm height to cone.
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "13-0"));
|
||||
//drive towards stack of cones while collecting
|
||||
addState(new CollectorDistanceState(robot, "RightSideAutonomous", "14-0"));
|
||||
//drive slightly back
|
||||
addState(new DriverState(robot, "RightSideAutonomous", "15-0"));
|
||||
// face to -90
|
||||
addState(new RotationState(robot, "RightSideAutonomous", "15-1"));
|
||||
//lift arm up
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "16-0"));
|
||||
// drive backwards too position
|
||||
addState(new DriverState(robot, "RightSideAutonomous", "17-0"));
|
||||
// rotate
|
||||
addState(new RotationState(robot, "RightSideAutonomous", "18-0"));
|
||||
|
||||
addState(new DriverState(robot, "RightSideAutonomous", "21-0"));
|
||||
// bring arm down.
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "22-0"));
|
||||
// get rid of cone
|
||||
addState(new CollectorState(robot, "RightSideAutonomous", "23-0"));
|
||||
// lift arm up to clear
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "24-0"));
|
||||
// rotate towards stack of cones
|
||||
addState(new RotationState(robot, "RightSideAutonomous", "28-0"));
|
||||
// Choose Parking Spot
|
||||
addState(new PathDecision(robot, "RightSideAutonomous", "29-0"));
|
||||
// case 1 drive forward
|
||||
addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-1"));
|
||||
// case 2 drive forward
|
||||
addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-2"));
|
||||
// case 3 drive forward
|
||||
addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "29-3"));
|
||||
// zero out
|
||||
addState(new RotationState(robot, "RightSideAutonomous", "30-0"));
|
||||
// Top Arm Down
|
||||
addState(new TopArm(robot, "RightSideAutonomous", "28-1"));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
super.loop();
|
||||
|
||||
telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
|
||||
}
|
||||
}
|
||||
@@ -1,89 +0,0 @@
|
||||
package org.timecrafters.Autonomous.Engines;
|
||||
|
||||
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.ConeIdentification;
|
||||
import org.timecrafters.Autonomous.States.DriverState;
|
||||
import org.timecrafters.Autonomous.States.BottomArm;
|
||||
import org.timecrafters.Autonomous.States.RotationState;
|
||||
import org.timecrafters.Autonomous.States.TopArm;
|
||||
import org.timecrafters.testing.states.PrototypeBot1;
|
||||
|
||||
@Autonomous (name = "Autonomous Test")
|
||||
|
||||
public class TestAutonomousEngine extends CyberarmEngine {
|
||||
PrototypeBot1 robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
robot = new PrototypeBot1(this);
|
||||
addState(new ConeIdentification(robot, "TestAutonomous", "00-0"));
|
||||
//drive to high pole
|
||||
addState(new DriverState(robot, "TestAutonomous", "01-0"));
|
||||
//turn towards high pole
|
||||
addState(new RotationState(robot, "TestAutonomous", "02-0"));
|
||||
//lift the upper arm
|
||||
addState(new TopArm(robot, "TestAutonomous", "03-0"));
|
||||
//lift the lower arm
|
||||
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 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 TopArm(robot, "TestAutonomous", "08-0"));
|
||||
//Drive Backwards
|
||||
addState(new DriverState(robot, "TestAutonomous", "09-0"));
|
||||
// lower the bottom arm so we dont fall over
|
||||
addState(new BottomArm(robot, "TestAutonomous", "10-0"));
|
||||
// lower the upper arm so we dont fall over
|
||||
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 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"));
|
||||
// lift arm up to clear
|
||||
addState(new TopArm(robot, "TestAutonomous", "24-0"));
|
||||
// drive back
|
||||
addState(new DriverState(robot, "TestAutonomous", "25-0"));
|
||||
// bring bottom arm down
|
||||
addState(new BottomArm(robot, "TestAutonomous", "26-0"));
|
||||
// bring top arm down
|
||||
addState(new TopArm(robot, "TestAutonomous", "27-0"));
|
||||
// rotate towards stack of cones
|
||||
addState(new RotationState(robot, "TestAutonomous", "28-0"));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
super.loop();
|
||||
|
||||
telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
|
||||
}
|
||||
}
|
||||
@@ -102,45 +102,46 @@ public class CollectorDistanceState extends CyberarmState {
|
||||
// and im close too my target.
|
||||
LastDistanceRead = currentDistance;
|
||||
debugRunTime = runTime();
|
||||
debugStatus = "RUNNING";
|
||||
debugStatus = "Driving Towards Cone";
|
||||
} else {
|
||||
// I have stopped
|
||||
debugStatus = "STOPPED";
|
||||
debugStatus = "Nothing Collected";
|
||||
robot.collectorLeft.setPower(0);
|
||||
robot.collectorRight.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) > 60) {
|
||||
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(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 (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 (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);
|
||||
}
|
||||
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);
|
||||
@@ -150,10 +151,9 @@ public class CollectorDistanceState extends CyberarmState {
|
||||
robot.collectorLeft.setPower(0);
|
||||
setHasFinished(true);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
package org.timecrafters.Autonomous.States;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.testing.states.PrototypeBot1;
|
||||
|
||||
public class DriverParkPlaceState extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PrototypeBot1 robot;
|
||||
private int RampUpDistance;
|
||||
private int RampDownDistance;
|
||||
private String intendedPlacement;
|
||||
|
||||
public DriverParkPlaceState(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();
|
||||
this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
private double drivePower, targetDrivePower;
|
||||
private int traveledDistance;
|
||||
|
||||
@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_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;
|
||||
}
|
||||
String placement = engine.blackboard.get("parkPlace");
|
||||
if (placement != null) {
|
||||
if (!placement.equals(intendedPlacement)){
|
||||
setHasFinished(true);
|
||||
}
|
||||
if (placement.equals(intendedPlacement)) {
|
||||
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.25;
|
||||
} else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
|
||||
// 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;
|
||||
}
|
||||
|
||||
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.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
setHasFinished(true);
|
||||
} // else ending
|
||||
} // placement equals if statement
|
||||
// setHasFinished(true);
|
||||
} // end of placement doesn't equal null
|
||||
|
||||
} // end of exec
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Position", intendedPlacement);
|
||||
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);
|
||||
|
||||
} // end of telemetry
|
||||
} // end of class
|
||||
@@ -17,17 +17,7 @@ public class PathDecision extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
String placement = engine.blackboard.get("parkPlace");
|
||||
|
||||
if (placement != null) {
|
||||
if (placement.equals("2")) {
|
||||
engine.insertState(this, new DriverState(robot, groupName, "29-1"));
|
||||
} else if (placement.equals("3")) {
|
||||
engine.insertState(this, new DriverState(robot, groupName, "29-2"));
|
||||
}
|
||||
}
|
||||
else {
|
||||
engine.insertState(this, new DriverState(robot, groupName, "29-0"));
|
||||
}
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -8,8 +8,9 @@ public class RotationState extends CyberarmState {
|
||||
PrototypeBot1 robot;
|
||||
public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
|
||||
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
|
||||
this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
|
||||
drivePowerVariable = drivePower;
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
|
||||
|
||||
@@ -18,46 +19,41 @@ public class RotationState extends CyberarmState {
|
||||
private double drivePower;
|
||||
private float targetRotation;
|
||||
float RobotRotation;
|
||||
private double RotationTarget, DeltaRotation;
|
||||
private double RotationTarget;
|
||||
private double RotationDirectionMinimum;
|
||||
private String debugStatus = "?";
|
||||
private double drivePowerVariable;
|
||||
|
||||
|
||||
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() {
|
||||
if (stateDisabled){
|
||||
setHasFinished(true);
|
||||
return;
|
||||
}
|
||||
} // end of if
|
||||
|
||||
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
|
||||
RotationTarget = targetRotation;
|
||||
CalculateDeltaRotation();
|
||||
if (drivePower < 0){
|
||||
RotationDirectionMinimum = -0.3;
|
||||
} else {
|
||||
RotationDirectionMinimum = 0.3;
|
||||
|
||||
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
|
||||
drivePowerVariable = 0.3 * drivePower;
|
||||
debugStatus = "Rotate Slow";
|
||||
} // end of if
|
||||
else {
|
||||
drivePowerVariable = drivePower * 0.75;
|
||||
debugStatus = "Rotate Fast";
|
||||
}
|
||||
drivePower = (drivePower * DeltaRotation/180) + RotationDirectionMinimum;
|
||||
if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
if (RobotRotation >= targetRotation + 1){
|
||||
drivePowerVariable = Math.abs(drivePowerVariable);
|
||||
} else {
|
||||
drivePowerVariable = -1 * Math.abs(drivePowerVariable);
|
||||
}
|
||||
|
||||
if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) {
|
||||
robot.backLeftDrive.setPower(drivePowerVariable);
|
||||
robot.backRightDrive.setPower(-drivePowerVariable);
|
||||
robot.frontLeftDrive.setPower(drivePowerVariable);
|
||||
robot.frontRightDrive.setPower(-drivePowerVariable);
|
||||
} else
|
||||
{
|
||||
robot.backLeftDrive.setPower(0);
|
||||
@@ -71,9 +67,13 @@ public class RotationState extends CyberarmState {
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("DEBUG Status", debugStatus);
|
||||
|
||||
engine.telemetry.addLine();
|
||||
|
||||
engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
|
||||
engine.telemetry.addData("Robot Target Rotation", targetRotation);
|
||||
engine.telemetry.addData("Drive Power", drivePower);
|
||||
engine.telemetry.addData("Drive Power", drivePowerVariable);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user