working drive code arm needs work
This commit is contained in:
robotics1
@ -0,0 +1,336 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots.paths;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
|
||||
|
||||
import com.acmerobotics.roadrunner.SequentialAction;
|
||||
import com.acmerobotics.roadrunner.SleepAction;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
||||
import org.firstinspires.ftc.teamcode.states.FieldStates;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
|
||||
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
|
||||
|
||||
public class CometBotTeleopCompetition {
|
||||
|
||||
/*
|
||||
Subsystems
|
||||
*/
|
||||
private MotorsSubsystem motors;
|
||||
private ClawSubsystem claw;
|
||||
private ArmSubsystem arm;
|
||||
private WristSubsystem wrist;
|
||||
private DualMotorSliderSubsystem lift;
|
||||
|
||||
/*
|
||||
Controllers
|
||||
*/
|
||||
public Gamepad GP1;
|
||||
public Gamepad GP2;
|
||||
public Gamepad currentGP1;
|
||||
public Gamepad previousGP1;
|
||||
public Gamepad currentGP2;
|
||||
public Gamepad previousGP2;
|
||||
|
||||
/*
|
||||
Pedro/FTC Components
|
||||
*/
|
||||
private Follower follower;
|
||||
private Telemetry telemetry;
|
||||
|
||||
/*
|
||||
States
|
||||
*/
|
||||
public FieldStates fieldStates;
|
||||
|
||||
/*
|
||||
Configurations
|
||||
*/
|
||||
public double currentPower = MAX_POWER;
|
||||
|
||||
public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
|
||||
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
|
||||
|
||||
this.GP1 = gp1;
|
||||
this.GP2 = gp2;
|
||||
this.telemetry = telemetry;
|
||||
this.currentGP1 = new Gamepad();
|
||||
this.currentGP2 = new Gamepad();
|
||||
this.previousGP1 = new Gamepad();
|
||||
this.previousGP2 = new Gamepad();
|
||||
this.fieldStates = new FieldStates();
|
||||
this.follower = new Follower(hardwareMap);
|
||||
claw = new ClawSubsystem(hardwareMap);
|
||||
arm = new ArmSubsystem(hardwareMap);
|
||||
wrist = new WristSubsystem(hardwareMap);
|
||||
lift = new DualMotorSliderSubsystem(hardwareMap);
|
||||
}
|
||||
|
||||
public void init() {
|
||||
this.motors.init();
|
||||
claw.init();
|
||||
arm.initTeleOp();
|
||||
lift.init();
|
||||
;
|
||||
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
|
||||
follower.setMaxPower(MAX_POWER);
|
||||
follower.startTeleopDrive();
|
||||
claw.closeClaw();
|
||||
wrist.initTeleOp();
|
||||
}
|
||||
|
||||
public void initCloseClaw(){
|
||||
this.motors.init();
|
||||
this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
|
||||
follower.setMaxPower(MAX_POWER);
|
||||
follower.startTeleopDrive();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
this.previousGP1.copy(currentGP1);
|
||||
this.currentGP1.copy(this.GP1);
|
||||
this.previousGP2.copy(currentGP2);
|
||||
this.currentGP2.copy(this.GP2);
|
||||
// this.moveSkyHook();
|
||||
this.toHighBucketScore();
|
||||
this.toLowBucketScore();
|
||||
this.toArmParkPosition();
|
||||
this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
|
||||
this.clawControl();
|
||||
// this.decreaseMaxPower();
|
||||
// this.increaseMaxPower();
|
||||
|
||||
|
||||
// Actions.runBlocking(this.lift.toFloorPosition());
|
||||
|
||||
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
|
||||
follower.update();
|
||||
lift.update();
|
||||
this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
|
||||
// this.telemetry.addData("Claw State", this.claw.getState());
|
||||
// this.telemetry.addData("Claw Position", this.claw.getPosition());
|
||||
// this.telemetry.addData("Wrist State", this.wrist.getState());
|
||||
// this.telemetry.addData("Arm State", this.arm.getState());
|
||||
// this.telemetry.addData("Lift State", this.lift.getState());
|
||||
// this.telemetry.addData("Lift Position", this.lift.getPosition());
|
||||
this.telemetry.addData("MaxPower", MAX_POWER);
|
||||
}
|
||||
|
||||
/*
|
||||
Type: PS4 / Logitech
|
||||
Controller: 1
|
||||
Button: Left Bumper
|
||||
Assumption: Working motor mechanism
|
||||
Action: Decreases maximum speed by -.05
|
||||
*/
|
||||
// public void decreaseMaxPower() {
|
||||
// if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
|
||||
// this.currentPower = this.currentPower - .05;
|
||||
// this.follower.setMaxPower(this.currentPower);
|
||||
// }
|
||||
// }
|
||||
|
||||
/*
|
||||
Type: PS4 / Logitech
|
||||
Controller: 1
|
||||
Button: Left Bumper
|
||||
Assumption: Working motor mechanism
|
||||
Action: Increases maximum speed by +.05
|
||||
*/
|
||||
// public void increaseMaxPower() {
|
||||
// if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
|
||||
// this.currentPower = this.currentPower + .05;
|
||||
// this.follower.setMaxPower(this.currentPower);
|
||||
// }
|
||||
// }
|
||||
// public void moveSkyHook() {
|
||||
// if (this.currentGP2.dpad_down) {
|
||||
// hook.moveSkyHook(-1.00);
|
||||
// }
|
||||
// else if (this.currentGP2.dpad_up) {
|
||||
// hook.moveSkyHook(1.00);
|
||||
// }
|
||||
// else{
|
||||
// hook.moveSkyHook(0.00);
|
||||
// }
|
||||
//
|
||||
// }
|
||||
|
||||
/*
|
||||
Type: PS4 / Logitech
|
||||
Controller: 2
|
||||
Button: TRIANGLE / Y
|
||||
Assumption: Claw is holding specimen, robot is facing buckets ready to score
|
||||
Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
|
||||
raises to high bucket. Once at high bucket position, move arm forward, wrist forward
|
||||
and open claw to drop specimen into bucket. Finally, put arm back up and wrist back up,
|
||||
retract lift all the way down to floor position and back to TRAVELING state.
|
||||
*/
|
||||
public void toHighBucketScore() {
|
||||
if (this.currentGP2.y && !this.previousGP2.y) {
|
||||
lift.toHighBucketPosition();
|
||||
if (lift.getFixedPosition() >= 4450) {
|
||||
arm.toBucketPosition();
|
||||
if(arm.getPosition() == 0.45)
|
||||
wrist.toBucketPosition();
|
||||
if(wrist.getPosition() == 0.56)
|
||||
claw.openClaw();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void highBucketDrop() {
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
// new SleepAction(.5),
|
||||
// this.lift.toHighBucketPosition(),
|
||||
// new SleepAction(.5),
|
||||
// this.arm.toBucketPosition(),
|
||||
// new SleepAction(.5),
|
||||
// this.wrist.toBucketPosition(),
|
||||
// new SleepAction(.5),
|
||||
// this.claw.openClaw(),
|
||||
// new SleepAction(.5),
|
||||
// this.wrist.toFloorPosition(),
|
||||
// new SleepAction(.5),
|
||||
// this.arm.toParkPosition(),
|
||||
// this.lift.toFloorPosition(),
|
||||
// new SleepAction(.5)
|
||||
));
|
||||
}
|
||||
public void highBucketDropAuto() {
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
// new SleepAction(.1),
|
||||
// this.lift.toHighBucketPosition(),
|
||||
// new SleepAction(.25),
|
||||
// this.arm.toBucketPosition(),
|
||||
// new SleepAction(.25),
|
||||
// this.wrist.toBucketPosition(),
|
||||
// new SleepAction(.25),
|
||||
// this.claw.openClaw(),
|
||||
// new SleepAction(.25),
|
||||
// this.wrist.toFloorPosition(),
|
||||
// new SleepAction(.25),
|
||||
// this.arm.toParkPosition(),
|
||||
// this.lift.toZeroPosition(),
|
||||
// new SleepAction(.25)
|
||||
));
|
||||
}
|
||||
/*
|
||||
Type: PS4 / Logitech
|
||||
Controller: 2
|
||||
Button: CIRCLE / B
|
||||
Assumption: Claw is holding specimen, robot is facing buckets ready to score
|
||||
Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
|
||||
raises to low bucket. Once at low bucket position, move arm forward, wrist forward
|
||||
and open claw to drop specimen into bucket. Finally, put arm back up and wrist back up,
|
||||
retract lift all the way down to floor position and back to TRAVELING state.
|
||||
*/
|
||||
public void toLowBucketScore() {
|
||||
if (this.currentGP2.b && !this.previousGP2.b) {
|
||||
lift.toLowBucketPosition();
|
||||
if (lift.getFixedPosition() >= 1700); {
|
||||
arm.toBucketPosition();
|
||||
if(arm.getPosition() == 0.45)
|
||||
wrist.toBucketPosition();
|
||||
if(wrist.getPosition() == 0.56)
|
||||
claw.openClaw();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Type: PS4 / Logitech
|
||||
Controller: 2
|
||||
Button: RIGHT BUMPER
|
||||
Assumption: Working claw mechanism
|
||||
Action: On button press, claw switches state from OPEN to CLOSE
|
||||
*/
|
||||
public void clawControl() {
|
||||
if (this.currentGP2.right_bumper && !this.previousGP2.right_bumper) {
|
||||
claw.switchState();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
Type: PS4 / Logitech
|
||||
Controller: 2
|
||||
Button: SQUARE / X
|
||||
Assumption: Working arm mechanism
|
||||
Action: On button press, pulls arm up and wrist up, ideal for traveling the field when
|
||||
holding a specimen in claws
|
||||
*/
|
||||
public void toArmParkPosition() {
|
||||
if (this.currentGP2.square && !this.previousGP2.square) {
|
||||
Actions.runBlocking(new SequentialAction(
|
||||
// this.wrist.toFloorPosition(),
|
||||
// this.arm.toParkPosition(),
|
||||
// this.lift.toFloorPosition()
|
||||
));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Type: PS4
|
||||
Controller: 2
|
||||
Button: CROSS / A
|
||||
Assumption: Working claw, arm and wrist mechanisms
|
||||
Action: On button press, if arm is in PARK (toArmParkPosition), drop the arm to SUBMARINE
|
||||
position. SUBMARINE position means the arm and wrist are parallel to the floor, raised
|
||||
3 INCHES off the ground. This state is ideal for moving the arm into the SUBMARINE
|
||||
area of the field.
|
||||
|
||||
When arm is in SUBMARINE position, pressing the button again puts the arm and wrist into
|
||||
FLOOR state. This angles the arm and wrist down so that it is able to pick specimens
|
||||
from within the SUBMARINE floor.
|
||||
*/
|
||||
public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
|
||||
if (this.currentGP2.cross && !previousGP2.cross) {
|
||||
// if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
|
||||
// Actions.runBlocking(
|
||||
// new SequentialAction(
|
||||
// this.arm.toSubmarinePosition(),
|
||||
// this.wrist.toFloorPosition()
|
||||
// )
|
||||
// );
|
||||
// } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) {
|
||||
// Actions.runBlocking(
|
||||
// new SequentialAction(
|
||||
//// this.lift.toFloorPosition(),
|
||||
// this.arm.toSubmarinePosition(),
|
||||
// this.wrist.toPickupPosition()
|
||||
// )
|
||||
// );
|
||||
// } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
|
||||
// Actions.runBlocking(
|
||||
// new SequentialAction(
|
||||
// // this.arm.toSubmarinePosition(),
|
||||
// this.wrist.toFloorPosition()
|
||||
// )
|
||||
// );
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,24 @@
|
||||
package org.firstinspires.ftc.teamcode.cometbots.paths;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
|
||||
@TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition")
|
||||
public class CometBotTeleopDriveV2 extends OpMode {
|
||||
|
||||
public CometBotTeleopCompetition runMode;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
|
||||
this.runMode.init();
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
this.runMode.update();
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user