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

Minibot teleop basic forward drive

Browse Source
This commit is contained in:
NerdyBirdy460
2023-10-31 16:12:43 -05:00
parent d3b6ac873e
commit 22c4c5149e
4 changed files with 183 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java Normal file
View File

@@ -0,0 +1,58 @@
package org.timecrafters.CenterStage.Common;
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import org.timecrafters.Library.Robot;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import dev.cyberarm.engine.V2.CyberarmEngine;
public class MinibotTeleOPBot extends Robot {
public HardwareMap hardwareMap;
public MotorEx flDrive, frDrive, blDrive, brDrive;
public IMU imu;
private String string;
private CyberarmEngine engine;
public TimeCraftersConfiguration configuration;
public MinibotTeleOPBot() {
}
@Override
public void setup() {
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
this.engine = CyberarmEngine.instance;
imu = engine.hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
//Motors
frDrive = new MotorEx(hardwareMap, "FrontRight");
flDrive = new MotorEx(hardwareMap, "FrontLeft");
brDrive = new MotorEx(hardwareMap, "BackRight");
blDrive = new MotorEx(hardwareMap, "BackLeft");
flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}

35
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java
View File

@@ -25,8 +25,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine;
public class ProtoBotSodi extends Robot {
public HardwareMap hardwareMap;
public MotorEx flDrive, frDrive, blDrive, brDrive /*liftMotor*/;
// public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw;
public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor;
public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw;
private String string;
private CyberarmEngine engine;
@@ -50,18 +50,19 @@ public class ProtoBotSodi extends Robot {
flDrive = new MotorEx(hardwareMap, "FrontLeft");
brDrive = new MotorEx(hardwareMap, "BackRight");
blDrive = new MotorEx(hardwareMap, "BackLeft");
// liftMotor = new MotorEx(hardwareMap, "Lift");
liftMotor = new MotorEx(hardwareMap, "Lift");
flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftMotor.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
flDrive.motor.setDirection(FORWARD);
// frDrive.motor.setDirection(REVERSE);
@@ -69,19 +70,19 @@ public class ProtoBotSodi extends Robot {
// brDrive.motor.setDirection(REVERSE);
//Servos
// grabJaw = hardwareMap.servo.get("GrabJaw");
// grabElbow = hardwareMap.servo.get("GrabElbow");
// grabShoulder = hardwareMap.servo.get("GrabShoulder");
// dropShoulder = hardwareMap.servo.get("DropShoulder");
// dropElbow = hardwareMap.servo.get("DropElbow");
// dropJaw = hardwareMap.servo.get("DropJaw");
//
// grabElbow.setDirection(Servo.Direction.FORWARD);
// grabJaw.setDirection(Servo.Direction.FORWARD);
// grabShoulder.setDirection(Servo.Direction.FORWARD);
// dropShoulder.setDirection(Servo.Direction.FORWARD);
// dropElbow.setDirection(Servo.Direction.FORWARD);
// dropJaw.setDirection(Servo.Direction.FORWARD);
grabJaw = hardwareMap.servo.get("GrabJaw");
grabElbow = hardwareMap.servo.get("GrabElbow");
grabShoulder = hardwareMap.servo.get("GrabShoulder");
dropShoulder = hardwareMap.servo.get("DropShoulder");
dropElbow = hardwareMap.servo.get("DropElbow");
dropJaw = hardwareMap.servo.get("DropJaw");
grabElbow.setDirection(Servo.Direction.FORWARD);
grabJaw.setDirection(Servo.Direction.FORWARD);
grabShoulder.setDirection(Servo.Direction.FORWARD);
dropShoulder.setDirection(Servo.Direction.FORWARD);
dropElbow.setDirection(Servo.Direction.FORWARD);
dropJaw.setDirection(Servo.Direction.FORWARD);
}

21
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java Normal file
View File

@@ -0,0 +1,21 @@
package org.timecrafters.CenterStage.TeleOp.Engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.timecrafters.CenterStage.Common.MinibotTeleOPBot;
import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP;
import dev.cyberarm.engine.V2.CyberarmEngine;
@TeleOp(name = "A Yellow Minibot")
public class MiniBotTeleOPEngine extends CyberarmEngine {
private MinibotTeleOPBot robot;
@Override
public void setup() {
this.robot = new MinibotTeleOPBot();
this.robot.setup();
addState(new YellowMinibotTeleOP(robot));
}
}

86
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java Normal file
View File

@@ -0,0 +1,86 @@
package org.timecrafters.CenterStage.TeleOp.States;
import dev.cyberarm.engine.V2.CyberarmState;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import dev.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.timecrafters.CenterStage.Common.MinibotTeleOPBot;
public class YellowMinibotTeleOP extends CyberarmState {
private final MinibotTeleOPBot robot;
public float angleDelta, drivePower;
YawPitchRollAngles imuInitAngle;
private GamepadChecker gamepad1Checker, gamepad2Checker;
public YellowMinibotTeleOP(MinibotTeleOPBot robot) {
this.robot = robot;
}
public float getAngleDelta() {
robot.imu.getRobotYawPitchRollAngles();
return angleDelta;
}
@Override
public void telemetry() {
engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity());
engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity());
engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity());
engine.telemetry.addData("Back Right Velocity", robot.brDrive.getVelocity());
engine.telemetry.addData("Front Left Power", robot.flDrive.motor.getPower());
engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower());
engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower());
engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower());
engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition());
engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles());
}
@Override
public void init() {
robot.flDrive.motor.setPower(0);
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
robot.imu.resetYaw();
imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
}
@Override
public void exec() {
if (engine.gamepad1.right_stick_y < 0.1) {
drivePower = 0;
}
if (engine.gamepad1.start && !engine.gamepad1.a) {
robot.flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.imu.resetYaw();
}
if (engine.gamepad1.right_stick_y > 0.1) {
drivePower = engine.gamepad1.right_stick_y;
robot.flDrive.motor.setPower(drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(drivePower);
robot.brDrive.motor.setPower(drivePower);
}
}
}