mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-12 23:32:36 +00:00
added a blue and red audience side autonomous (it works and has been tested)
This commit is contained in:
SpencerPiha
@@ -0,0 +1,73 @@
|
||||
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 Audience blue", preselectTeleOp = "Competition V1 TeleOp")
|
||||
|
||||
public class StateAudienceBlue 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", "9-00-0"));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
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 Audience red", preselectTeleOp = "Competition V1 TeleOp")
|
||||
|
||||
public class StateAudienceRed 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 blue");
|
||||
addTask(new DriveToCoordinatesTask(robot));
|
||||
addTask(new OdometryLocalizerTask(robot));
|
||||
|
||||
this.robot.setup();
|
||||
|
||||
addState(new CameraVisionState(robot));
|
||||
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "1-00-0"));
|
||||
|
||||
// drive to the left, center, or right spike mark
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
|
||||
|
||||
// bring arm to hover
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
|
||||
|
||||
//open claw
|
||||
addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
|
||||
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0"));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -13,7 +13,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
@Autonomous(name = "State BackDrop red", preselectTeleOp = "Competition V1 TeleOp")
|
||||
@Autonomous(name = "State BackDrop blue", preselectTeleOp = "Competition V1 TeleOp")
|
||||
|
||||
public class StateBackDropBlue extends CyberarmEngine {
|
||||
|
||||
@@ -34,7 +34,7 @@ public class StateBackDropBlue extends CyberarmEngine {
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new CompetitionRobotV1("State BackDrop red");
|
||||
this.robot = new CompetitionRobotV1("State BackDrop blue");
|
||||
addTask(new DriveToCoordinatesTask(robot));
|
||||
addTask(new OdometryLocalizerTask(robot));
|
||||
|
||||
@@ -42,55 +42,55 @@ public class StateBackDropBlue extends CyberarmEngine {
|
||||
|
||||
addState(new CameraVisionState(robot));
|
||||
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "1-00-0"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "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"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-1"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-03-2"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-02-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "2-01-0"));
|
||||
|
||||
// bring arm to hover
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "3-00-0"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "3-00-0"));
|
||||
|
||||
//open claw
|
||||
addState(new ClawFingerState(robot,"State BackDrop red", "4-00-0"));
|
||||
addState(new ClawFingerState(robot,"State BackDrop blue", "4-00-0"));
|
||||
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "5-00-0"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "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"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-1"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-1"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-1"));
|
||||
|
||||
// pause
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "6-00-0"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "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"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-01-2"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-02-2"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "6-03-2"));
|
||||
|
||||
// pause
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "6-00-1"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "6-00-1"));
|
||||
|
||||
//open right close left
|
||||
addState(new ClawFingerState(robot,"State BackDrop red", "7-00-0"));
|
||||
addState(new ClawFingerState(robot,"State BackDrop blue", "7-00-0"));
|
||||
|
||||
// bring arm up
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "8-00-0"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "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 DriveToCoordinatesState(robot,"State BackDrop blue", "9-01-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-02-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-0"));
|
||||
addState(new DriveToCoordinatesState(robot,"State BackDrop blue", "9-03-1"));
|
||||
|
||||
addState(new ClawArmState(robot,"State BackDrop red", "9-00-0"));
|
||||
addState(new ClawArmState(robot,"State BackDrop blue", "9-00-0"));
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -24,8 +24,8 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
public int target = 0;
|
||||
|
||||
// ---------------------------------------------------------------------------------------------------------------------shoot variables:
|
||||
public static double releasePos = 0.95;
|
||||
public static double holdPos = 0.55 ;
|
||||
public static double releasePos = 0.45;
|
||||
public static double holdPos = 0.95 ;
|
||||
|
||||
|
||||
public double maxVelocityX;
|
||||
@@ -71,7 +71,7 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
// ---------------------------------------------------------------------------------------------------------------Arm control Variables:
|
||||
public String armPos = "collect";
|
||||
// chin up servo
|
||||
public static double chinUpServoUp = 0.7;
|
||||
public static double chinUpServoUp = 0.55;
|
||||
public static double chinUpServoDown = 0;
|
||||
public long lastExecutedTime;
|
||||
|
||||
@@ -299,9 +299,8 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
}
|
||||
|
||||
if (Objects.equals(armPos, "lift up")) {
|
||||
robot.shoulder.setPosition(robot.shoulderCollect);
|
||||
robot.elbow.setPosition(robot.elbowDeposit);
|
||||
target = 120;
|
||||
robot.shoulder.setPosition(0.2);
|
||||
target = 850;
|
||||
robot.chinUpServo.setPosition(chinUpServoUp);
|
||||
}
|
||||
|
||||
@@ -311,9 +310,8 @@ public class CompetitionTeleOpState extends CyberarmState {
|
||||
robot.chinUpServo.setPosition(chinUpServoDown);
|
||||
} else {
|
||||
robot.lift.setPower(0);
|
||||
robot.chinUpServo.setPosition(chinUpServoDown);
|
||||
robot.shoulder.setPosition(robot.shoulderCollect);
|
||||
target = 120;
|
||||
robot.shoulder.setPosition(0.2);
|
||||
target = 850;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user