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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
NerdyBirdy460
2023-12-04 21:32:06 -06:00
parent db49950f30 208d7c1513
commit f2ce235e3b
28 changed files with 1226 additions and 486 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
TeamCode/src/main/java/dev/cyberarm/engine/V2/Utilities.java Normal file
View File

@@ -0,0 +1,86 @@
package dev.cyberarm.engine.V2;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
public class Utilities {
/***
* The current heading of the robot as a full 360 degree value, instead of that half radian mess.
* @param imu IMU of rev hub or similar
* @param imuAngleOffset optional angle offset added to IMU value
* @return full range heading of the robot, in DEGREES.
*/
static public double facing(IMU imu, double imuAngleOffset) {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
}
static public double facing(IMU imu) {
return facing(imu, 0);
}
static public double heading(double facing) {
return AngleUnit.normalizeRadians(-facing * Math.PI / 180.0);
}
static public double turnRate(IMU imu) {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate;
}
/***
*
* @param value value to check with
* @param min minimum value
* @param max maximum value
* @return true if value is between min and max. inclusive.
*/
static public boolean isBetween(double value, double min, double max) {
return value >= min && value <= max;
}
static public boolean isBetween(int value, int min, int max) {
return value >= min && value <= max;
}
// Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
/***
* The angular difference between two angles
* **NOTE** flip flopped from and to values may result in continuous inversion of the angle difference (180 to -180 for example)
* @param from angle in DEGREES
* @param to angle in DEGREES
* @return Angular difference two angles in DEGREES
*/
static public double angleDiff(double from, double to) {
double value = (to - from + 180);
double fmod = (value - 0.0) % (360.0 - 0.0);
return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180;
}
/***
* Linear interpolation
* @param min minimum value
* @param max maximum value
* @param t factor
* @return
*/
static public double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
/***
* Calculates motor angle in ticks
* @param motorTicksPerRevolution
* @param gearRatio
* @param angleInDegrees
* @return Angle in motor ticks
*/
static public int motorAngle(int motorTicksPerRevolution, double gearRatio, double angleInDegrees) {
return (int) (angleInDegrees / (360.0 / (motorTicksPerRevolution * gearRatio)));
}
}

202
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java Normal file
View File

