things and stuff (stuff and things)
This commit is contained in:
parent
08f8b1a4f3
commit
217460af6b
|
@ -0,0 +1,36 @@
|
|||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
@Autonomous(name = "idk something left")
|
||||
public class AutonLeft extends LinearOpMode {
|
||||
Hardware hardware;
|
||||
MovementAPI movement;
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
hardware = new Hardware(this);
|
||||
movement = new MovementAPI(hardware);
|
||||
|
||||
hardware.wrist.setPosition(0.5);
|
||||
sleep(500);
|
||||
hardware.claw.setPosition(1);
|
||||
|
||||
waitForStart();
|
||||
|
||||
movement.move(0, 1, 0, 0.4);
|
||||
sleep(1000);
|
||||
movement.stop();
|
||||
sleep(1000);
|
||||
hardware.claw.setPosition(0);
|
||||
sleep(1000);
|
||||
movement.move(0, -1, 0, 0.4);
|
||||
sleep(1000);
|
||||
movement.stop();
|
||||
sleep(1000);
|
||||
movement.move(-1, 0, 0, 0.4);
|
||||
sleep(2500);
|
||||
movement.stop();
|
||||
}
|
||||
}
|
|
@ -0,0 +1,36 @@
|
|||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
@Autonomous(name = "idk something right")
|
||||
public class AutonRight extends LinearOpMode {
|
||||
Hardware hardware;
|
||||
MovementAPI movement;
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
hardware = new Hardware(this);
|
||||
movement = new MovementAPI(hardware);
|
||||
|
||||
hardware.wrist.setPosition(0.5);
|
||||
sleep(500);
|
||||
hardware.claw.setPosition(1);
|
||||
|
||||
waitForStart();
|
||||
|
||||
movement.move(0, 1, 0, 0.4);
|
||||
sleep(1000);
|
||||
movement.stop();
|
||||
sleep(1000);
|
||||
hardware.claw.setPosition(0);
|
||||
sleep(1000);
|
||||
movement.move(0, -1, 0, 0.4);
|
||||
sleep(1000);
|
||||
movement.stop();
|
||||
sleep(1000);
|
||||
movement.move(1, 0, 0, 0.4);
|
||||
sleep(2500);
|
||||
movement.stop();
|
||||
}
|
||||
}
|
|
@ -0,0 +1,46 @@
|
|||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
@TeleOp(name = "BBBBBBBBB")
|
||||
public class Gamepad extends OpMode {
|
||||
Hardware hardware;
|
||||
MovementAPI movement;
|
||||
double speed = 0.6;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
hardware = new Hardware(this);
|
||||
movement = new MovementAPI(hardware);
|
||||
|
||||
hardware.claw.setPosition(1);
|
||||
hardware.wrist.setPosition(0.5);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
if (gamepad1.left_bumper) {
|
||||
speed -= 0.005;
|
||||
}
|
||||
|
||||
if (gamepad1.right_bumper) {
|
||||
speed += 0.005;
|
||||
}
|
||||
|
||||
speed = Math.max(0, Math.min(speed, 1));
|
||||
|
||||
hardware.spool.setPower(gamepad2.left_stick_y);
|
||||
hardware.claw.setPosition(1 - gamepad2.left_trigger);
|
||||
hardware.wrist.setPosition((-gamepad2.right_stick_y + 1) / 2);
|
||||
|
||||
movement.move(
|
||||
gamepad1.left_stick_x,
|
||||
-gamepad1.left_stick_y,
|
||||
gamepad1.right_stick_x,
|
||||
speed
|
||||
);
|
||||
|
||||
telemetry.addLine(speed + "");
|
||||
}
|
||||
}
|
|
@ -0,0 +1,59 @@
|
|||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
|
||||
public class Hardware {
|
||||
public final DcMotor fl;
|
||||
public final DcMotor fr;
|
||||
public final DcMotor bl;
|
||||
public final DcMotor br;
|
||||
public final DcMotor spool;
|
||||
public final Servo claw;
|
||||
public final Servo wrist;
|
||||
public final BNO055IMU imu;
|
||||
|
||||
public Hardware(OpMode opMode) {
|
||||
this(opMode.hardwareMap);
|
||||
}
|
||||
|
||||
public Hardware(HardwareMap hardwareMap) {
|
||||
this.fl = hardwareMap.get(DcMotor.class, "fl");
|
||||
this.fr = hardwareMap.get(DcMotor.class, "fr");
|
||||
this.bl = hardwareMap.get(DcMotor.class, "bl");
|
||||
this.br = hardwareMap.get(DcMotor.class, "br");
|
||||
|
||||
this.fl.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
this.bl.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
this.fr.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
this.br.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
this.fl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
this.fr.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
this.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
this.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
this.spool = hardwareMap.get(DcMotor.class, "spool");
|
||||
this.claw = hardwareMap.get(Servo.class, "claw");
|
||||
this.wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
this.imu = hardwareMap.get(BNO055IMU.class, "imu");
|
||||
|
||||
this.wrist.scaleRange(0.25, 1);
|
||||
}
|
||||
|
||||
public void initIMU() {
|
||||
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
||||
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
||||
parameters.calibrationDataFile = "BNO055IMUCalibration.json";
|
||||
|
||||
this.imu.initialize(parameters);
|
||||
this.imu.startAccelerationIntegration(new Position(), new Velocity(), 1);
|
||||
}
|
||||
}
|
|
@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode;
|
|||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple.Direction;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
public class MovementAPI {
|
||||
private final DcMotor fl;
|
||||
|
@ -14,16 +15,20 @@ public class MovementAPI {
|
|||
public DcMotor getBL() { return this.bl; }
|
||||
public DcMotor getBR() { return this.br; }
|
||||
|
||||
public MovementAPI(Hardware hardware) {
|
||||
this(
|
||||
hardware.fl,
|
||||
hardware.fr,
|
||||
hardware.bl,
|
||||
hardware.br
|
||||
);
|
||||
}
|
||||
|
||||
public MovementAPI(DcMotor fl, DcMotor fr, DcMotor bl, DcMotor br) {
|
||||
this.fl = fl;
|
||||
this.fr = fr;
|
||||
this.bl = bl;
|
||||
this.br = br;
|
||||
|
||||
fl.setDirection(Direction.REVERSE);
|
||||
bl.setDirection(Direction.REVERSE);
|
||||
fr.setDirection(Direction.FORWARD);
|
||||
br.setDirection(Direction.FORWARD);
|
||||
}
|
||||
|
||||
public void move(double powerX, double powerY, double turn, double speed) {
|
||||
|
@ -43,4 +48,11 @@ public class MovementAPI {
|
|||
bl.setPower(blPower);
|
||||
br.setPower(brPower);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
fl.setPower(0);
|
||||
fr.setPower(0);
|
||||
bl.setPower(0);
|
||||
br.setPower(0);
|
||||
}
|
||||
}
|
Loading…
Reference in a new issue