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 drive, veer balancing added

Browse Source
This commit is contained in:
NerdyBirdy460
2023-11-07 12:05:47 -06:00
parent c485470294
commit 8b88d401a9
7 changed files with 88 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java Normal file
View File

@@ -0,0 +1,13 @@
package org.timecrafters.CenterStage.Autonomous.States;
import dev.cyberarm.engine.V2.CyberarmState;
public class MiniYAutonomousState extends CyberarmState {
@Override
public void exec() {
}
}

17
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java
View File

@@ -3,6 +3,7 @@ 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.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
@@ -17,8 +18,9 @@ public class MiniBTeleOPBot extends Robot {
public HardwareMap hardwareMap;
public MotorEx leftDrive, rightDrive;
public Servo servLowLeft, servLowRight, servTop;
public Servo servLeft, servLow, servTop;
public IMU imu;
public double wheelCircum = 28.888 /* <- Wheel circumference in cm, 11.37 inches */;
public TimeCraftersConfiguration configuration;
@@ -49,9 +51,18 @@ public MiniBTeleOPBot() {
leftDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD);
rightDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD);
//Servos
servLowLeft = hardwareMap.servo.get("ServoLowLeft");
servLowRight = hardwareMap.servo.get("ServoLowRight");
servLeft = hardwareMap.servo.get("ServoLeft");
servLow = hardwareMap.servo.get("ServoLow");
servTop = hardwareMap.servo.get("ServoTop");
servLeft.setDirection(Servo.Direction.FORWARD);
servLow.setDirection(Servo.Direction.FORWARD);
servTop.setDirection(Servo.Direction.FORWARD);
}
}

2
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java
View File

@@ -18,6 +18,7 @@ public class MiniYTeleOPBot extends Robot {
public MotorEx flDrive, frDrive, blDrive, brDrive;
public IMU imu;
private String string;
public double wheelCircum = 15.9/* <- Wheel circumference in cm, 6.26 inches, meaning it would take 3.83 wheel revolutions to move 2 feet */;
public TimeCraftersConfiguration configuration;
@@ -37,6 +38,7 @@ public class MiniYTeleOPBot extends Robot {
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
imu.resetYaw();
//Motors
frDrive = new MotorEx(hardwareMap, "FrontRight");

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

@@ -41,7 +41,7 @@ public class ProtoBotSodi extends Robot {
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
CyberarmEngine engine = CyberarmEngine.instance;
// TimeCraftersConfiguration configuration = new TimeCraftersConfiguration();
configuration = new TimeCraftersConfiguration("Blue Crab");
//Motors
frDrive = new MotorEx(hardwareMap, "FrontRight");

2
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java
View File

@@ -17,6 +17,6 @@ public class MiniBTeleOPEngine extends CyberarmEngine {
this.robot = new MiniBTeleOPBot();
this.robot.setup();
addState(new BlackMiniTeleOP());
addState(new BlackMiniTeleOP(robot));
}
}

26
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java
View File

@@ -1,5 +1,6 @@
package org.timecrafters.CenterStage.TeleOp.States;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.CenterStage.Common.MiniBTeleOPBot;
import dev.cyberarm.engine.V2.CyberarmState;
@@ -9,9 +10,13 @@ public class BlackMiniTeleOP extends CyberarmState {
public BlackMiniTeleOP(MiniBTeleOPBot robot) {this.robot = robot;}
private double rPower, lPower;
private double rPower, lPower, servoPower;
private boolean critTipPoint;
public BlackMiniTeleOP() {
public BlackMiniTeleOP() {}
@Override
public void telemetry() {
}
@Override
@@ -20,22 +25,25 @@ public class BlackMiniTeleOP extends CyberarmState {
lPower = 0;
robot.rightDrive.motor.setPower(rPower);
robot.leftDrive.motor.setPower(lPower);
}
@Override
public void exec() {
if (engine.gamepad1.right_trigger > 0.1) {
rPower = engine.gamepad1.right_trigger;
if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
rPower = engine.gamepad1.right_stick_y;
robot.rightDrive.motor.setPower(rPower);
} else {
rPower = 0;
robot.rightDrive.motor.setPower(rPower);
}
if (engine.gamepad1.left_trigger > 0.1) {
lPower = engine.gamepad1.left_trigger;
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
lPower = engine.gamepad1.left_stick_y;
robot.leftDrive.motor.setPower(lPower);
} else {
lPower = 0;
robot.leftDrive.motor.setPower(lPower);
}
}
}

47
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java
View File

@@ -14,6 +14,7 @@ import org.timecrafters.CenterStage.Common.MiniYTeleOPBot;
public class YellowMinibotTeleOP extends CyberarmState {
private final MiniYTeleOPBot robot;
public float angleDelta, drivePower;
public double lastToldAngle /** <- The angle the bot was last told to stop at **/;
YawPitchRollAngles imuInitAngle;
public YellowMinibotTeleOP(MiniYTeleOPBot robot) {
@@ -52,6 +53,7 @@ public class YellowMinibotTeleOP extends CyberarmState {
robot.imu.resetYaw();
imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1);
GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
@@ -60,15 +62,40 @@ public class YellowMinibotTeleOP extends CyberarmState {
@Override
public void exec() {
if (Math.abs(engine.gamepad1.left_stick_y) < 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1) {
if (Math.abs(engine.gamepad1.left_stick_y) < 0.1 &&
Math.abs(engine.gamepad1.left_stick_x) < 0.1 &&
Math.abs(engine.gamepad1.right_stick_x) < 0.1) {
drivePower = 0;
robot.flDrive.motor.setPower(0);
robot.frDrive.motor.setPower(0);
robot.blDrive.motor.setPower(0);
robot.brDrive.motor.setPower(0);
robot.flDrive.motor.setPower(drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(drivePower);
robot.brDrive.motor.setPower(drivePower);
}
if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5 &&
Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
robot.frDrive.motor.setPower(robot.flDrive.motor.getPower() * 0.8);
robot.brDrive.motor.setPower(robot.blDrive.motor.getPower() * 0.8);
} else
if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 &&
Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
robot.flDrive.motor.setPower(robot.frDrive.motor.getPower() * 0.8);
robot.blDrive.motor.setPower(robot.brDrive.motor.getPower() * 0.8);
} else {
robot.flDrive.motor.setPower(drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(drivePower);
robot.brDrive.motor.setPower(drivePower);
}
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);
@@ -76,7 +103,10 @@ public class YellowMinibotTeleOP extends CyberarmState {
robot.imu.resetYaw();
}
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1 &&
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 &&
robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5) {
drivePower = engine.gamepad1.left_stick_y;
robot.flDrive.motor.setPower(drivePower);
robot.frDrive.motor.setPower(drivePower);
@@ -85,6 +115,7 @@ public class YellowMinibotTeleOP extends CyberarmState {
}
if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) {
drivePower = engine.gamepad1.left_stick_x;
robot.flDrive.motor.setPower(-drivePower);
robot.frDrive.motor.setPower(drivePower);
@@ -92,12 +123,14 @@ public class YellowMinibotTeleOP extends CyberarmState {
robot.brDrive.motor.setPower(-drivePower);
}
if (Math.abs(engine.gamepad1.right_stick_x) > 0.1){
if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
drivePower = engine.gamepad1.right_stick_x;
robot.flDrive.motor.setPower(-drivePower);
robot.frDrive.motor.setPower(drivePower);
robot.blDrive.motor.setPower(-drivePower);
robot.brDrive.motor.setPower(drivePower);
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
}
}