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

worked on odometry which is broken, and i fixed teleOp controls for drivetrain

Browse Source
This commit is contained in:
SpencerPiha
2024-01-10 20:04:44 -06:00
parent 5f2bb56fed
commit a2ac7bf946
5 changed files with 103 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java
View File

@@ -22,7 +22,9 @@ public class Competition2BlueBackStage extends CyberarmEngine {
public void init() {
super.init();
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.clawArm.setTargetPosition(0);
robot.clawArm.setPower(0);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
@@ -32,7 +34,7 @@ public class Competition2BlueBackStage extends CyberarmEngine {
public void setup() {
this.robot = new CompetitionRobotV1("burnsville audience blue");
this.robot.setup();
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0"));
@@ -41,9 +43,9 @@ public class Competition2BlueBackStage extends CyberarmEngine {
addState(new ClawArmState(robot,"burnsville audience blue", "0-01-1"));
// drive to the left, center, or right spike mark
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-02-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-02-0"));
// place pixel
addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-1"));
@@ -55,49 +57,49 @@ public class Competition2BlueBackStage extends CyberarmEngine {
addState(new ClawFingerState(robot,"burnsville audience blue", "0-02-3"));
// drive back and away from the spike mark (x,y) (1050, 1000) H = 0
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-03-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-03-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-03-0"));
// drive to the middle truss (right version) (1130,980) H = -90
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-04-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-04-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-04-0"));
// drive to hover mode to clear the middle truss
addState(new ClawArmState(robot,"burnsville audience blue", "0-04-1"));
// drive under the middle truss across the field (right version) (1170,1080) H = -90
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-05-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-05-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-05-0"));
// drive to deposit mode to place on back drop
addState(new ClawArmState(robot,"burnsville audience blue", "0-05-1"));
// drive to back drop
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-06-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-06-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-06-0"));
// open claw to deposit gold
addState(new ClawFingerState(robot,"burnsville audience blue", "0-06-1"));
// drive back from backdrop
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-07-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-07-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-07-0"));
// close left claw
addState(new ClawFingerState(robot,"burnsville audience blue", "0-07-1"));
// drive to parking spot
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "3-08-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "2-08-0"));
addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0"));
// addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "1-08-0"));
// arm to collect pos
addState(new ClawArmState(robot,"burnsville audience blue", "0-08-1"));

1
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java
View File

@@ -29,6 +29,7 @@ public class ClawArmState extends CyberarmState {
@Override
public void exec() {
robot.armTime = System.currentTimeMillis() - initTime;
// odometry driving ALWAYS
robot.DriveToCoordinates();
robot.OdometryLocalizer();

16
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java
View File

@@ -1,11 +1,11 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
import android.util.Log;
import com.acmerobotics.dashboard.config.Config;
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;
@Config
@@ -21,8 +21,11 @@ public class DriveToCoordinatesState extends CyberarmState {
public boolean posSpecific;
public double maxXPower;
public double maxYPower;
private String actionName;
public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
this.actionName = actionName;
this.robot = robot;
this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value();
this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
@@ -43,6 +46,9 @@ public class DriveToCoordinatesState extends CyberarmState {
robot.yMaxPower = maxYPower;
robot.xMaxPower = maxXPower;
Log.d("TTT?", "" + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.d("TTT?", "" + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);
}
@Override
@@ -64,10 +70,8 @@ public class DriveToCoordinatesState extends CyberarmState {
robot.DriveToCoordinates();
robot.OdometryLocalizer();
if ((Math.abs(robot.backLeftPower) < 0.15 &&
Math.abs(robot.backRightPower) < 0.15 &&
Math.abs(robot.frontLeftPower) < .15 &&
Math.abs(robot.frontRightPower) < 0.15)){
if (Math.abs(robot.positionX - robot.xTarget) < 2
&& Math.abs(robot.positionY - robot.yTarget) < 2){
setHasFinished(true);
}
}

49
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java
View File

@@ -5,10 +5,12 @@ 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.DcMotorEx;
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.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
@@ -38,10 +40,11 @@ public class CompetitionRobotV1 extends Robot {
// ------------------------------------------------------------------------------------------------------------------ HardwareMap setup:
public OpenCvWebcam webcam1 = null;
public WebcamName webCamName;
public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp;
public DcMotor frontLeft, frontRight, backLeft, backRight, lift, /*clawArm,*/ chinUp;
public DcMotorEx clawArm;
public IMU imu;
public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo;
public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo, shootServo;
public DistanceSensor customObject;
public TouchSensor touchLeftArm, touchRightArm;
@@ -97,14 +100,16 @@ public class CompetitionRobotV1 extends Robot {
PIDController pidController;
public double power;
public String armPos;
public long armTime;
public int target;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 0.38;
public double shoulderDeposit = 0.36;
public double shoulderPassive = 0.8;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double shoulderCollect = 1;
public double shoulderDeposit = 1;
public double shoulderPassive = 1;
public double elbowCollect = 0.02;
public double elbowDeposit = 0;
private HardwareMap hardwareMap;
@@ -140,7 +145,7 @@ public class CompetitionRobotV1 extends Robot {
backLeft = engine.hardwareMap.dcMotor.get("backLeft");
chinUp = engine.hardwareMap.dcMotor.get("chinUp");
lift = engine.hardwareMap.dcMotor.get("Lift");
clawArm = engine.hardwareMap.dcMotor.get("clawArm");
clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm");
frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -160,9 +165,6 @@ public class CompetitionRobotV1 extends Robot {
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);
@@ -198,7 +200,7 @@ public class CompetitionRobotV1 extends Robot {
leftClaw = hardwareMap.servo.get("leftClaw");
rightClaw = hardwareMap.servo.get("rightClaw");
chinUpServo = hardwareMap.servo.get("chinUpServo");
shootServo = hardwareMap.servo.get("shoot");
elbow.setDirection(Servo.Direction.REVERSE);
@@ -365,24 +367,27 @@ public class CompetitionRobotV1 extends Robot {
if (lift.getCurrentPosition() >= 20) {
lift.setPower(-0.6);
} else {
shoulder.setPosition(0.38);
shoulder.setPosition(shoulderCollect);
target = 120;
}
}
if (armPos.equals("search")) {
shoulder.setPosition(0.15);
target = 570;
shoulder.setPosition(0.48);
if (armTime > 400){
target = 570;
}
}
pidController.setPID(p, i, d);
int armPos = clawArm.getCurrentPosition();
double pid = pidController.calculate(armPos, target);
// pidController.setPID(p, i, d);
// int armPos = clawArm.getCurrentPosition();
// double pid = pidController.calculate(armPos, target);
//
// power = pid;
power = pid;
clawArm.setPower(power);
clawArm.setTargetPosition(target);
clawArm.setPower(0.4);
}
}

68
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java
View File

@@ -20,9 +20,13 @@ public class CompetitionTeleOpState extends CyberarmState {
private CompetitionRobotV1 robot;
// ------------------------------------------------------------------------------------------------------------robot claw arm variables:
private PIDController pidController;
public double p = 0.007, i = 0, d = 0.0001, f = 0;
public double p = 0.005, i = 0, d = 0.0001, f = 0;
public int target = 0;
// ---------------------------------------------------------------------------------------------------------------------shoot variables:
public static double releasePos = 0.95;
public static double holdPos = 0.55 ;
// ------------------------------------------------------------------------------------------------------------- Heading lock variables:
public double integralSum = 0;
@@ -111,19 +115,16 @@ public class CompetitionTeleOpState extends CyberarmState {
boolean lbs1 = engine.gamepad1.left_stick_button;
if (lbs1 && !lbsVar1) {
if (drivePower == 1) {
drivePower = 0.5;
drivePower = 0.35;
} else {
drivePower = 1;
}
}
lbsVar1 = lbs1;
lbsVar1 = lbs1
if (engine.gamepad1.left_stick_x != 0 || engine.gamepad1.left_stick_y != 0){
boost = engine.gamepad1.right_trigger + 1;
}
double x = -((engine.gamepad1.left_stick_x * 0.5) * boost);
double y = ((engine.gamepad1.left_stick_y * 0.5) * boost);
double x = -(engine.gamepad1.left_stick_x);
double y = (engine.gamepad1.left_stick_y);
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
// angle math to make things field oriented
@@ -139,10 +140,10 @@ 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);
robot.backRight.setPower(-backRightPower);
robot.frontLeft.setPower(frontLeftPower);
robot.frontRight.setPower(frontRightPower);
robot.backLeft.setPower(backLeftPower * drivePower);
robot.backRight.setPower(-backRightPower * drivePower);
robot.frontLeft.setPower(frontLeftPower * drivePower);
robot.frontRight.setPower(frontRightPower * drivePower);
}
public double angleWrap(double radians) {
@@ -211,6 +212,9 @@ public class CompetitionTeleOpState extends CyberarmState {
} else if (engine.gamepad2.b) {
armPos = "hover";
depositMode = true;
} else if (engine.gamepad2.x) {
armPos = "passive";
depositMode = true;
} else if (engine.gamepad2.dpad_left) {
armPos = "lift up";
depositMode = true;
@@ -230,7 +234,7 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
robot.chinUpServo.setPosition(chinUpServoDown);
target = 30;
target = 0;
}
}
@@ -251,8 +255,18 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.elbow.setPosition(robot.elbowCollect);
target = 120;
}
}
if (Objects.equals(armPos, "passive")) {
if (robot.lift.getCurrentPosition() >= 20) {
robot.chinUpServo.setPosition(chinUpServoDown);
robot.lift.setPower(-0.6);
} else {
robot.shoulder.setPosition(robot.shoulderPassive);
target = 570;
}
}
if (Objects.equals(armPos, "lift up")) {
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.elbow.setPosition(robot.elbowDeposit);
@@ -275,9 +289,10 @@ public class CompetitionTeleOpState extends CyberarmState {
if (armPos.equals("reset")) {
robot.shoulder.setPosition(robot.shoulderPassive);
target = -10000;
if (robot.touchLeftArm.isPressed() || robot.touchRightArm.isPressed()) {
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
armPos = "collect";
}
}
@@ -290,11 +305,22 @@ public class CompetitionTeleOpState extends CyberarmState {
pidController = new PIDController(p, i, d);
robot.imu.resetYaw();
robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.clawArm.setTargetPosition(0);
robot.clawArm.setPower(0);
robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
@Override
public void exec() {
if (engine.gamepad2.start && engine.gamepad2.x){
robot.shootServo.setPosition(releasePos);
} else {
robot.shootServo.setPosition(holdPos);
}
if (engine.gamepad2.dpad_up) {
robot.chinUp.setPower(-1);
@@ -334,16 +360,16 @@ public class CompetitionTeleOpState extends CyberarmState {
DriveTrainTeleOp();
// ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift:
pidController.setPID(p, i, d);
int armCurrentPos = robot.clawArm.getCurrentPosition();
double pid = pidController.calculate(armCurrentPos, target);
// pidController.setPID(p, i, d);
// int armCurrentPos = robot.clawArm.getCurrentPosition();
// double pid = pidController.calculate(armCurrentPos, target);
if (armPos.equals("reset")){
armPower = -0.2;
robot.clawArm.setPower(armPower);
} else {
armPower = pid;
armPower = 0.4; // pid
}
robot.clawArm.setTargetPosition(target);
robot.clawArm.setPower(armPower);
// ------------------------------------------------------------------------------------------------------------------- Lift Control: