added turbo mode on driver 1 right bumper, started adding pid stuff and added runtime check to start working on macros. pid and advanced macro in ExperimentalKhangMain and the turbo and variable that keeps track fo time is in KhangMain. Do not mix these two opmodes very bad things will happen that im still smoothing out.
This commit is contained in:
UntitledRice
parent
76d8b67c01
commit
252ba82597
|
|
@ -0,0 +1,259 @@
|
|||
package org.firstinspires.ftc.teamcode.opmode.teleop;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.controller.Controller;
|
||||
import org.firstinspires.ftc.teamcode.hardware.ArmPos;
|
||||
import org.firstinspires.ftc.teamcode.hardware.DoorPos;
|
||||
import org.firstinspires.ftc.teamcode.hardware.Down;
|
||||
import org.firstinspires.ftc.teamcode.hardware.Hieght;
|
||||
import org.firstinspires.ftc.teamcode.hardware.HopperPos;
|
||||
import org.firstinspires.ftc.teamcode.hardware.Intake;
|
||||
import org.firstinspires.ftc.teamcode.hardware.Robot;
|
||||
import org.firstinspires.ftc.teamcode.hardware.SlidePos;
|
||||
|
||||
@Config
|
||||
@TeleOp(name = "experimental opmode", group = "OpModes")
|
||||
public class ExperimentalKhangMain extends OpMode {
|
||||
|
||||
//keep track of runtime for advanced macros
|
||||
private ElapsedTime runTime = new ElapsedTime();
|
||||
|
||||
//create and set start position of intake
|
||||
private Intake.Position Currentpos = Intake.Position.UP;
|
||||
//inform if slides are in lowest position or not
|
||||
private Down.DownArm DownQuestion = Down.DownArm.YES;
|
||||
//create and set start position of slides
|
||||
private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
|
||||
//create and set start position of arm
|
||||
private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN;
|
||||
//create and set start position of door, default close
|
||||
private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
|
||||
//create and set start position of hopper
|
||||
private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
|
||||
//create manual slide height varable and set it to hold position as start
|
||||
private Hieght.height CurrentHeight = Hieght.height.HOLD;
|
||||
|
||||
//robot position stuff
|
||||
Pose2d robot_pos;
|
||||
double robot_x, robot_y, robot_heading;
|
||||
|
||||
//create robot instance
|
||||
private Robot robot;
|
||||
//create servo for plane
|
||||
private Servo planeServo;
|
||||
//create controller 1 (driver)
|
||||
private Controller controller1;
|
||||
//create controller 2 (macro user and one with hard life)
|
||||
private Controller controller2;
|
||||
//set starting slide height to 0
|
||||
private double sHeight = 0;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
//reset runtime when opmode is initialized
|
||||
runTime.reset();
|
||||
|
||||
//make each servo, motor, and controller and configure them
|
||||
planeServo = hardwareMap.get(Servo.class, "Plane Servo");
|
||||
this.robot = new Robot(hardwareMap);
|
||||
controller1 = new Controller(gamepad1);
|
||||
controller2 = new Controller(gamepad2);
|
||||
//feedback to driver hub to know if init was successful
|
||||
telemetry.addLine("Started");
|
||||
//update to amke sure we receive last lien of code
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
|
||||
// robot position update
|
||||
robot_pos = robot.drive.getPoseEstimate();
|
||||
robot_x = robot_pos.getX();
|
||||
robot_y = robot_pos.getY();
|
||||
robot_heading = robot_pos.getHeading(); // in radians
|
||||
|
||||
// Calculate the runtime in seconds
|
||||
double currentTime = runTime.seconds();
|
||||
|
||||
//set start of macro runtime
|
||||
double macroStartTime = 0;
|
||||
|
||||
//create and set X, Y, and Z for drive inputs
|
||||
double x = -gamepad1.left_stick_y;
|
||||
double y = -gamepad1.left_stick_x;
|
||||
double z = -gamepad1.right_stick_x;
|
||||
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
|
||||
|
||||
//set intake to be pressure reactant to right trigger
|
||||
robot.intake.setDcMotor(gamepad2.right_trigger);
|
||||
//manual slide height control
|
||||
double sHeight = gamepad2.right_stick_y;
|
||||
//activate intake or not
|
||||
double intakeON = gamepad2.right_trigger;
|
||||
//graph input of Y joystick to make macros for slides
|
||||
double sY = -gamepad2.left_stick_y;
|
||||
//graph of X
|
||||
double sX = gamepad2.left_stick_x;
|
||||
|
||||
//reset value to set slides to starting value
|
||||
double reset = gamepad2.left_trigger;
|
||||
|
||||
//variable to determine shoot drone or not
|
||||
double shoot = gamepad1.right_trigger;
|
||||
|
||||
//update variables to constantly feed position each component should be in
|
||||
//intake
|
||||
robot.intake.setpos(Currentpos);
|
||||
//slide macro
|
||||
robot.slides.setSPos(CurrentSpos);
|
||||
//slide manual
|
||||
// robot.slides.setHeight(CurrentHeight);
|
||||
//arm position
|
||||
robot.arm.setPos(CurrentApos);
|
||||
//door open or not
|
||||
robot.arm.openDoor(CurrentDpos);
|
||||
//update
|
||||
controller2.update();
|
||||
|
||||
//manual slide input reader
|
||||
// if (sHeight >= 0.2) {
|
||||
// CurrentHeight = Hieght.height.ASCEND;
|
||||
// } else if (sHeight <= -0.2) {
|
||||
// CurrentHeight = Hieght.height.DESCEND;
|
||||
// } else {
|
||||
// CurrentHeight = Hieght.height.HOLD;
|
||||
// }
|
||||
|
||||
//read values to determine if plane should shoot or not
|
||||
if (shoot >= 0.9) {
|
||||
planeServo.setPosition(0.2);
|
||||
} else {
|
||||
planeServo.setPosition(0);
|
||||
}
|
||||
|
||||
//make door rise as intake goes on
|
||||
if (intakeON >= 0.01) {
|
||||
CurrentDpos = DoorPos.DoorPosition.OPEN;
|
||||
} else {
|
||||
CurrentDpos = DoorPos.DoorPosition.CLOSE;
|
||||
}
|
||||
|
||||
if (intakeON >= 0.35) {
|
||||
Currentpos = Intake.Position.STACK1;
|
||||
} else {
|
||||
Currentpos = Intake.Position.UP;
|
||||
}
|
||||
|
||||
//reset slide position
|
||||
if (reset >= 0.2) {
|
||||
CurrentSpos = CurrentSpos.DOWN;
|
||||
}
|
||||
|
||||
// //control position of hopper
|
||||
// if (controller2.getLeftBumper().isJustPressed()) {
|
||||
// hopperpos = HopperPos.hopperPos.UP;
|
||||
// } else if (controller2.getLeftBumper().isJustReleased()) {
|
||||
// hopperpos = HopperPos.hopperPos.DOWN;
|
||||
// }
|
||||
|
||||
//shift intake up one pixel
|
||||
if (controller2.getDLeft().isJustPressed()) {
|
||||
//prevent from going higher than highest legal value
|
||||
if (Currentpos != Intake.Position.UP) {
|
||||
Currentpos = Currentpos.nextPosition();
|
||||
}
|
||||
}
|
||||
|
||||
//shift intake down one pixel
|
||||
if (controller2.getDRight().isJustPressed()) {
|
||||
//prevent from going lower than lowest value
|
||||
if (Currentpos != Intake.Position.STACK1) {
|
||||
Currentpos = Currentpos.previousPosition();
|
||||
}
|
||||
}
|
||||
|
||||
//set intake to max position
|
||||
if (controller2.getDUp().isJustPressed()) {
|
||||
Currentpos = Currentpos.UP;
|
||||
}
|
||||
|
||||
//set intake to lowest position
|
||||
if (controller2.getDDown().isJustPressed()) {
|
||||
Currentpos = Currentpos.STACK1;
|
||||
}
|
||||
|
||||
// //arm safety pause going up
|
||||
// if (controller2.getA().isJustPressed()){
|
||||
// CurrentApos = CurrentApos.SAFTEYUP;
|
||||
// DownQuestion = DownQuestion.NO;
|
||||
// }
|
||||
|
||||
if (controller2.getA().isJustPressed()) {
|
||||
macroStartTime = currentTime;
|
||||
CurrentSpos = CurrentSpos.TAPE1;
|
||||
if (macroStartTime + 1000 <= currentTime) {
|
||||
CurrentApos = CurrentApos.SCORE;
|
||||
}
|
||||
}
|
||||
|
||||
//arm safety pause going down
|
||||
if (controller2.getX().isJustPressed()) {
|
||||
CurrentApos = CurrentApos.SAFTEYDOWN;
|
||||
DownQuestion = DownQuestion.NO;
|
||||
}
|
||||
|
||||
//arm all the way up
|
||||
if (controller2.getB().isJustPressed()) {
|
||||
CurrentApos = CurrentApos.SCORE;
|
||||
DownQuestion = DownQuestion.NO;
|
||||
}
|
||||
|
||||
//arm all the way down
|
||||
if (controller2.getY().isJustPressed()) {
|
||||
CurrentApos = CurrentApos.SDOWN;
|
||||
DownQuestion = DownQuestion.YES;
|
||||
}
|
||||
|
||||
//set slides to max when left joystick is up
|
||||
if (sY >= 0.5) {
|
||||
CurrentSpos = CurrentSpos.MAX;
|
||||
}
|
||||
|
||||
//set slides to tape 1 level when left joystick is down
|
||||
if (sY <= -0.5) {
|
||||
CurrentSpos = CurrentSpos.TAPE1;
|
||||
}
|
||||
|
||||
//set slides to tape 3 when left joystick is right
|
||||
if (sX >= 0.2) {
|
||||
CurrentSpos = CurrentSpos.TAPE3;
|
||||
}
|
||||
|
||||
//set slides to tape 2 when left joystick is left
|
||||
if (sX <= -0.2) {
|
||||
CurrentSpos = CurrentSpos.TAPE2;
|
||||
}
|
||||
|
||||
//set slides all the way down when left bumper gets pressed
|
||||
if (controller2.getLeftBumper().isJustPressed()) {
|
||||
CurrentSpos = CurrentSpos.DOWN;
|
||||
}
|
||||
|
||||
//set intake all teh way up when right bumper is pressed
|
||||
if (controller2.getRightBumper().isJustPressed()) {
|
||||
Currentpos = Currentpos.UP;
|
||||
}
|
||||
|
||||
// update the runtime on the telemetry
|
||||
telemetry.addData("Runtime", currentTime);
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
|
|
@ -20,6 +20,10 @@ import com.qualcomm.robotcore.util.ElapsedTime;
|
|||
@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
|
||||
public class KhangMain extends OpMode {
|
||||
|
||||
//turbo mode
|
||||
public static double normal = 0.5;
|
||||
public static double turbo = 1;
|
||||
|
||||
//keep track of runtime for advanced macros
|
||||
private ElapsedTime runTime = new ElapsedTime();
|
||||
|
||||
|
|
@ -80,6 +84,17 @@ public class KhangMain extends OpMode {
|
|||
double z = -gamepad1.right_stick_x;
|
||||
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
|
||||
|
||||
//turbo activation
|
||||
if (controller1.getRightBumper().isJustPressed()){
|
||||
x *= turbo;
|
||||
y *= turbo;
|
||||
z *= turbo;
|
||||
} else {
|
||||
x *= normal;
|
||||
y *= normal;
|
||||
z *= normal;
|
||||
}
|
||||
|
||||
//set intake to be pressure reactant to right trigger
|
||||
robot.intake.setDcMotor(gamepad2.right_trigger);
|
||||
//manual slide height control
|
||||
|
|
|
|||
Loading…
Reference in New Issue