TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2024-02-03 14:13:37 -06:00
parent d279848b7a c3ea4c475d
commit 4dc220ecc3
2 changed files with 29 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
View File

@@ -69,6 +69,11 @@ public class SodiPizzaMinibotObject extends Robot {
leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
//Servo Defining
shoulder = engine.hardwareMap.servo.get("shoulder");
gripper = engine.hardwareMap.servo.get("gripper");

40
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
View File

@@ -63,11 +63,15 @@ public class SodiPizzaTeleOPState extends CyberarmState {
engine.telemetry.addData("Arm servo position", robot.shoulder.getPosition());
engine.telemetry.addLine();
engine.telemetry.addData("Approx Object Dist", approxObjPos);
engine.telemetry.addLine();
engine.telemetry.addData("Yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
engine.telemetry.addData("Roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
}
@Override
public void init() {
drivePower = 0;
drivePower = 1;
robot.leftFront.setPower(0);
robot.rightFront.setPower(0);
robot.leftBack.setPower(0);
@@ -78,7 +82,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
robot.imu.resetYaw();
imuInitAngle = robot.imu.getRobotYawPitchRollAngles();
lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
// lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
gp1checker = new GamepadChecker(engine, engine.gamepad1);
gp2checker = new GamepadChecker(engine, engine.gamepad1);
@@ -104,31 +108,35 @@ public class SodiPizzaTeleOPState extends CyberarmState {
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
// double frontLeftPower = (y + x + rx) / denominator;
// double backLeftPower = (y - x + rx) / denominator;
// double frontRightPower = (y - x - rx) / denominator;
// double backRightPower = (y + x - rx) / denominator;
//
// robot.leftFront.setPower(frontLeftPower);
// robot.leftBack.setPower(backLeftPower);
// robot.rightFront.setPower(frontRightPower);
// robot.rightBack.setPower(backRightPower);
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
robot.leftFront.setPower(frontLeftPower);
robot.leftBack.setPower(backLeftPower);
robot.rightFront.setPower(frontRightPower);
robot.rightBack.setPower(backRightPower);
double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double heading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
double rotX = x * Math.cos(heading) - y * Math.sin(heading);
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
frontLeftPower = (rotY + rotX + rx) / denominator;
backLeftPower = (rotY - rotX + rx) / denominator;
frontRightPower = (rotY - rotX - rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
robot.leftBack.setPower(backLeftPower * drivePower);
robot.rightBack.setPower(backRightPower * drivePower);
robot.leftFront.setPower(frontLeftPower * drivePower);
robot.rightFront.setPower(frontRightPower * drivePower);
if (engine.gamepad1.start && !engine.gamepad1.a) {
robot.imu.resetYaw();
}
if (engine.gamepad1.left_stick_button) {
if (System.currentTimeMillis() - lastMoveTime >= 200) {