TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 08:42:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

IMU telemetry, Linear Slide, shoulder, elbow, depositor movements.

Browse Source
This commit is contained in:
SpencerPiha
2023-10-03 20:30:11 -05:00
parent f7a04f298c
commit d4b2496a02
2 changed files with 66 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
View File

@@ -2,12 +2,13 @@ package org.timecrafters.CenterStage.Common;
import com.arcrobotics.ftclib.drivebase.HDrive;
import com.arcrobotics.ftclib.hardware.RevIMU;
import com.arcrobotics.ftclib.hardware.ServoEx;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.timecrafters.Library.Robot;
@@ -17,9 +18,9 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
public class PrototypeRobot extends Robot {
private HardwareMap hardwareMap;
public MotorEx frontLeft, frontRight, backLeft, backRight;
private RevIMU imu;
public Servo depositorFlip, depositor;
public MotorEx frontLeft, frontRight, backLeft, backRight, lift;
public IMU imu;
public Servo depositorShoulder, depositorElbow, depositor;
private HDrive xDrive;
private String string;
private CyberarmEngine engine;
@@ -34,31 +35,39 @@ public class PrototypeRobot extends Robot {
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
this.engine = CyberarmEngine.instance;
imu = engine.hardwareMap.get(IMU.class, "imu");
//MOTORS
frontRight = new MotorEx(hardwareMap, "frontRight");
frontLeft = new MotorEx(hardwareMap, "frontLeft");
backRight = new MotorEx(hardwareMap, "backRight");
backLeft = new MotorEx(hardwareMap, "backLeft");
lift = new MotorEx(hardwareMap, "lift");
frontRight.motor.setDirection(DcMotorSimple.Direction.FORWARD);
backRight.motor.setDirection(DcMotorSimple.Direction.FORWARD);
backLeft.motor.setDirection(DcMotorSimple.Direction.FORWARD);
frontLeft.motor.setDirection(DcMotorSimple.Direction.FORWARD);
lift.motor.setDirection(DcMotorSimple.Direction.FORWARD);
frontRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
lift.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
//IMU
imu = new RevIMU(hardwareMap, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
imu.init(parameters);
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
//SERVO
depositorFlip = hardwareMap.servo.get("depositorFlip");
depositorShoulder = hardwareMap.servo.get("depositor_shoulder");
depositorElbow = hardwareMap.servo.get("depositor_elbow");
depositor = hardwareMap.servo.get("depositor");
// input motors exactly as shown below
@@ -70,9 +79,4 @@ public class PrototypeRobot extends Robot {
public void driveTrainTeleOp() {
xDrive.driveRobotCentric(-engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, -engine.gamepad1.right_stick_x);
}
public double heading() {
return imu.getHeading();
}
}

52
TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
View File

@@ -1,11 +1,14 @@
package org.timecrafters.CenterStage.States;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.CenterStage.Common.PrototypeRobot;
import dev.cyberarm.engine.V2.CyberarmState;
public class PrototypeRobotDrivetrainState extends CyberarmState {
private PrototypeRobot robot;
private int maxExtension = 2000;
private int minExtension = 0;
public PrototypeRobotDrivetrainState(PrototypeRobot robot) {
this.robot = robot;
@@ -13,19 +16,58 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
@Override
public void exec() {
// drivetrain
robot.driveTrainTeleOp();
// flip arms
if (engine.gamepad1.x) {
robot.depositorShoulder.setPosition(0.75);
} else if (engine.gamepad1.a) {
robot.depositorShoulder.setPosition(0.05);
}
if (engine.gamepad1.y){
robot.depositorFlip.setPosition(0.75);
} else if (engine.gamepad1.a){
robot.depositorFlip.setPosition(0.05);
robot.depositorElbow.setPosition(0.33);
} else if (engine.gamepad1.b){
robot.depositorElbow.setPosition(0);
}
// depositor
if (engine.gamepad1.b){
if (engine.gamepad1.right_bumper) {
robot.depositor.setPosition(0.8);
} else if (engine.gamepad1.x){
} else if (engine.gamepad1.left_bumper) {
robot.depositor.setPosition(0.2);
}
// lift
if (engine.gamepad1.right_trigger != 0){
if (robot.lift.getCurrentPosition() >= maxExtension){
robot.lift.motor.setPower(0);
} else if (robot.lift.getCurrentPosition() >= maxExtension - 200){
robot.lift.motor.setPower(0.35);
}else {
robot.lift.motor.setPower(engine.gamepad1.right_trigger);
}
} else if (engine.gamepad1.left_trigger != 0){
if (robot.lift.getCurrentPosition() <= minExtension) {
robot.lift.motor.setPower(0);
} else if (robot.lift.getCurrentPosition() < 350){
robot.lift.motor.setPower(-0.3);
}else {
robot.lift.motor.setPower(-engine.gamepad1.left_trigger);
}
} else {
robot.lift.motor.setPower(0);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Lift Encoder Pos", robot.lift.motor.getCurrentPosition());
engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
}
}