diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java new file mode 100644 index 0000000..da091ba --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java @@ -0,0 +1,55 @@ +package org.timecrafters.CenterStage.Autonomous.CompetitionStates; + +import com.arcrobotics.ftclib.controller.PIDController; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class DriveToCoordinatesState extends CyberarmState { + + CompetitionRobotV1 robot; + public double xTarget; + public double yTarget; + public double hTarget; + public boolean posAchieved = false; + + public DriveToCoordinatesState(CompetitionRobotV1 robot/*, String groupName, String actionName*/) { + this.robot = robot; +// this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); +// this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); +// this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); + } + + @Override + public void exec() { + robot.hTarget = hTarget; + robot.yTarget = yTarget; + robot.xTarget = xTarget; + + if (posAchieved){ +// setHasFinished(true); + } else { + robot.OdometryLocalizer(); + if (((robot.positionX > xTarget - 1) && (robot.positionX < xTarget + 1)) && + ((robot.positionH > hTarget - 1) && (robot.positionH < hTarget + 1)) && + ((robot.positionY > yTarget - 1) && (robot.positionY < yTarget + 1))){ + posAchieved = true; + } + } + } + + + @Override + public void telemetry() { + engine.telemetry.addData("x pos", robot.positionX); + engine.telemetry.addData("y pos", robot.positionY); + engine.telemetry.addData("h pos odo", robot.positionH); + engine.telemetry.addData("aux encoder", robot.odometerA.getCurrentPosition()); + engine.telemetry.addData("left encoder", robot.odometerL.getCurrentPosition()); + engine.telemetry.addData("right encoder", robot.odometerR.getCurrentPosition()); + engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java new file mode 100644 index 0000000..53e28be --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java @@ -0,0 +1,23 @@ +package org.timecrafters.CenterStage.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState; +import org.timecrafters.CenterStage.Common.CompetitionRobotV1; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@Autonomous(name = "odo test and tune") + +public class OdometryTestEngine extends CyberarmEngine { + + CompetitionRobotV1 robot; + @Override + public void setup() { + this.robot = new CompetitionRobotV1("Autonomous odometry test"); + this.robot.setup(); + + addState(new DriveToCoordinatesState(robot/*,"odoTest", "00-1"*/)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java index c147779..9964f7f 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; //import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.timecrafters.CenterStage.Common.ProtoBotSodi; -import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi; +import org.timecrafters.CenterStage.Autonomous.SodiStates.ProtoBotStateSodi; import dev.cyberarm.engine.V2.CyberarmEngine; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java index 9fd1f9e..49ef095 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java @@ -1,13 +1,9 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import org.timecrafters.CenterStage.Autonomous.States.AutoStateScrimmage; -import org.timecrafters.CenterStage.Autonomous.States.DepositorArmPosState; +import org.timecrafters.CenterStage.Autonomous.SodiStates.AutoStateScrimmage; import org.timecrafters.CenterStage.Common.PrototypeRobot; -import org.timecrafters.CenterStage.Common.ServoTestRobot; -import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState; import dev.cyberarm.engine.V2.CyberarmEngine; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java index 2c05058..b7debf1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java @@ -2,14 +2,10 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoArmState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoFirstDriveState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoSecDriveState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaAutoTurnState; -import org.timecrafters.CenterStage.Autonomous.States.SodiPizzaWheelTest; +import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoFirstDriveState; +import org.timecrafters.CenterStage.Autonomous.SodiStates.SodiPizzaAutoTurnState; import dev.cyberarm.engine.V2.CyberarmEngine; -import dev.cyberarm.engine.V2.CyberarmState; @Autonomous(name = "Sodi's Pizza Box Bot Auto", group = "") public class SodiPizzaAutoRedRightEngine extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java similarity index 90% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java index bea0a49..21ddbf1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java @@ -1,11 +1,8 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; /**BEGAN WITH 43 LINES**/ -import com.arcrobotics.ftclib.hardware.motors.Motor; - import org.timecrafters.CenterStage.Common.ProtoBotSodi; -import org.timecrafters.CenterStage.Common.PrototypeRobot; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java similarity index 91% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java index 1be2687..501ea3d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.timecrafters.CenterStage.Common.PrototypeRobot; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java similarity index 98% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java index d0ebc50..2f8b426 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import com.qualcomm.robotcore.hardware.DcMotor; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java similarity index 99% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java index ca8688c..6ebfecf 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java @@ -1,6 +1,5 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; -import org.timecrafters.CenterStage.Common.PrototypeRobot; import org.timecrafters.CenterStage.Common.ServoTestRobot; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java similarity index 72% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java index a7e909b..4263946 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java similarity index 93% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java index 5384597..d156607 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java @@ -1,9 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; - -import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.Servo; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import dev.cyberarm.engine.V2.CyberarmState; import org.timecrafters.CenterStage.Common.ProtoBotSodi; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java similarity index 88% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java index f9651a3..5f2c6a2 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java similarity index 97% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java index 87556d7..7ebce28 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import android.annotation.SuppressLint; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java similarity index 90% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java index 453d17c..e5b7d8a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java similarity index 95% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java index 0823dc9..f3fe252 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java @@ -1,10 +1,8 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject; -import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmState; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java similarity index 98% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java index 87bcf10..bd816cb 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java @@ -1,4 +1,4 @@ -package org.timecrafters.CenterStage.Autonomous.States; +package org.timecrafters.CenterStage.Autonomous.SodiStates; import android.annotation.SuppressLint; diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java deleted file mode 100644 index 5eb7c00..0000000 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.timecrafters.CenterStage.Autonomous.States; - -import com.arcrobotics.ftclib.controller.PIDController; - -import org.timecrafters.CenterStage.Common.PrototypeRobot; - -import dev.cyberarm.engine.V2.CyberarmState; - -public class DriveToCoordinatesState extends CyberarmState { - - private final boolean stateDisabled; - PrototypeRobot robot; - private PIDController driveXPidController; - private PIDController driveYPidController; - private PIDController driveHPidController; - private double pidX; - private double pidY; - private double pidH; - - public double xTarget; - public double yTarget; - public double hTarget; - - public double currentVelocityX; - public double currentVelocityY; - public double currentVelocityH; - - public double targetVelocityFL; - public double targetVelocityBL; - public double targetVelocityFR; - public double targetVelocityBR; - - - public static double Xp = 0, Xi = 0, Xd = 0; // p = 0.02 i = 0.09 d = 0 - public static double Yp = 0, Yi = 0, Yd = 0;// p = 0.03 i = 0.07 d = 0 - public static double Hp = 0, Hi = 0, Hd = 0;// p = 0.03 i = 0.07 d = 0 - - public DriveToCoordinatesState(PrototypeRobot robot, String groupName, String actionName) { - this.robot = robot; - driveXPidController = new PIDController(Xp, Xi, Xd); - driveYPidController = new PIDController(-Yp, -Yi, -Yd); - driveHPidController = new PIDController(-Hp, -Hi, -Hd); - this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; - this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value(); - this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value(); - this.hTarget = robot.configuration.variable(groupName, actionName, "hTarget").value(); - } - - @Override - public void start() { - //add variables that need to be reinitillized - } - - @Override - public void exec() { - // PID functions to come up with the velocity to get to the final point. - driveXPidController.setPID(Xp, Xi, Xd); - pidX = driveXPidController.calculate(robot.positionX, xTarget); - driveYPidController.setPID(Yp, Yi, Yd); - pidY = (driveYPidController.calculate(robot.positionY, yTarget)); - driveHPidController.setPID(Hp, Hi, Hd); - pidH = (driveYPidController.calculate(robot.positionH, hTarget)); - - // PID robot Velocity Ratios - currentVelocityX = robot.MaxVelocityForward * pidX; - currentVelocityY = robot.MaxStrafeVelocity * pidY; - currentVelocityH = robot.MaxRotationalVelocity * pidH; - - // Kinematic Formulas with plugged in velocities to solve for wheel velocities. - targetVelocityFL = currentVelocityX - currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - targetVelocityBL = currentVelocityX + currentVelocityY - ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - targetVelocityBR = currentVelocityX - currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - targetVelocityFR = currentVelocityX + currentVelocityY + ((2 * (robot.L / 2) * robot.MaxRotationalVelocity)); - -// robot.frontLeft.setVelocity((targetVelocityFL / robot.R)); -// robot.backLeft.setVelocity((targetVelocityBL / robot.R)); -// robot.backRight.setVelocity((targetVelocityBR / robot.R)); -// robot.frontRight.setVelocity((targetVelocityFR / robot.R)); - -// setHasFinished(true) - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java new file mode 100644 index 0000000..39a2a59 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java @@ -0,0 +1,214 @@ +package org.timecrafters.CenterStage.Common; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + + +public class CompetitionRobotV1 extends Robot { + // --------------------------------------------------------------------------------------------------- engine and state setup variables: + private String string; + private CyberarmEngine engine; + + public TimeCraftersConfiguration configuration; + + // HardwareMap setup + + public DcMotor frontLeft, frontRight, backLeft, backRight, lift; + public DcMotor odometerR, odometerL, odometerA; + + public IMU imu; + public Servo shoulder, elbow, leftClaw, rightClaw; + + // ---------------------------------------------------------------------------------------------------- odometry variables: + public static double Hp = 0, Hi = 0, Hd = 0; + public static double Xp = 0, Xi = 0, Xd = 0; + public static double Yp = 0, Yi = 0, Yd = 0; + private double drivePower = 1; + + public double xMultiplier = 1; + public double yMultiplier = 1; + public double positionX = 0; + public double positionY = 0; + public double positionH = 0; + public double xTarget; + public double yTarget; + public double hTarget; + + public int currentRightPosition = 0; + public int currentLeftPosition = 0; + public int currentAuxPosition = 0; + public int oldRightPosition = 0; + public int oldLeftPosition = 0; + public int oldAuxPosition = 0; + public double rx; + public final static double L = 23.425; // distance between left and right encoder in cm + final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm + public final static double R = 4.5; // wheel radius in cm + final static double N = 8192; // encoder ticks per revolution (REV encoder) + + // heading math variables for pid with imu + public double integralSum = 0; + public double targetHeading; + public final double cm_per_tick = (2 * Math.PI * R) / N; + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); + + // ---------------------------------------------------------------------------------------------------- collector / depositor variables: + + public float depositorPos; + public float collectorPos; + public boolean lbsVar2; + public boolean rbsVar2; + + //-------------------------------------------------------------------------------------------------------------- arm sequence variables: + public int armPosition = 0; + public long startOfSequencerTime; + public int oldArmPosition = 0; + public float DEPOSITOR_SHOULDER_IN; + public float DEPOSITOR_SHOULDER_OUT; + public float DEPOSITOR_ELBOW_IN; + public float DEPOSITOR_ELBOW_OUT; + public float COLLECTOR_SHOULDER_IN; + public float COLLECTOR_SHOULDER_PASSIVE; + public float COLLECTOR_SHOULDER_OUT; + public float COLLECTOR_ELBOW_IN; + public float COLLECTOR_ELBOW_PASSIVE; + public float COLLECTOR_ELBOW_OUT; + private HardwareMap hardwareMap; + + + + public CompetitionRobotV1(String string) { + this.engine = engine; + setup(); + this.string = string; + } + + public void initConstants() { + DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value(); + DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value(); + DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value(); + DEPOSITOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_OUT").value(); + COLLECTOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_IN").value(); + COLLECTOR_SHOULDER_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_PASSIVE").value(); + COLLECTOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_SHOULDER_OUT").value(); + COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value(); + COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value(); + COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value(); + } + + @Override + public void setup() { + System.out.println("Bacon: " + this.string); + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + //----------------------------------------------------------------------------------------------------------------------------MOTORS + frontRight = engine.hardwareMap.dcMotor.get("frontRight"); + frontLeft = engine.hardwareMap.dcMotor.get("frontLeft"); + backRight = engine.hardwareMap.dcMotor.get("backRight"); + backLeft = engine.hardwareMap.dcMotor.get("backLeft"); + lift = engine.hardwareMap.dcMotor.get("Lift"); + odometerL = engine.hardwareMap.dcMotor.get("leftodo"); + odometerR = engine.hardwareMap.dcMotor.get("rightodo"); + odometerA = engine.hardwareMap.dcMotor.get("auxodo"); + + frontRight.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + lift.setDirection(DcMotorSimple.Direction.FORWARD); + odometerR.setDirection(DcMotorSimple.Direction.FORWARD); + odometerL.setDirection(DcMotorSimple.Direction.REVERSE); + odometerA.setDirection(DcMotorSimple.Direction.FORWARD); + + odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + configuration = new TimeCraftersConfiguration("Blue Crab"); + + initConstants(); + + + //-----------------------------------------------------------------------------------------------------------------------------SERVO + shoulder = hardwareMap.servo.get("shoulder"); + elbow = hardwareMap.servo.get("elbow"); + leftClaw = hardwareMap.servo.get("leftClaw"); + rightClaw = hardwareMap.servo.get("leftClaw"); + + + + } + + public void CollectorToggle() { + boolean lbs2 = engine.gamepad2.left_stick_button; + if (lbs2 && !lbsVar2) { + if (collectorPos == 1F) { + collectorPos = 0F; + } else { + collectorPos = 1F; + } + } + lbsVar2 = lbs2; + } + + + public void DepositorToggle() { + boolean rbs2 = engine.gamepad2.right_stick_button; + if (rbs2 && !rbsVar2) { + if (depositorPos == 0.6F) { + depositorPos = 0F; + } else { + depositorPos = 0.6F; + } + } + rbsVar2 = rbs2; + } + + public void OdometryLocalizer() { + + // update positions + oldRightPosition = currentRightPosition; + oldLeftPosition = currentLeftPosition; + oldAuxPosition = currentAuxPosition; + + currentRightPosition = -odometerR.getCurrentPosition(); + currentLeftPosition = -odometerL.getCurrentPosition(); + currentAuxPosition = odometerA.getCurrentPosition(); + + int dnl1 = currentLeftPosition - oldLeftPosition; + int dnr2 = currentRightPosition - oldRightPosition; + int dna3 = currentAuxPosition - oldAuxPosition; + + // the robot has turned and moved a tiny bit between two measurements + double dtheta = cm_per_tick * (dnr2 - dnl1) / L; + double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; + double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); + + // the small movement of the bot gets added to the field coordinates + double theta = positionH + (dtheta / 2.0); + positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; + positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; + positionH += dtheta; + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index 9ccf95e..717e60e 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.Common; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.drivebase.HDrive; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -10,32 +11,21 @@ import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.Library.Robot; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmEngine; - +@Config public class PrototypeRobot extends Robot { - -// public double integralSum = 0; -// double Kp = 0; -// double Ki = 0; -// double Kd = 0; -// public double lastError = 0; - public double PIDrx; - public double targetHeading; public boolean headingLock = false; + public double collectLock = Math.toRadians(-90); public double backDropLock = Math.toRadians(90); - ElapsedTime timer = new ElapsedTime(); public int armPosition = 0; - public boolean stateFinished; public long startOfSequencerTime; public int oldArmPosition = 0; - public long waitTime; - public double servoWaitTime; - public double servoSecPerDeg = 0.14/60; public float DEPOSITOR_SHOULDER_IN; public float DEPOSITOR_SHOULDER_OUT; public float DEPOSITOR_ELBOW_IN; @@ -46,56 +36,56 @@ public class PrototypeRobot extends Robot { public float COLLECTOR_ELBOW_IN; public float COLLECTOR_ELBOW_PASSIVE; public float COLLECTOR_ELBOW_OUT; - public float lastSetPosShoulder; - public float lastSetPosElbow; - public float currentSetPosShoulder; - public float currentSetPosElbow; + + // hardware map private HardwareMap hardwareMap; public DcMotor frontLeft, frontRight, backLeft, backRight, lift; public DcMotor odometerR, odometerL, odometerA; public IMU imu; public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow, depositor, collector; - private HDrive xDrive; private String string; + + // robot geometry constants for odometry ----------------------------------------------------------------------------------------------- + public static double Hp = 0, Hi = 0, Hd = 0; + public static double Xp = 0, Xi = 0, Xd = 0; + public static double Yp = 0, Yi = 0, Yd = 0; private double drivePower = 1; public double xMultiplier = 1; public double yMultiplier = 1; - public double positionX; - public double positionY; - public double positionH; + public double positionX = 0; + public double positionY = 0; + public double positionH = 0; + public double xTarget; + public double yTarget; + public double hTarget; - // robot geometry constants for odometry ----------------------------------------------------------------------------------------------- public int currentRightPosition = 0; public int currentLeftPosition = 0; public int currentAuxPosition = 0; public int oldRightPosition = 0; public int oldLeftPosition = 0; public int oldAuxPosition = 0; - public double globalPositionX; - public double globalPositionY; - public double globalPositionH; - public double localPositionX; - public double localPositionY; - public double localPositionH; - + public double rx; public final static double L = 23.425; // distance between left and right encoder in cm final static double B = 10; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm public final static double R = 4.5; // wheel radius in cm final static double N = 8192; // encoder ticks per revolution (REV encoder) - public final double MaxVelocityForward = 40; - public final double MaxStrafeVelocity = 34; - public final double MaxRotationalVelocity = 20; + // heading math variables for pid with imu + public double integralSum = 0; + public double targetHeading; + + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); + + + // collector / depositor variables private boolean lbsVar1; private boolean lbsVar2; private boolean rbsVar2; public float depositorPos; public float collectorPos; - public double rx; - - - public final double cm_per_tick = (2 * Math.PI * R) / N; private CyberarmEngine engine; @@ -108,7 +98,7 @@ public class PrototypeRobot extends Robot { this.string = string; } - public void initConstants(){ + public void initConstants() { DEPOSITOR_SHOULDER_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_IN").value(); DEPOSITOR_SHOULDER_OUT = configuration.variable("Robot", "Tuning", "DEPOSITOR_SHOULDER_OUT").value(); DEPOSITOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "DEPOSITOR_ELBOW_IN").value(); @@ -119,7 +109,7 @@ public class PrototypeRobot extends Robot { COLLECTOR_ELBOW_IN = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_IN").value(); COLLECTOR_ELBOW_PASSIVE = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_PASSIVE").value(); COLLECTOR_ELBOW_OUT = configuration.variable("Robot", "Tuning", "COLLECTOR_ELBOW_OUT").value(); - } + } @Override public void setup() { @@ -135,7 +125,9 @@ public class PrototypeRobot extends Robot { backRight = engine.hardwareMap.dcMotor.get("backRight"); backLeft = engine.hardwareMap.dcMotor.get("backLeft"); lift = engine.hardwareMap.dcMotor.get("Lift"); - odometerR = engine.hardwareMap.dcMotor.get("odometerR"); + odometerL = engine.hardwareMap.dcMotor.get("leftodo"); + odometerR = engine.hardwareMap.dcMotor.get("rightodo"); + odometerA = engine.hardwareMap.dcMotor.get("auxodo"); configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -146,8 +138,14 @@ public class PrototypeRobot extends Robot { backLeft.setDirection(DcMotorSimple.Direction.REVERSE); frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); lift.setDirection(DcMotorSimple.Direction.FORWARD); + odometerR.setDirection(DcMotorSimple.Direction.FORWARD); + odometerL.setDirection(DcMotorSimple.Direction.REVERSE); + odometerA.setDirection(DcMotorSimple.Direction.FORWARD); + odometerR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + odometerA.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -166,8 +164,6 @@ public class PrototypeRobot extends Robot { //SERVO depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); depositorElbow = hardwareMap.servo.get("depositor_elbow"); - collectorShoulder = hardwareMap.servo.get("collector_shoulder"); - collectorElbow = hardwareMap.servo.get("collector_elbow"); depositor = hardwareMap.servo.get("depositor"); collector = hardwareMap.servo.get("collector"); @@ -176,305 +172,166 @@ public class PrototypeRobot extends Robot { collectorElbow.setPosition(COLLECTOR_ELBOW_IN); collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); collector.setPosition(0F); - - - } - public void driveTrainTeleOp() { - boolean lbs1 = engine.gamepad1.left_stick_button; - if (lbs1 && !lbsVar1) { - if (drivePower == 1) { - drivePower = 0.5; - } else { - drivePower = 1; - } - } - lbsVar1 = lbs1; - - double y = -engine.gamepad1.left_stick_y; - double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing - - if (headingLock){ - ; - } else if (headingLock == false){ - rx = engine.gamepad1.right_stick_x; - } - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - - // angle math to make things field oriented - double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - double rotX = x * Math.cos(heading) - y * Math.sin(heading); - double rotY = x * Math.sin(heading) + y * Math.cos(heading); - - // joystick math to find the approximate power across each wheel for a movement - double frontLeftPower = (rotY + rotX + rx) / denominator; - double backLeftPower = (rotY - rotX + rx) / denominator; - double frontRightPower = (rotY + rotX - rx) / denominator; - double backRightPower = (rotY - rotX - rx) / denominator; - - // setting each power determined previously from the math above - // as well as multiplying it by a drive power that can be changed. - backLeft.setPower(backLeftPower * drivePower); - backRight.setPower(backRightPower * drivePower); - frontLeft.setPower(frontLeftPower * drivePower); - frontRight.setPower(frontRightPower * drivePower); - } - - public void ShoulderServoWaitTime(){ - - servoWaitTime = servoSecPerDeg * (Math.abs(lastSetPosShoulder - currentSetPosShoulder)); - - } - - public void ElbowServoWaitTime(){ - - servoWaitTime = 1000 * (servoSecPerDeg * (Math.abs(lastSetPosElbow - currentSetPosElbow))); - - } - public void OdometryLocalizer(){ - - if (Math.toDegrees(positionH) > 360){ - positionH -= 360; - } - - globalPositionX = localPositionX; - globalPositionY = localPositionY; - globalPositionH = localPositionH; - - - // update positions - oldRightPosition = currentRightPosition; - oldLeftPosition = currentLeftPosition; - oldAuxPosition = currentAuxPosition; - - currentRightPosition = -odometerR.getCurrentPosition(); - currentLeftPosition = -odometerL.getCurrentPosition(); - currentAuxPosition = odometerA.getCurrentPosition(); - - int dnl1 = currentLeftPosition - oldLeftPosition; - int dnr2 = currentRightPosition - oldRightPosition; - int dna3 = currentAuxPosition - oldAuxPosition; - - // the robot has turned and moved a tiny bit between two measurements - double dtheta = cm_per_tick * (dnr2 - dnl1) / L; - double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; - double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); - - // the small movement of the bot gets added to the field coordinates - double theta = positionH + (dtheta / 2.0); - positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; - positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; - positionH += dtheta; - - } - -// public double PIDControl(double reference, double current){ -// double error = reference - current; -// integralSum += error * timer.seconds(); -// double derivative = (error - lastError) / timer.seconds(); -// -// timer.reset(); -// -// double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); -// return output; -// } - - public void CollectorToggle(){ - boolean lbs2 = engine.gamepad2.left_stick_button; - if (lbs2 && !lbsVar2) { - if (collectorPos == 1F) { - collectorPos = 0F; - } else { - collectorPos = 1F; - } - } - lbsVar2 = lbs2; - } - public void DepositorToggle(){ - boolean rbs2 = engine.gamepad2.right_stick_button; - if (rbs2 && !rbsVar2) { - if (depositorPos == 0.6F) { - depositorPos = 0F; - } else { - depositorPos = 0.6F; - } - } - rbsVar2 = rbs2; - } - public void ArmSequences(){ - switch (armPosition) { - case 0: // ----------------------------------------------------------------------------------------------- drive to transfer pos - switch (oldArmPosition) { - case 0: - // transfer - case 1: - // driving - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 0; - } - } - case 2: - // collect - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { - oldArmPosition = 0; - } - } - case 3: - // deposit - depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met - depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { - collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position - if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met - collectorElbow.setPosition(COLLECTOR_ELBOW_IN); - if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { - oldArmPosition = 0; - } - } - } - } + public void driveTrainTeleOp () { + boolean lbs1 = engine.gamepad1.left_stick_button; + if (lbs1 && !lbsVar1) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; } + } + lbsVar1 = lbs1; -// case 1:// ----------------------------------------------------------------------------------------------- drive to driving pos -// switch (oldArmPosition) { -// case 0: -// // transfer -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 1; -// } -// } -// break; -// case 1: -// // drive pos -// break; -// case 2: -// // collect -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 600) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2100) { -// oldArmPosition = 1; -// } -// } -// break; -// case 3: -// // deposit -// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 800) { // wait to move till time is met -// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1600) { -// collectorShoulder.setPosition(COLLECTOR_SHOULDER_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 2300) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 3100) { -// oldArmPosition = 1; -// } -// } -// } -// } -// break; -// } -// break; -// -// case 2:// ----------------------------------------------------------------------------------------------- drive to collect pos -// switch (oldArmPosition) { -// case 0: -// // transfer -// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 2; -// } -// } -// break; -// case 1: -// // driving -// collectorShoulder.setPosition(COLLECTOR_ELBOW_OUT); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_OUT); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { -// oldArmPosition = 2; -// } -// } -// break; -// case 2: -// // collect -// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { -// oldArmPosition = 2; -// } -// } -// break; -// case 3: -// // deposit -// depositorShoulder.setPosition(DEPOSITOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met -// depositorElbow.setPosition(DEPOSITOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_OUT); -// if (System.currentTimeMillis() - startOfSequencerTime >= 100) { -// oldArmPosition = 2; -// } -// } -// } -// } -// break; -// } -// break; + double y = -engine.gamepad1.left_stick_y; + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing -// case 3:// ----------------------------------------------------------------------------------------------- drive to deposit pos -// switch (oldArmPosition) { -// case 0: -// // transfer -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 3; -// } -// } -// break; -// case 1: -// // driving -// collectorShoulder.setPosition(COLLECTOR_ELBOW_PASSIVE); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 700) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_SHOULDER_PASSIVE); -// if (System.currentTimeMillis() - startOfSequencerTime >= 2000) { -// oldArmPosition = 3; -// } -// } -// break; -// case 2: -// // collect -// collectorShoulder.setPosition(COLLECTOR_SHOULDER_IN); // drive the shoulder to the transfer position -// if (System.currentTimeMillis() - startOfSequencerTime >= 750) { // wait to move till time is met -// collectorElbow.setPosition(COLLECTOR_ELBOW_IN); -// if (System.currentTimeMillis() - startOfSequencerTime >= 1500) { -// oldArmPosition = 3; -// } -// } -// break; -// case 3: -// // deposit -// break; -// } -// break; + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + + // angle math to make things field oriented + double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double rotX = x * Math.cos(heading) - y * Math.sin(heading); + double rotY = x * Math.sin(heading) + y * Math.cos(heading); + + // joystick math to find the approximate power across each wheel for a movement + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY + rotX - rx) / denominator; + double backRightPower = (rotY - rotX - rx) / denominator; + + // setting each power determined previously from the math above + // as well as multiplying it by a drive power that can be changed. + backLeft.setPower(backLeftPower * drivePower); + backRight.setPower(backRightPower * drivePower); + frontLeft.setPower(frontLeftPower * drivePower); + frontRight.setPower(frontRightPower * drivePower); + } + + public void OdometryLocalizer () { + + // update positions + oldRightPosition = currentRightPosition; + oldLeftPosition = currentLeftPosition; + oldAuxPosition = currentAuxPosition; + + currentRightPosition = -odometerR.getCurrentPosition(); + currentLeftPosition = -odometerL.getCurrentPosition(); + currentAuxPosition = odometerA.getCurrentPosition(); + + int dnl1 = currentLeftPosition - oldLeftPosition; + int dnr2 = currentRightPosition - oldRightPosition; + int dna3 = currentAuxPosition - oldAuxPosition; + + // the robot has turned and moved a tiny bit between two measurements + double dtheta = cm_per_tick * (dnr2 - dnl1) / L; + double dx = cm_per_tick * (dnl1 + dnr2) / 2.0; + double dy = cm_per_tick * (dna3 - (dnr2 - dnl1) * B / L); + + // the small movement of the bot gets added to the field coordinates + double theta = positionH + (dtheta / 2.0); + positionX += (dx * Math.cos(theta) - dy * Math.sin(theta)) * xMultiplier; + positionY += (dx * Math.sin(theta) + dy * Math.cos(theta)) * yMultiplier; + positionH += dtheta; } + + public void CollectorToggle () { + boolean lbs2 = engine.gamepad2.left_stick_button; + if (lbs2 && !lbsVar2) { + if (collectorPos == 1F) { + collectorPos = 0F; + } else { + collectorPos = 1F; + } + } + lbsVar2 = lbs2; + } + + public void DepositorToggle () { + boolean rbs2 = engine.gamepad2.right_stick_button; + if (rbs2 && !rbsVar2) { + if (depositorPos == 0.6F) { + depositorPos = 0F; + } else { + depositorPos = 0.6F; + } + } + rbsVar2 = rbs2; + } + + public double angleWrap ( double radians){ + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI) { + radians += 2 * Math.PI; + } + return radians; + } + + public double HeadingPIDControl ( double reference, double current){ + double error = angleWrap(current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Hp) + (derivative * Hd) + (integralSum * Hi); + return output; + } + + public double XPIDControl ( double reference, double current){ + double error = (current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Xp) + (derivative * Xd) + (integralSum * Xi); + return output; + } + + public double YPIDControl ( double reference, double current){ + double error = (current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Yp) + (derivative * Yd) + (integralSum * Yi); + return output; + } + + public void DriveToCoordinates () { + + // determine the velocities needed for each direction + // this uses PID to adjust needed Power for robot to move to target + double heading = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double rx = HeadingPIDControl(Math.toRadians(hTarget), heading); + double pidX = XPIDControl(xTarget, positionX); + double pidY = YPIDControl(yTarget, positionY); + + double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1); + + // field oriented math, (rotating the global field to the relative field) + double rotY = pidY * Math.cos(heading) - pidX * Math.sin(heading); + double rotX = pidY * Math.sin(heading) + pidX * Math.cos(heading); + + + // finding approximate power for each wheel. + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY + rotX - rx) / denominator; + double backRightPower = (rotY - rotX - rx) / denominator; + + // apply my powers + frontLeft.setPower(frontLeftPower); + backLeft.setPower(backLeftPower); + backRight.setPower(backRightPower); + frontRight.setPower(frontRightPower); + } } -} \ No newline at end of file + + diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java new file mode 100644 index 0000000..e473faa --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java @@ -0,0 +1,168 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +@Config + +public class CompetitionTeleOpState extends CyberarmState { + private PrototypeRobot robot; + private int maxExtension = 2000; + private int minExtension = 0; + public double integralSum = 0; + private double targetHeading; + + public double power; + private double currentHeading; + private boolean headingLock = false; + + public static double Kp = 0; + public static double Ki = 0; + public static double Kd = 0; + private double lastError = 0; + ElapsedTime timer = new ElapsedTime(); + + + private long lastCheckedTime; + + public CompetitionTeleOpState(PrototypeRobot robot) { + this.robot = robot; + + } + + // } + // --------------------------------------------------------------------------------------------------------- Slider control function + private void SliderTeleOp() { + if (engine.gamepad2.right_trigger != 0) { + if (robot.lift.getCurrentPosition() >= maxExtension) { + robot.lift.setPower(0); + } else if (robot.lift.getCurrentPosition() >= maxExtension - 200) { + robot.lift.setPower(0.35); + } else { + robot.lift.setPower(engine.gamepad2.right_trigger); + } + } else if (engine.gamepad2.left_trigger != 0) { + + if (robot.lift.getCurrentPosition() <= minExtension) { + robot.lift.setPower(0); + } else if (robot.lift.getCurrentPosition() < 350) { + robot.lift.setPower(-0.3); + } else { + robot.lift.setPower(-engine.gamepad2.left_trigger); + } + } else { + robot.lift.setPower(0); + } + } + public double angleWrap(double radians) { + while (radians > Math.PI) { + radians -= 2 * Math.PI; + } + while (radians < -Math.PI){ + radians += 2 * Math.PI; + } + return radians; + } + + public double HeadingPIDControl(double reference, double current){ + double error = angleWrap(current - reference); + integralSum += error * timer.seconds(); + double derivative = (error - lastError) / timer.seconds(); + + timer.reset(); + + double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); + return output; + } + + @Override + public void init() { + } + + @Override + public void exec() { + + currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + if (engine.gamepad1.right_stick_button) { + robot.imu.resetYaw(); + } + + if (headingLock){ + robot.rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + robot.rx = engine.gamepad1.right_stick_x; + + } + + // drivetrain + robot.driveTrainTeleOp(); + + if (engine.gamepad1.b){ + headingLock = true; + targetHeading = robot.backDropLock; + } + if (engine.gamepad1.x){ + headingLock = true; + targetHeading = robot.collectLock; + } + if (engine.gamepad1.a){ + headingLock = true; + targetHeading = currentHeading; + } + if (engine.gamepad1.right_stick_x != 0){ + headingLock = false; + } + +// if (engine.gamepad2.a) { +// robot.armPosition = 0; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } else if (engine.gamepad2.x) { +// robot.armPosition = 1; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } else if (engine.gamepad2.b) { +// robot.armPosition = 2; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } else if (engine.gamepad2.y) { +// robot.armPosition = 3; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } +// +// robot.depositor.setPosition(robot.depositorPos); +// robot.collector.setPosition(robot.collectorPos); +// +// +// // drivetrain +// robot.driveTrainTeleOp(); + // lift +// SliderTeleOp(); + // collector depositor +// robot.CollectorToggle(); + // depositor toggle +// robot.DepositorToggle(); + + + } + + @Override + public void telemetry () { + engine.telemetry.addData("Lift Encoder Pos", robot.lift.getCurrentPosition()); + engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); + engine.telemetry.addData("arm Pos", robot.armPosition); + engine.telemetry.addData("old arm pos", robot.oldArmPosition); + engine.telemetry.addData("depositor pos", robot.depositorPos); + engine.telemetry.addData("collector pos", robot.collectorPos); + engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); + engine.telemetry.addData("pid power", power); + engine.telemetry.addData("heading Lock?", headingLock); + engine.telemetry.addData("Kp", Kp); + engine.telemetry.addData("Ki", Ki); + engine.telemetry.addData("Kd", Kd); + } + } + diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index 326b411..a8fc2d6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.TeleOp.States; +import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.robotcore.util.ElapsedTime; @@ -7,18 +8,26 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.CenterStage.Common.PrototypeRobot; import dev.cyberarm.engine.V2.CyberarmState; +@Config public class PrototypeRobotDrivetrainState extends CyberarmState { private PrototypeRobot robot; private int maxExtension = 2000; private int minExtension = 0; public double integralSum = 0; - double Kp = 0; - double Ki = 0; - double Kd = 0; + private double targetHeading; + private double lastError = 0; ElapsedTime timer = new ElapsedTime(); + public double power; + private double currentHeading; + private boolean headingLock = false; + + public static double Kp = 0; + public static double Ki = 0; + public static double Kd = 0; + private long lastCheckedTime; @@ -61,12 +70,8 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { return radians; } - private void HeadingLock(){ - double currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - - } public double HeadingPIDControl(double reference, double current){ - double error = angleWrap(reference - current); + double error = angleWrap(current - reference); integralSum += error * timer.seconds(); double derivative = (error - lastError) / timer.seconds(); @@ -83,36 +88,64 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { @Override public void exec() { + currentHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + if (engine.gamepad1.right_stick_button) { robot.imu.resetYaw(); } - if (engine.gamepad2.a) { - robot.armPosition = 0; - robot.startOfSequencerTime = System.currentTimeMillis(); - } else if (engine.gamepad2.x) { - robot.armPosition = 1; - robot.startOfSequencerTime = System.currentTimeMillis(); - } else if (engine.gamepad2.b) { - robot.armPosition = 2; - robot.startOfSequencerTime = System.currentTimeMillis(); - } else if (engine.gamepad2.y) { - robot.armPosition = 3; - robot.startOfSequencerTime = System.currentTimeMillis(); + if (headingLock){ + robot.rx = HeadingPIDControl(targetHeading, currentHeading); + } else { + robot.rx = engine.gamepad1.right_stick_x; + } - robot.depositor.setPosition(robot.depositorPos); - robot.collector.setPosition(robot.collectorPos); + // drivetrain + robot.driveTrainTeleOp(); + if (engine.gamepad1.b){ + headingLock = true; + targetHeading = robot.backDropLock; + } + if (engine.gamepad1.x){ + headingLock = true; + targetHeading = robot.collectLock; + } + if (engine.gamepad1.a){ + headingLock = true; + targetHeading = currentHeading; + } + if (engine.gamepad1.right_stick_x != 0){ + headingLock = false; + } - // drivetrain - robot.driveTrainTeleOp(); +// if (engine.gamepad2.a) { +// robot.armPosition = 0; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } else if (engine.gamepad2.x) { +// robot.armPosition = 1; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } else if (engine.gamepad2.b) { +// robot.armPosition = 2; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } else if (engine.gamepad2.y) { +// robot.armPosition = 3; +// robot.startOfSequencerTime = System.currentTimeMillis(); +// } +// +// robot.depositor.setPosition(robot.depositorPos); +// robot.collector.setPosition(robot.collectorPos); +// +// +// // drivetrain +// robot.driveTrainTeleOp(); // lift - SliderTeleOp(); +// SliderTeleOp(); // collector depositor - robot.CollectorToggle(); +// robot.CollectorToggle(); // depositor toggle - robot.DepositorToggle(); +// robot.DepositorToggle(); } @@ -126,6 +159,14 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { engine.telemetry.addData("depositor pos", robot.depositorPos); engine.telemetry.addData("collector pos", robot.collectorPos); engine.telemetry.addData("time", System.currentTimeMillis() - robot.startOfSequencerTime); + engine.telemetry.addData("heading Lock?", headingLock); + engine.telemetry.addData("Kp", Kp); + engine.telemetry.addData("Ki", Ki); + engine.telemetry.addData("Kd", Kd); + engine.telemetry.addData("front left motor", robot.frontLeft.getCurrentPosition()); + engine.telemetry.addData("front right motor", robot.frontRight.getCurrentPosition()); + engine.telemetry.addData("back left motor", robot.backLeft.getCurrentPosition()); + engine.telemetry.addData("back right motor", robot.backRight.getCurrentPosition()); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java index ffa943a..ea162d6 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java @@ -33,13 +33,10 @@ public class XDrivetrainRobotState extends CyberarmState { private double ArmlastError = 0; private boolean headingLock = false; - private long lastCheckedTime; - public XDrivetrainRobotState(XDrivetrainBot robot) { this.robot = robot; } - // } // --------------------------------------------------------------------------------------------------------- Slider control function public double angleWrap(double radians) { @@ -149,7 +146,7 @@ public class XDrivetrainRobotState extends CyberarmState { engine.telemetry.addData("heading lock?", headingLock); engine.telemetry.addData("pid heading power", HeadingPIDControl(targetHeading, currentHeading)); engine.telemetry.addData("robot arm current pos ", robot.armMotor.getCurrentPosition()); - engine.telemetry.addData("arm pid power ", power); + engine.telemetry.addData("arm pid power ", HeadingPIDControl(targetHeading, currentHeading)); engine.telemetry.addData("p", ArmP); engine.telemetry.addData("i", ArmI); engine.telemetry.addData("d", ArmD); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java index 9cae281..610de49 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java @@ -54,7 +54,7 @@ public class headingLockTeleOp extends CyberarmState { timer.reset(); double output = (error * Kp) + (derivative * Kd) + (integralSum * Ki); - robot.PIDrx = output; +// robot.PIDrx = output; } // drivetrain @@ -82,7 +82,7 @@ public class headingLockTeleOp extends CyberarmState { public void telemetry () { engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); engine.telemetry.addData("rx", robot.rx); - engine.telemetry.addData("PIDrx", robot.PIDrx); +// engine.telemetry.addData("PIDrx", robot.PIDrx); } }