mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-12 23:32:36 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
@@ -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"));
|
||||
|
||||
@@ -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"));
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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"));
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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"));
|
||||
|
||||
//
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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"));
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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();}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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();}
|
||||
}
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user