TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

auto work right side done

Browse Source
This commit is contained in:
SpencerPiha
2024-02-13 20:31:48 -06:00
parent c12c813a14
commit 255f75e3d4
4 changed files with 124 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropBlue.java Normal file
View File

@@ -0,0 +1,106 @@
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.CameraVisionState;
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 = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
public class StateBackDropBlue 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("State BackDrop red");
addTask(new DriveToCoordinatesTask(robot));
addTask(new OdometryLocalizerTask(robot));
this.robot.setup();
addState(new CameraVisionState(robot));
addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
// drive to the left, center, or right spike mark
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "2-03-0"));
// bring arm to hover
addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
//open claw
addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
// drive towards backboard
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
// drive into board
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
//open right close left
addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
// bring arm up
addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
// drivw to park
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
// 65, on the left, 235 on the right
}
}

14
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/StateBackDropRed.java
View File

@@ -64,6 +64,8 @@ public class StateBackDropRed extends CyberarmEngine {
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-1"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-1"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
@@ -71,15 +73,27 @@ public class StateBackDropRed extends CyberarmEngine {
// drive into board
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-01-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-02-2"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "6-03-2"));
// pause
addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
//open right close left
addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
// bring arm up
addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
// drivw to park
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-01-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-02-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-0"));
addState(new DriveToCoordinatesState(robot,"State BackDrop red", "9-03-1"));
addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));

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

@@ -69,7 +69,7 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) < 2) {
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
setHasFinished(true);
}
}
@@ -80,7 +80,7 @@ public class DriveToCoordinatesState extends CyberarmState {
if (Math.abs(robot.positionX - robot.xTarget) < 5
&& Math.abs(robot.positionY - robot.yTarget) < 5
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - Math.toDegrees(robot.hTarget)) < 2) {
&& Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) - Math.abs(Math.toDegrees(robot.hTarget)) < 5) {
setHasFinished(true);
}
}

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

@@ -71,8 +71,8 @@ public class CompetitionTeleOpState extends CyberarmState {
// ---------------------------------------------------------------------------------------------------------------Arm control Variables:
public String armPos = "collect";
// chin up servo
public static double chinUpServoUp = 0.58;
public static double chinUpServoDown = 1;
public static double chinUpServoUp = 0.7;
public static double chinUpServoDown = 0;
public long lastExecutedTime;