eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

expansion IMU

This commit is contained in:
ImposterVarunoverlord 2024-02-20 12:51:53 -06:00
parent 28c4f0432b
commit bab6de06bb
4 changed files with 35 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java
View File

@ -14,6 +14,8 @@ public class endGame_Mechs {
private DcMotor hang; private DcMotor hang;
public static double initPos = 0.42; public static double initPos = 0.42;
public static double launchPos = 0.8; public static double launchPos = 0.8;
public static double finger_out = 0.5;
public static double finger_in = 0.2;
public static int initHang = -1000; public static int initHang = -1000;
public static double hold = 0.8; public static double hold = 0.8;
public static double release = 0.5; public static double release = 0.5;
@ -32,6 +34,8 @@ public class endGame_Mechs {
public endGame_Mechs(HardwareMap hardwareMap) { public endGame_Mechs(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, "Drone"); this.servo = hardwareMap.get(Servo.class, "Drone");
this.servo.setPosition(initPos); this.servo.setPosition(initPos);
this.servo = hardwareMap.get(Servo.class, "Finger");
this.servo.setPosition(finger_out);
this.hang = hardwareMap.get(DcMotor.class, "Hang"); this.hang = hardwareMap.get(DcMotor.class, "Hang");
this.hang.setTargetPosition(0); this.hang.setTargetPosition(0);
this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
@ -54,6 +58,11 @@ public class endGame_Mechs {
} }
public void Finger_in() {this.servo.setPosition(finger_in);}
public void Finger_out () {this.servo.setPosition(finger_out);}
public void hang_init_pos(){ public void hang_init_pos(){
this.hang.setTargetPosition(0); this.hang.setTargetPosition(0);
this.hang.setPower(1); this.hang.setPower(1);

33
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous; package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
@ -27,24 +28,24 @@ public class BlueBackStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90)); public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.8, 33.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187)); public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187)); public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 31.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187)); public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187));
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36 public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-187));//-36
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180)); public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180)); public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180)); public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-187));
@Override @Override
@ -137,7 +138,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0))); builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
break; break;
case 2: case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0))); builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5)));
break; break;
case 3: case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5))); builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5)));
@ -150,7 +151,7 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 5: case 5:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.68); robot.intake.setDcMotor(0.7);
robot.intake.setpos(STACK5); robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
macroState++; macroState++;
@ -159,13 +160,13 @@ public class BlueBackStageAuto extends AutoBase {
//First 2 pixels off the stack are intaken by this //First 2 pixels off the stack are intaken by this
case 6: case 6:
robot.intake.setDcMotor(0.68); robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK5); robot.intake.setpos(STACK4);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
//goes back to the easel //goes back to the easel
case 7: case 7:
if (getRuntime() > macroTime + 0.03) { if (getRuntime() > macroTime + 0.5) {
//robot.intake.setDcMotor(-0.0); //robot.intake.setDcMotor(-0.0);
robot.intake.setDcMotor(-0.35); robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
@ -192,14 +193,14 @@ public class BlueBackStageAuto extends AutoBase {
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
robot.macroState = 0; robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 60, getRuntime());
macroState++; macroState++;
} }
break; break;
//extending the macro and about to score //extending the macro and about to score
case 9: case 9:
if (robot.macroState != 0) { if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 60, getRuntime());
} }
if (robot.macroState == 0 && !robot.drive.isBusy()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
@ -219,7 +220,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2))); builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
break; break;
case 2: case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2))); builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3)));
break; break;
case 3: case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 ))); builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 )));
@ -241,13 +242,13 @@ public class BlueBackStageAuto extends AutoBase {
//Third and 4th pixels off the stack are intaken by this //Third and 4th pixels off the stack are intaken by this
case 12: case 12:
robot.intake.setDcMotor(0.68); robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK2); robot.intake.setpos(STACK1);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
//goes back to the easel //goes back to the easel
case 13: case 13:
if (getRuntime() > macroTime + 0.03) { if (getRuntime() > macroTime + 0.5) {
robot.intake.setDcMotor(-0.35); robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
@ -273,7 +274,7 @@ public class BlueBackStageAuto extends AutoBase {
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
robot.macroState = 0; robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 140, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
macroState++; macroState++;
} }
break; break;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -77,6 +77,11 @@ public class MainTeleOp extends OpMode {
this.robot.endGameMechs.reset(); this.robot.endGameMechs.reset();
} }
if (controller1.getRightBumper().isPressed()) {
this.robot.endGameMechs.Finger_in();
}else {
this.robot.endGameMechs.Finger_out();
}
//Hang Motor //Hang Motor
// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){ // if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
@ -88,6 +93,7 @@ public class MainTeleOp extends OpMode {
// hang_counter = false; // hang_counter = false;
// } // }
if (controller1.getB().isPressed()) { if (controller1.getB().isPressed()) {
this.robot.endGameMechs.hang_final_pos(); this.robot.endGameMechs.hang_final_pos();
} else { } else {

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java
View File

@ -31,6 +31,8 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigu
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner;
import org.firstinspires.ftc.teamcode.roadrunner.util.AxisDirection;
import org.firstinspires.ftc.teamcode.roadrunner.util.BNO055IMUUtil;
import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil; import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil;
import java.util.ArrayList; import java.util.ArrayList;
@ -101,7 +103,7 @@ public class SampleMecanumDrive extends MecanumDrive {
// and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location // and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
// //
// For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y. // For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
// BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y); BNO055IMUUtil.remapZAxis(imu, AxisDirection.POS_X);
rightRear = hardwareMap.get(DcMotorEx.class, "BackRight"); rightRear = hardwareMap.get(DcMotorEx.class, "BackRight");