@@ -0,0 +1,202 @@
package dev.cyberarm.minibots.red_crab;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import dev.cyberarm.engine.V2.CyberarmEngine;
public class RedCrabMinibot {
/// CLAW ARM ///
public static final int ClawArm_STOW = 0;
public static final int ClawArm_DEPOSIT = 1;
public static final int ClawArm_COLLECT = 2;
/// TUNING CONSTANTS ///
public static final double DRIVETRAIN_MAX_SPEED = 1.0;
public static final double CLAW_ARM_MAX_SPEED = 0.5;
public static final double WINCH_MAX_SPEED = 0.5;
public static final double CLAW_ARM_STOW_ANGLE = 0.0;
public static final double CLAW_ARM_DEPOSIT_ANGLE = 120.0;
public static final double CLAW_ARM_COLLECT_ANGLE = 150.0;
public static final double CLAW_WRIST_STOW_POSITION = 0.5;
public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.5;
public static final double CLAW_WRIST_COLLECT_POSITION = 0.5;
public static final double CLAW_LEFT_CLOSED_POSITION = 0.5;
public static final double CLAW_LEFT_OPEN_POSITION = 0.5;
public static final double CLAW_RIGHT_CLOSED_POSITION = 0.5;
public static final double CLAW_RIGHT_OPEN_POSITION = 0.5;
public static final double DRONE_LATCH_INITIAL_POSITION = 0.5;
public static final double DRONE_LATCH_LAUNCH_POSITION = 0.5;
public static final int DRONE_LAUNCH_CONFIRMATION_TIME_MS = 1_000;
public static final double HOOK_ARM_STOW_POSITION = 0.8; // just off of airplane 0.8
public static final double HOOK_ARM_UP_POSITION = 0.4; // streight up4.0
/// MOTOR CONSTANTS ///
public static final int CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = 4;
public static final double CLAW_ARM_MOTOR_GEAR_RATIO = 72;
/// HARDWARE ///
public final IMU imu;
public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm;
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
final CyberarmEngine engine;
public RedCrabMinibot() {
engine = CyberarmEngine.instance;
/// IMU ///
/// ------------------------------------------------------------------------------------ ///
imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
/// DRIVE TRAIN ///
/// ------------------------------------------------------------------------------------ ///
frontLeft = new MotorEx(engine.hardwareMap, "frontLeft"); // | Ctrl|Ex Hub, Port: ??
frontRight = new MotorEx(engine.hardwareMap, "frontRight"); // | Ctrl|Ex Hub, Port: ??
backLeft = new MotorEx(engine.hardwareMap, "backLeft"); // | Ctrl|Ex Hub, Port: ??
backRight = new MotorEx(engine.hardwareMap, "backRight"); // | Ctrl|Ex Hub, Port: ??
/// --- (SOFT) RESET MOTOR ENCODERS
frontLeft.resetEncoder();
frontRight.resetEncoder();
backLeft.resetEncoder();
backRight.resetEncoder();
/// --- MOTOR DIRECTIONS
frontLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
frontRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.motorEx.setDirection(DcMotorSimple.Direction.REVERSE);
backRight.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- MOTOR BRAKING MODE
/// --- NOTE: Having BRAKE mode set for drivetrain helps with consistently of control
frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior((Motor.ZeroPowerBehavior.BRAKE));
/// WINCH ///
/// ------------------------------------------------------------------------------------ ///
winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ??
/// --- (SOFT) MOTOR ENCODER RESET
winch.resetEncoder();
/// --- MOTOR DIRECTION
/// --- NOTE: Unknown if FORWARD or REVERSE is correct
winch.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
/// CLAW and Co. ///
/// ------------------------------------------------------------------------------------ ///
clawArm = new MotorEx(engine.hardwareMap, "clawArm"); // | Ctrl|Ex Hub, Port: ??
clawWrist = engine.hardwareMap.servo.get("clawWrist"); // | Ctrl|Ex Hub, Port: ??
leftClaw = engine.hardwareMap.servo.get("leftClaw"); // | Ctrl|Ex Hub, Port: ??
rightClaw = engine.hardwareMap.servo.get("rightClaw"); // | Ctrl|Ex Hub, Port: ??
/// --- Claw Arm Motor
/// --- --- (SOFT) RESET MOTOR ENCODER
clawArm.resetEncoder();
/// --- --- DIRECTION
clawArm.motorEx.setDirection(DcMotorSimple.Direction.FORWARD);
/// --- --- BRAKING
/// --- --- NOTE: This won't hold back much, if anything, but its a small help, maybe? 😃
clawArm.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
/// --- --- Run Mode
clawArm.setRunMode(Motor.RunMode.PositionControl);
clawArm.setTargetPosition(0);
/// --- Claws
/// --- --- Wrist
clawWrist.setDirection(Servo.Direction.FORWARD);
// clawWrist.setPosition(CLAW_WRIST_INITIAL_POSITION);
/// --- --- Left
leftClaw.setDirection(Servo.Direction.FORWARD);
// leftClaw.setPosition((CLAW_LEFT_CLOSED_POSITION));
/// --- --- Right
leftClaw.setDirection(Servo.Direction.FORWARD);
// leftClaw.setPosition((CLAW_RIGHT_CLOSED_POSITION));
/// DRONE LATCH ///
droneLatch = engine.hardwareMap.servo.get("droneLatch");
droneLatch.setDirection(Servo.Direction.FORWARD);
// droneLatch.setPosition(DRONE_LATCH_INITIAL_POSITION);
/// HOOK ARM ///
hookArm = engine.hardwareMap.servo.get("hookArm");
hookArm.setDirection(Servo.Direction.FORWARD);
// hookArm.setPosition(HOOK_ARM_STOW_POSITION);
}
public void standardTelemetry() {
engine.telemetry.addLine();
engine.telemetry.addLine("Motors");
engine.telemetry.addData(
"Front Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
frontLeft.motorEx.getPower(),
frontLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
frontLeft.motorEx.getCurrentPosition());
engine.telemetry.addData(
"Front Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
frontRight.motorEx.getPower(),
frontRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
frontRight.motorEx.getCurrentPosition());
engine.telemetry.addData(
"Back Left",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
backLeft.motorEx.getPower(),
backLeft.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
backLeft.motorEx.getCurrentPosition());
engine.telemetry.addData(
"Back Right",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
backRight.motorEx.getPower(),
backRight.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
backRight.motorEx.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData(
"Winch",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
winch.motorEx.getPower(),
winch.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
winch.motorEx.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData(
"Claw Arm",
"Power: %.2f, Current: %.2f mAmp, Position: %d",
clawArm.motorEx.getPower(),
clawArm.motorEx.getCurrent(CurrentUnit.MILLIAMPS),
clawArm.motorEx.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addLine("Servos");
engine.telemetry.addData("Claw Wrist", clawWrist.getPosition());
engine.telemetry.addData("Left Claw", leftClaw.getPosition());
engine.telemetry.addData("Right Claw", rightClaw.getPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Drone Latch", droneLatch.getPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Hook Arm", hookArm.getPosition());
engine.telemetry.addLine();
}
}

15
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java Normal file
View File

@@ -0,0 +1,15 @@
package dev.cyberarm.minibots.red_crab.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
import dev.cyberarm.minibots.red_crab.states.Pilot;
@TeleOp(name = "Cyberarm Red Crab TeleOp", group = "MINIBOT")
public class RedCrabTeleOpEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new Pilot(new RedCrabMinibot()));
}
}

