mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 16:52:35 +00:00
autonomous states and engine
This commit is contained in:
SpencerPiha
@@ -0,0 +1,30 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
|
||||
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
|
||||
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DistanceCheckState;
|
||||
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
@Autonomous(name = "Competition Red Close")
|
||||
|
||||
public class CompetitionRedClose extends CyberarmEngine {
|
||||
|
||||
CompetitionRobotV1 robot;
|
||||
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new CompetitionRobotV1("Competition Red Close");
|
||||
this.robot.setup();
|
||||
|
||||
addState(new DriveToCoordinatesState(robot,"Competition Red Close", "00-1"));
|
||||
addState(new DistanceCheckState(robot,"Competition Red Close", "00-2"));
|
||||
|
||||
addState(new ClawArmState(robot,"Competition Red Close", "01-0"));
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,89 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@Config
|
||||
public class ClawArmState extends CyberarmState {
|
||||
|
||||
CompetitionRobotV1 robot;
|
||||
public String armPos;
|
||||
|
||||
public ClawArmState(CompetitionRobotV1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.armPos = robot.configuration.variable(groupName, actionName, "armPos").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
// odometry driving ALWAYS
|
||||
robot.DriveToCoordinates();
|
||||
robot.OdometryLocalizer();
|
||||
|
||||
// driving arm to pos
|
||||
if (armPos.equals("collect")){
|
||||
if (robot.lift.getCurrentPosition() >= 1){
|
||||
robot.lift.setPower(-0.6);
|
||||
} else {
|
||||
robot.lift.setPower(0);
|
||||
robot.shoulder.setPosition(robot.shoulderCollect);
|
||||
robot.elbow.setPosition(robot.elbowCollect);
|
||||
robot.target = 10;
|
||||
|
||||
}
|
||||
}
|
||||
if (armPos.equals("passive")){
|
||||
if (robot.lift.getCurrentPosition() >= 1){
|
||||
robot.lift.setPower(-0.6);
|
||||
} else {
|
||||
robot.lift.setPower(0);
|
||||
robot.shoulder.setPosition(robot.shoulderPassive);
|
||||
robot.elbow.setPosition(robot.elbowPassive);
|
||||
robot.target = 850;
|
||||
}
|
||||
}
|
||||
if (armPos.equals("deposit")){
|
||||
robot.shoulder.setPosition(robot.shoulderDeposit);
|
||||
robot.elbow.setPosition(robot.elbowDeposit);
|
||||
robot.target = 370;
|
||||
|
||||
}
|
||||
if (armPos.equals("hover")) {
|
||||
robot.shoulder.setPosition(robot.shoulderCollect);
|
||||
robot.elbow.setPosition(robot.elbowCollect);
|
||||
robot.target = 150;
|
||||
|
||||
}
|
||||
robot.armPos = armPos;
|
||||
robot.clawArmControl();
|
||||
|
||||
if (robot.clawArm.getCurrentPosition() >= robot.target - 20 && robot.clawArm.getCurrentPosition() <= robot.target + 20) {
|
||||
// setHasFinished(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", Math.toDegrees(robot.positionH));
|
||||
engine.telemetry.addData("aux encoder", robot.currentAuxPosition);
|
||||
engine.telemetry.addData("left encoder", robot.currentLeftPosition);
|
||||
engine.telemetry.addData("right encoder", robot.currentRightPosition);
|
||||
engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
engine.telemetry.addData("front right power", robot.frontRightPower);
|
||||
engine.telemetry.addData("front left power", robot.frontLeftPower);
|
||||
engine.telemetry.addData("back right power", robot.backRightPower);
|
||||
engine.telemetry.addData("back left power", robot.backLeftPower);
|
||||
engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition());
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
@Config
|
||||
public class DistanceCheckState extends CyberarmState {
|
||||
|
||||
CompetitionRobotV1 robot;
|
||||
public String armPos;
|
||||
public long lastCheckedTIme = System.currentTimeMillis();
|
||||
|
||||
public DistanceCheckState(CompetitionRobotV1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
// odometry driving ALWAYS
|
||||
robot.DriveToCoordinates();
|
||||
robot.OdometryLocalizer();
|
||||
|
||||
if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > -47 && robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < -43){
|
||||
robot.objectPos = "Right";
|
||||
} else if (robot.customObject.getDistance(DistanceUnit.MM) > 1000){
|
||||
robot.hTarget = 0;
|
||||
}
|
||||
if (robot.objectPos != "right")
|
||||
if (Math.abs(robot.backLeftPower) < 0.15 &&
|
||||
Math.abs(robot.backRightPower) < 0.15 &&
|
||||
Math.abs(robot.frontLeftPower) < 0.15 &&
|
||||
Math.abs(robot.frontRightPower) < 0.15 && System.currentTimeMillis() - lastCheckedTIme > 2000){
|
||||
if (robot.customObject.getDistance(DistanceUnit.MM) < 1000 && System.currentTimeMillis() - lastCheckedTIme > 2000){
|
||||
robot.objectPos = "middle";
|
||||
} else {
|
||||
robot.hTarget = 45;
|
||||
robot.objectPos = "left";
|
||||
}
|
||||
}
|
||||
|
||||
if (robot.objectPos.equals("left") || robot.objectPos.equals("right") || robot.objectPos.equals("middle")){
|
||||
setHasFinished(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", Math.toDegrees(robot.positionH));
|
||||
engine.telemetry.addData("aux encoder", robot.currentAuxPosition);
|
||||
engine.telemetry.addData("left encoder", robot.currentLeftPosition);
|
||||
engine.telemetry.addData("right encoder", robot.currentRightPosition);
|
||||
engine.telemetry.addData("h pos imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
|
||||
engine.telemetry.addData("front right power", robot.frontRightPower);
|
||||
engine.telemetry.addData("front left power", robot.frontLeftPower);
|
||||
engine.telemetry.addData("back right power", robot.backRightPower);
|
||||
engine.telemetry.addData("back left power", robot.backLeftPower);
|
||||
engine.telemetry.addData("arm pos", robot.clawArm.getCurrentPosition());
|
||||
engine.telemetry.addData("distance ", robot.customObject.getDistance(DistanceUnit.MM));
|
||||
engine.telemetry.addData("object location ", robot.objectPos);
|
||||
}
|
||||
}
|
||||
@@ -16,12 +16,14 @@ public class DriveToCoordinatesState extends CyberarmState {
|
||||
public static double yTarget = 0;
|
||||
public static double hTarget= 0;
|
||||
public boolean posAchieved = false;
|
||||
public boolean armDrive;
|
||||
|
||||
public DriveToCoordinatesState(CompetitionRobotV1 robot/*, String groupName, String actionName*/) {
|
||||
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();
|
||||
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();
|
||||
this.armDrive = robot.configuration.variable(groupName, actionName, "armDrive").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -32,13 +34,19 @@ public class DriveToCoordinatesState extends CyberarmState {
|
||||
robot.DriveToCoordinates();
|
||||
robot.OdometryLocalizer();
|
||||
|
||||
|
||||
if (armDrive){
|
||||
robot.clawArmControl();
|
||||
}
|
||||
|
||||
if (posAchieved){
|
||||
// setHasFinished(true);
|
||||
setHasFinished(true);
|
||||
} else {
|
||||
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;
|
||||
if (Math.abs(robot.backLeftPower) < 0.15 &&
|
||||
Math.abs(robot.backRightPower) < 0.15 &&
|
||||
Math.abs(robot.frontLeftPower) < 0.15 &&
|
||||
Math.abs(robot.frontRightPower) < 0.15){
|
||||
posAchieved = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -48,7 +56,7 @@ public class DriveToCoordinatesState extends CyberarmState {
|
||||
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("h pos odo", Math.toDegrees(robot.positionH));
|
||||
engine.telemetry.addData("aux encoder", robot.currentAuxPosition);
|
||||
engine.telemetry.addData("left encoder", robot.currentLeftPosition);
|
||||
engine.telemetry.addData("right encoder", robot.currentRightPosition);
|
||||
|
||||
@@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Autonomous.Engines;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
|
||||
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
@@ -15,18 +16,14 @@ 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"*/));
|
||||
addState(new DriveToCoordinatesState(robot,"odoTest", "00-1"));
|
||||
addState(new ClawArmState(robot,"odoTest", "01-0"));
|
||||
}
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
robot.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,9 +1,12 @@
|
||||
package org.timecrafters.CenterStage.Common;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.IMU;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
@@ -22,25 +25,27 @@ public class CompetitionRobotV1 extends Robot {
|
||||
private CyberarmEngine engine;
|
||||
|
||||
public TimeCraftersConfiguration configuration;
|
||||
public String objectPos;
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------ HardwareMap setup:
|
||||
|
||||
public DcMotor frontLeft, frontRight, backLeft, backRight, lift;
|
||||
public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp;
|
||||
public DcMotor odometerR, odometerL, odometerA;
|
||||
|
||||
public IMU imu;
|
||||
public Servo shoulder, elbow, leftClaw, rightClaw;
|
||||
public DistanceSensor customObject;
|
||||
|
||||
// ----------------------------------------------------------------------------------------------------------------- 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;
|
||||
public static double Hp = 0.8, Hi = 0, Hd = 0;
|
||||
public static double Xp = -0.03, Xi = 0, Xd = 0;
|
||||
public static double Yp = 0.03, Yi = 0, Yd = 0.0013;
|
||||
private double drivePower = 1;
|
||||
public double rx;
|
||||
public double xMultiplier = 1;
|
||||
public double yMultiplier = 1;
|
||||
public double positionX = 0;
|
||||
public double positionY = 0;
|
||||
public double positionX = 1000;
|
||||
public double positionY = 1000;
|
||||
public double positionH = 0;
|
||||
public double xTarget;
|
||||
public double yTarget;
|
||||
@@ -53,7 +58,7 @@ public class CompetitionRobotV1 extends Robot {
|
||||
public int oldLeftPosition = 0;
|
||||
public int oldAuxPosition = 0;
|
||||
public final static double L = 22.5; // distance between left and right encoder in cm
|
||||
final static double B = 11.5; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
|
||||
final static double B = 15; // distance between the midpoint of the left and right encoder from the auxillary encoder in cm
|
||||
public final static double R = 3; // wheel radius in cm
|
||||
final static double N = 8192; // encoder ticks per revolution (REV encoder)
|
||||
|
||||
@@ -75,12 +80,17 @@ public class CompetitionRobotV1 extends Robot {
|
||||
public double backRightPower;
|
||||
|
||||
//-------------------------------------------------------------------------------------------------------------- arm sequence variables:
|
||||
PIDController pidController;
|
||||
public String armPos;
|
||||
public int target;
|
||||
public static double p = 0.0015, i = 0, d = 0, f = 0;
|
||||
public static double shoulderCollect = 0.25;
|
||||
public static double shoulderDeposit = 0.55;
|
||||
public static double shoulderPassive = 1;
|
||||
public static double elbowCollect = 0.7;
|
||||
public static double elbowDeposit = 0.8;
|
||||
public static double elbowPassive = 0.28;
|
||||
public static double shoulderDeposit = 0.32;
|
||||
public static double shoulderPassive = 0.8;
|
||||
public static double elbowCollect = 0;
|
||||
public static double elbowDeposit = 0;
|
||||
public static double elbowPassive = 0;
|
||||
|
||||
|
||||
private HardwareMap hardwareMap;
|
||||
|
||||
@@ -90,6 +100,8 @@ public class CompetitionRobotV1 extends Robot {
|
||||
this.engine = engine;
|
||||
setup();
|
||||
this.string = string;
|
||||
pidController = new PIDController(p, i, d);
|
||||
|
||||
}
|
||||
|
||||
public void initConstants() {
|
||||
@@ -109,24 +121,38 @@ public class CompetitionRobotV1 extends Robot {
|
||||
frontLeft = engine.hardwareMap.dcMotor.get("frontLeft");
|
||||
backRight = engine.hardwareMap.dcMotor.get("backRight");
|
||||
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
|
||||
chinUp = engine.hardwareMap.dcMotor.get("chinUp");
|
||||
lift = engine.hardwareMap.dcMotor.get("Lift");
|
||||
clawArm = engine.hardwareMap.dcMotor.get("clawArm");
|
||||
|
||||
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backRight.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
clawArm.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
lift.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
chinUp.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
|
||||
backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
backLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
chinUp.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_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);
|
||||
chinUp.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
//IMU
|
||||
IMU.Parameters parameters = new IMU.Parameters(
|
||||
@@ -142,15 +168,20 @@ public class CompetitionRobotV1 extends Robot {
|
||||
|
||||
initConstants();
|
||||
|
||||
pidController = new PIDController(p, i, d);
|
||||
|
||||
customObject = engine.hardwareMap.get(Rev2mDistanceSensor.class, "customObject");
|
||||
|
||||
//--------------------------------------------------------------------------------------------------------------------------- SERVO:
|
||||
shoulder = hardwareMap.servo.get("shoulder");
|
||||
elbow = hardwareMap.servo.get("elbow");
|
||||
leftClaw = hardwareMap.servo.get("leftClaw");
|
||||
rightClaw = hardwareMap.servo.get("leftClaw");
|
||||
rightClaw = hardwareMap.servo.get("rightClaw");
|
||||
|
||||
shoulder.setPosition(shoulderPassive);
|
||||
elbow.setPosition(elbowPassive);
|
||||
elbow.setDirection(Servo.Direction.REVERSE);
|
||||
|
||||
// shoulder.setPosition(shoulderPassive);
|
||||
// elbow.setPosition(elbowPassive);
|
||||
|
||||
|
||||
|
||||
@@ -160,11 +191,12 @@ public class CompetitionRobotV1 extends Robot {
|
||||
|
||||
public void OdometryLocalizer() { // ------------------------------------------------------------------------------- Odometry Localizer:
|
||||
// update positions
|
||||
|
||||
oldRightPosition = currentRightPosition;
|
||||
oldLeftPosition = currentLeftPosition;
|
||||
oldAuxPosition = currentAuxPosition;
|
||||
|
||||
currentRightPosition = -frontLeft.getCurrentPosition();
|
||||
currentRightPosition = frontLeft.getCurrentPosition();
|
||||
currentLeftPosition = -backRight.getCurrentPosition();
|
||||
currentAuxPosition = backLeft.getCurrentPosition();
|
||||
|
||||
@@ -205,7 +237,7 @@ public class CompetitionRobotV1 extends Robot {
|
||||
}
|
||||
|
||||
public double XPIDControl ( double reference, double current){
|
||||
double error = (current - reference);
|
||||
double error = (reference - current);
|
||||
xIntegralSum += error * xTimer.seconds();
|
||||
double derivative = (error - xLastError) / xTimer.seconds();
|
||||
|
||||
@@ -216,7 +248,7 @@ public class CompetitionRobotV1 extends Robot {
|
||||
}
|
||||
|
||||
public double YPIDControl ( double reference, double current){
|
||||
double error = (current - reference);
|
||||
double error = (reference - current);
|
||||
yIntegralSum += error * yTimer.seconds();
|
||||
double derivative = (error - yLastError) / yTimer.seconds();
|
||||
|
||||
@@ -232,8 +264,9 @@ public class CompetitionRobotV1 extends Robot {
|
||||
// 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 pidX = YPIDControl(yTarget, positionY);
|
||||
double pidY = XPIDControl(xTarget, positionX);
|
||||
|
||||
double denominator = Math.max(Math.abs(pidX) + Math.abs(pidY) + Math.abs(rx), 1);
|
||||
|
||||
@@ -246,12 +279,24 @@ public class CompetitionRobotV1 extends Robot {
|
||||
frontLeftPower = (rotY + rotX + rx) / denominator;
|
||||
backLeftPower = (rotY - rotX + rx) / denominator;
|
||||
frontRightPower = (rotY - rotX - rx) / denominator;
|
||||
backRightPower = (rotY - rotX + rx) / denominator;
|
||||
backRightPower = (rotY + rotX - rx) / denominator;
|
||||
|
||||
// apply my powers
|
||||
frontLeft.setPower(frontLeftPower);
|
||||
backLeft.setPower(backLeftPower);
|
||||
backRight.setPower(backRightPower);
|
||||
backRight.setPower(-backRightPower);
|
||||
frontRight.setPower(frontRightPower);
|
||||
}
|
||||
|
||||
public void clawArmControl(){
|
||||
|
||||
pidController.setPID(p, i, d);
|
||||
int armPos = clawArm.getCurrentPosition();
|
||||
double pid = pidController.calculate(armPos, target);
|
||||
|
||||
double power = pid;
|
||||
|
||||
clawArm.setPower(power);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -3,9 +3,7 @@ package org.timecrafters.CenterStage.TeleOp.Engines;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
import org.timecrafters.CenterStage.Common.PrototypeRobot;
|
||||
import org.timecrafters.CenterStage.TeleOp.States.CompetitionTeleOpState;
|
||||
import org.timecrafters.CenterStage.TeleOp.States.PrototypeRobotDrivetrainState;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
package org.timecrafters.CenterStage.TeleOp.States;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.arcrobotics.ftclib.controller.PIDController;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
@@ -14,6 +16,11 @@ import dev.cyberarm.engine.V2.CyberarmState;
|
||||
public class CompetitionTeleOpState extends CyberarmState {
|
||||
// ------------------------------------------------------------------------------------------------------------- State and engine setup:
|
||||
private CompetitionRobotV1 robot;
|
||||
// ------------------------------------------------------------------------------------------------------------robot claw arm variables:
|
||||
private PIDController pidController;
|
||||
public static double p = 0.0015, i = 0, d = 0, f = 0;
|
||||
public static int target = 0;
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
|
||||
public double integralSum = 0;
|
||||
private double targetHeading;
|
||||
@@ -31,8 +38,8 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
ElapsedTime timer = new ElapsedTime();
|
||||
|
||||
// ---------------------------------------------------------------------------------------------------- collector / depositor variables:
|
||||
public static double leftOpen = 0;
|
||||
public static double leftClose = 0;
|
||||
public static double leftOpen = 0.25;
|
||||
public static double leftClose = 0.6;
|
||||
public static double rightOpen = 0.6;
|
||||
public static double rightClose = 0.25;
|
||||
public double collectorPosLeft = leftClose;
|
||||
@@ -58,7 +65,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
|
||||
public CompetitionTeleOpState(CompetitionRobotV1 robot) {
|
||||
this.robot = robot;
|
||||
|
||||
pidController = new PIDController(p, i, d);
|
||||
}
|
||||
|
||||
// }
|
||||
@@ -129,7 +136,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
// setting each power determined previously from the math above
|
||||
// as well as multiplying it by a drive power that can be changed.
|
||||
robot.backLeft.setPower(backLeftPower * drivePower);
|
||||
robot.backRight.setPower(backRightPower * drivePower);
|
||||
robot.backRight.setPower(-backRightPower * drivePower);
|
||||
robot.frontLeft.setPower(frontLeftPower * drivePower);
|
||||
robot.frontRight.setPower(frontRightPower * drivePower);
|
||||
}
|
||||
@@ -155,7 +162,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
return output;
|
||||
}
|
||||
public void ClawControlTeleOp() {
|
||||
boolean b2 = engine.gamepad2.b;
|
||||
boolean b2 = engine.gamepad1.y;
|
||||
if (b2 && !bVar2) {
|
||||
if (collectorPosLeft == leftClose && collectorPosRight == rightClose) {
|
||||
collectorPosLeft = leftOpen;
|
||||
@@ -167,7 +174,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
}
|
||||
bVar2 = b2;
|
||||
|
||||
boolean lbs2 = engine.gamepad2.left_stick_button;
|
||||
boolean lbs2 = engine.gamepad1.left_bumper;
|
||||
if (lbs2 && !lbsVar2) {
|
||||
if (collectorPosLeft == leftClose) {
|
||||
collectorPosLeft = leftOpen;
|
||||
@@ -177,7 +184,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
}
|
||||
lbsVar2 = lbs2;
|
||||
|
||||
boolean rbs2 = engine.gamepad2.right_stick_button;
|
||||
boolean rbs2 = engine.gamepad1.right_bumper;
|
||||
if (rbs2 && !rbsVar2) {
|
||||
if (collectorPosRight == rightClose) {
|
||||
collectorPosRight = rightOpen;
|
||||
@@ -200,6 +207,10 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
armPos = "deposit";
|
||||
depositMode = true;
|
||||
}
|
||||
else if (engine.gamepad2.b){
|
||||
armPos = "hover";
|
||||
depositMode = true;
|
||||
}
|
||||
|
||||
if (armPos == "collect"){
|
||||
if (robot.lift.getCurrentPosition() >= 1){
|
||||
@@ -208,6 +219,8 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
robot.lift.setPower(0);
|
||||
robot.shoulder.setPosition(robot.shoulderCollect);
|
||||
robot.elbow.setPosition(robot.elbowCollect);
|
||||
target = 10;
|
||||
|
||||
}
|
||||
}
|
||||
if (armPos == "passive"){
|
||||
@@ -217,20 +230,42 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
robot.lift.setPower(0);
|
||||
robot.shoulder.setPosition(robot.shoulderPassive);
|
||||
robot.elbow.setPosition(robot.elbowPassive);
|
||||
target = 850;
|
||||
}
|
||||
}
|
||||
if (armPos == "deposit"){
|
||||
robot.shoulder.setPosition(robot.shoulderDeposit);
|
||||
robot.elbow.setPosition(robot.elbowDeposit);
|
||||
}
|
||||
target = 370;
|
||||
|
||||
}
|
||||
if (armPos == "hover"){
|
||||
|
||||
robot.shoulder.setPosition(robot.shoulderCollect);
|
||||
robot.elbow.setPosition(robot.elbowCollect);
|
||||
target = 120;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
pidController = new PIDController(p, i, d);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if (engine.gamepad2.dpad_up) {
|
||||
robot.chinUp.setPower(1);
|
||||
} else if (engine.gamepad2.dpad_down){
|
||||
robot.chinUp.setPower(-1);
|
||||
} else {
|
||||
robot.chinUp.setPower(0);
|
||||
}
|
||||
// ---------------------------------------------------------------------------------------------------------- Game Pad 1, drivetrain
|
||||
if (engine.gamepad1.right_stick_button) {
|
||||
robot.imu.resetYaw();
|
||||
@@ -265,35 +300,25 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift:
|
||||
pidController.setPID(p, i, d);
|
||||
int armPos = robot.clawArm.getCurrentPosition();
|
||||
double pid = pidController.calculate(armPos, target);
|
||||
// double ff = Math.cos(Math.toRadians(target / ticksInDegree)) * f;
|
||||
|
||||
double power = pid;
|
||||
// double power = pid + ff;
|
||||
|
||||
robot.clawArm.setPower(power);
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------- Lift Control:
|
||||
SliderTeleOp();
|
||||
|
||||
robot.leftClaw.setPosition(collectorPosLeft);
|
||||
robot.rightClaw.setPosition(collectorPosRight);
|
||||
|
||||
boolean lbs2 = engine.gamepad2.left_stick_button;
|
||||
if (lbs2 && !lbsVar2) {
|
||||
if (collectorPosLeft == leftClose) {
|
||||
collectorPosLeft = leftOpen;
|
||||
} else {
|
||||
collectorPosLeft = leftClose;
|
||||
}
|
||||
}
|
||||
lbsVar2 = lbs2;
|
||||
|
||||
boolean rbs2 = engine.gamepad2.right_stick_button;
|
||||
if (rbs2 && !rbsVar2) {
|
||||
if (collectorPosRight == rightClose) {
|
||||
collectorPosRight = rightOpen;
|
||||
} else {
|
||||
collectorPosRight = rightClose;
|
||||
}
|
||||
}
|
||||
rbsVar2 = rbs2;
|
||||
|
||||
ArmPosControl();
|
||||
|
||||
ClawControlTeleOp();
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -308,6 +333,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
engine.telemetry.addData("Ki", Ki);
|
||||
engine.telemetry.addData("Kd", Kd);
|
||||
engine.telemetry.addData("arm pos", armPos);
|
||||
engine.telemetry.addData("arm pos ticks", robot.clawArm.getCurrentPosition());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user