Merge remote-tracking branch 'origin/master'

This commit is contained in:
NerdyBirdy460
2024-01-16 20:42:14 -06:00
11 changed files with 405 additions and 38 deletions

View File

@@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "BURNSVILLE blue audience")
@Autonomous(name = "old BURNSVILLE blue audience")
public class Competition2BlueBackStage extends CyberarmEngine {
@@ -43,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"));
@@ -57,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"));

View File

@@ -0,0 +1,65 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmControlTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience blue")
public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
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);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("Burnsville audience blue");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0"));
// addState(new CameraVisionState(robot));
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-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0"));
// place pixel
addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1"));
// drive to search pos
addState(new ClawArmState(robot,"Burnsville audience blue", "0-02-2"));
// close right finger
addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-3"));
}
}

View File

@@ -0,0 +1,65 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
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.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville audience red")
public class CompetitionBurnsvilleAudienceRed extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
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);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("Burnsville audience red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville audience red", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville audience red", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville audience red", "1-02-0"));
// place pixel
addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-1"));
// drive to search pos
addState(new ClawArmState(robot,"Burnsville audience red", "0-02-2"));
// close right finger
addState(new ClawFingerState(robot,"Burnsville audience red", "0-02-3"));
}
}

View File

@@ -0,0 +1,86 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
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.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop blue")
public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
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);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("Burnsville BackDrop blue");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-01-1"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "3-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "2-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop blue", "1-02-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-4"));
// place pixel
addState(new ClawFingerState(robot,"Burnsville BackDrop blue", "0-02-1"));
// drive to search pos
addState(new ClawArmState(robot,"Burnsville BackDrop blue", "0-02-2"));
// close right finger
addState(new ClawFingerState(robot,"Burnsville BackDrop blue", "0-02-3"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-03-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1"));
addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-04-2"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-05-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "end task"));
//
}
}

View File

@@ -0,0 +1,87 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
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.ClawFingerState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmEngine;
@Autonomous(name = "Burnsville BackDrop red")
public class CompetitionBurnsvilleBackDropRed extends CyberarmEngine {
CompetitionRobotV1 robot;
@Override
public void init() {
super.init();
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);
robot.imu.resetYaw();
robot.leftClaw.setPosition(0.25);
robot.rightClaw.setPosition(0.6);
}
@Override
public void setup() {
this.robot = new CompetitionRobotV1("Burnsville BackDrop red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
// addTask(new ClawArmControlTask(robot));
this.robot.setup();
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-0"));
// addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-01-1"));
// drive to the left, center, or right spike mark
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-02-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-02-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-5"));
// place pixel
addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-02-1"));
// drive to search pos
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-02-2"));
// close right finger
addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-02-3"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-03-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-03-1"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "1-04-0"));
// addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "2-04-0"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "3-04-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "0-04-1"));
addState(new ClawFingerState(robot,"Burnsville BackDrop red", "0-04-2"));
addState(new DriveToCoordinatesState(robot,"Burnsville BackDrop red", "0-05-0"));
addState(new ClawArmState(robot,"Burnsville BackDrop red", "end task"));
}
}

View File

@@ -0,0 +1,21 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
import com.acmerobotics.dashboard.config.Config;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmState;
import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
@Config
public class ClawArmControlTask extends CyberarmState {
CompetitionRobotV1 robot;
public ClawArmControlTask(CompetitionRobotV1 robot) {
this.robot = robot;
}
@Override
public void exec() {robot.clawArmControl();}
}

View File

@@ -57,27 +57,29 @@ public class DriveToCoordinatesState extends CyberarmState {
if (objectPos != robot.objectPos) {
// enter the ending loop
setHasFinished(true);
} else {
if (armDrive) {
robot.clawArmControl();
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
setHasFinished(true);
}
}
} else {
if (armDrive) {
robot.clawArmControl();
}
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5) {
setHasFinished(true);
}
}
}
if (armDrive) {
robot.clawArmControl();
}
robot.OdometryLocalizer();
robot.XDrivePowerModifier();
robot.YDrivePowerModifier();
robot.DriveToCoordinates();
robot.OdometryLocalizer();
if (Math.abs(robot.positionX - robot.xTarget) < 2
&& Math.abs(robot.positionY - robot.yTarget) < 2){
setHasFinished(true);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("x pos", robot.positionX);

View File

@@ -0,0 +1,23 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
import android.util.Log;
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 DriveToCoordinatesTask extends CyberarmState {
CompetitionRobotV1 robot;
public DriveToCoordinatesTask(CompetitionRobotV1 robot) {this.robot = robot;}
@Override
public void exec() {
robot.XDrivePowerModifier();
robot.YDrivePowerModifier();
robot.DriveToCoordinates();
}
}

View File

@@ -0,0 +1,18 @@
package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
import com.acmerobotics.dashboard.config.Config;
import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
import dev.cyberarm.engine.V2.CyberarmState;
@Config
public class OdometryLocalizerTask extends CyberarmState {
CompetitionRobotV1 robot;
public OdometryLocalizerTask(CompetitionRobotV1 robot) {
this.robot = robot;
}
@Override
public void exec() {robot.OdometryLocalizer();}
}

View File

@@ -40,7 +40,7 @@ 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, chinUp;
public DcMotorEx clawArm;
public IMU imu;
@@ -208,6 +208,7 @@ public class CompetitionRobotV1 extends Robot {
}
// -------------------------------------------------------------------------------------------------------------------------- Functions:
@@ -258,7 +259,7 @@ public class CompetitionRobotV1 extends Robot {
public double HeadingPIDControl(double reference, double current){
double error = angleWrap(current - reference);
headingIntegralSum += error * headingTimer.seconds();
double derivative = (headingLastError - error) / headingTimer.seconds();
double derivative = (error - headingLastError) / headingTimer.seconds();
headingTimer.reset();

View File

@@ -268,7 +268,7 @@ public class CompetitionTeleOpState extends CyberarmState {
}
if (Objects.equals(armPos, "lift up")) {
robot.shoulder.setPosition(robot.shoulderDeposit);
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowDeposit);
target = 120;
robot.chinUpServo.setPosition(chinUpServoUp);
@@ -282,8 +282,7 @@ public class CompetitionTeleOpState extends CyberarmState {
robot.lift.setPower(0);
robot.chinUpServo.setPosition(chinUpServoDown);
robot.shoulder.setPosition(robot.shoulderCollect);
robot.elbow.setPosition(robot.elbowCollect);
target = 850;
target = 120;
}
}