182
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java Normal file
View File

@@ -0,0 +1,182 @@
package dev.cyberarm.minibots.red_crab.states;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.engine.V2.Utilities;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
public class Pilot extends CyberarmState {
private final RedCrabMinibot robot;
private boolean leftClawOpen = false;
private boolean rightClawOpen = false;
private int clawArmPosition = RedCrabMinibot.ClawArm_STOW;
private boolean hookArmUp = false;
private boolean droneLaunchAuthorized = false;
private boolean droneLaunchRequested = false;
private double droneLastLaunchRequestStartMS = 0;
public Pilot(RedCrabMinibot robot) {
this.robot = robot;
}
@Override
public void exec() {
drivetrain();
clawArmAndWristController();
// clawController();
// droneLatchController();
// hookArmController(); // disabled for swrist debug
// winchController();
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad == engine.gamepad1) {
switch (button) {
case "y":
droneLaunchRequested = true;
droneLastLaunchRequestStartMS = runTime();
break;
case "x":
hookArmUp = true;
break;
case "b":
hookArmUp = false;
break;
case "guide":
robot.imu.resetYaw();
break;
case "left_bumper":
leftClawOpen = !leftClawOpen;
break;
case "right_bumper":
rightClawOpen = !rightClawOpen;
break;
case "dpad_up":
clawArmPosition = RedCrabMinibot.ClawArm_COLLECT;
break;
case "dpad_down":
clawArmPosition = RedCrabMinibot.ClawArm_DEPOSIT;
break;
case "dpad_left":
case "dpad_right":
clawArmPosition = RedCrabMinibot.ClawArm_STOW;
break;
}
}
}
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (gamepad == engine.gamepad1) {
switch (button) {
case "y":
droneLaunchRequested = false;
droneLastLaunchRequestStartMS = runTime();
break;
}
}
}
@Override
public void telemetry() {
robot.standardTelemetry();
}
private void drivetrain() {
// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html#field-centric
double y = -engine.gamepad1.left_stick_y; // Remember, Y stick value is reversed;
double x = engine.gamepad1.left_stick_x;
double rx = engine.gamepad1.right_stick_x;
double botHeading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
// Rotate the movement direction counter to the bot's rotation
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
rotX = rotX * 1.1; // Counteract imperfect strafing
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
double maxPower = RedCrabMinibot.DRIVETRAIN_MAX_SPEED;
robot.frontLeft.motorEx.setPower(frontLeftPower * maxPower);
robot.backLeft.motorEx.setPower(backLeftPower * maxPower);
robot.frontRight.motorEx.setPower(frontRightPower * maxPower);
robot.backRight.motorEx.setPower(backRightPower * maxPower);
}
private void clawArmAndWristController() {
switch (clawArmPosition) {
case RedCrabMinibot.ClawArm_STOW:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_STOW_ANGLE));
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_STOW_POSITION);
break;
case RedCrabMinibot.ClawArm_DEPOSIT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE));
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION);
break;
case RedCrabMinibot.ClawArm_COLLECT:
robot.clawArm.setTargetPosition(Utilities.motorAngle(
RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,
RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO,
RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE));
robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_POSITION);
break;
}
robot.clawArm.set(RedCrabMinibot. CLAW_ARM_MAX_SPEED);
}
private void clawController() {
robot.leftClaw.setPosition((leftClawOpen ? RedCrabMinibot.CLAW_LEFT_OPEN_POSITION : RedCrabMinibot.CLAW_LEFT_CLOSED_POSITION));
robot.rightClaw.setPosition((rightClawOpen ? RedCrabMinibot.CLAW_RIGHT_OPEN_POSITION : RedCrabMinibot.CLAW_RIGHT_CLOSED_POSITION));
}
private void droneLatchController() {
if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS)
droneLaunchAuthorized = true;
if (droneLaunchAuthorized) {
robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION);
}
}
private void hookArmController() {
robot.hookArm.setPosition((hookArmUp ? RedCrabMinibot.HOOK_ARM_UP_POSITION : RedCrabMinibot.HOOK_ARM_STOW_POSITION));
}
private void winchController() {
if (engine.gamepad1.right_trigger > 0) {
robot.winch.motorEx.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
} else if (engine.gamepad1.left_trigger > 0) {
robot.winch.motorEx.setPower(engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED);
} else {
robot.winch.motorEx.setPower(0);
}
}
}

55
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java Normal file
View File

@@ -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));
}
}

23
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/OdometryTestEngine.java Normal file
View File

@@ -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"*/));
}
}

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java
View File

@@ -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;

6
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ServoTestEngine.java
View File

@@ -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;

8
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/SodiPizzaAutoRedRightEngine.java
View File

@@ -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 {

5
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ArmStateSodi.java
View File

@@ -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;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateSample.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateSample.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import org.timecrafters.CenterStage.Common.PrototypeRobot;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/AutoStateScrimmage.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/AutoStateScrimmage.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import com.qualcomm.robotcore.hardware.DcMotor;

3
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DepositorArmPosState.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/DepositorArmPosState.java
View File

@@ -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;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/MiniYAutonomousState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import dev.cyberarm.engine.V2.CyberarmState;

7
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/ProtoBotStateSodi.java
View File

@@ -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;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoArmState.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoArmState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoFirstDriveState.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoFirstDriveState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import android.annotation.SuppressLint;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoSecDriveState.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoSecDriveState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import com.qualcomm.robotcore.hardware.DcMotor;

4
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaAutoTurnState.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaAutoTurnState.java
View File

@@ -1,12 +1,10 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import android.annotation.SuppressLint;
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;

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/SodiPizzaWheelTest.java → TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/SodiStates/SodiPizzaWheelTest.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.CenterStage.Autonomous.States;
package org.timecrafters.CenterStage.Autonomous.SodiStates;
import android.annotation.SuppressLint;

82
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/DriveToCoordinatesState.java
View File

@@ -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)
}
}

214
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java Normal file
View File

@@ -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;
}
}

529
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -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);
}
}
}

168
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java Normal file
View File

@@ -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);
}
}

95
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java
View File

@@ -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());
}
}

5
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/XDrivetrainRobotState.java
View File

@@ -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);

4
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/headingLockTeleOp.java
View File

@@ -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);
}
}

3
gradle/wrapper/gradle-wrapper.properties vendored
View File

@@ -1,5 +1,6 @@
#Mon Dec 04 18:52:45 CST 2023
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip
distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists