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

Compensating for shifted init

Browse Source
This commit is contained in:
Sodi
2022-11-17 20:33:44 -06:00
parent 89628c8fd9
commit b5d77f6b79
2 changed files with 64 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java
View File

@@ -1,6 +1,8 @@
package org.timecrafters.testing.states;
import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cColorSensor;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -46,6 +48,8 @@ public class PhoenixBot1 {
public TimeCraftersConfiguration configuration;
public AdafruitI2cColorSensor AdafruitEncoder;
public PhoenixBot1(CyberarmEngine engine) {
this.engine = engine;
@@ -71,6 +75,7 @@ public class PhoenixBot1 {
imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
configuration = new TimeCraftersConfiguration("Phoenix");
AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit");
//motors configuration
frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");
@@ -155,4 +160,5 @@ public class PhoenixBot1 {
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
}
}
}

114
TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java
View File

@@ -172,9 +172,9 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.dpad_left) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 135;
RotationTarget = -45;
CalculateDeltaRotation();
if (RobotRotation > -45 && RobotRotation < 134) {//CCW
if (RobotRotation > -45 && RobotRotation <= 135) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
@@ -188,7 +188,7 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < 136 && RobotRotation > 134) {
if (RobotRotation < -44 && RobotRotation > -46) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
@@ -198,6 +198,60 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -90;
CalculateDeltaRotation();
if (RobotRotation < 90 && RobotRotation < -89) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > 90 || RobotRotation < -91) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > -91 && RobotRotation < -89) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.dpad_right) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 45;
CalculateDeltaRotation();
if (RobotRotation > -135 && RobotRotation < 44) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -135 || RobotRotation < 46) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < 46 && RobotRotation > 44) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 90;
CalculateDeltaRotation();
@@ -224,60 +278,6 @@ public class PhoenixTeleOPState extends CyberarmState {
}
}
if (engine.gamepad1.dpad_right) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -135;
CalculateDeltaRotation();
if (RobotRotation < 45 && RobotRotation > -134) {//CCW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > 45 || RobotRotation < -136) {//CW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -134 && RobotRotation > -136) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -90;
CalculateDeltaRotation();
if (RobotRotation < 90 && RobotRotation > -89) {//CCW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > 90 || RobotRotation < -91) {//CW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -89 && RobotRotation > -91) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
if (engine.gamepad2.dpad_left) {
robot.collectorLeft.setPower(-1);
robot.collectorRight.setPower(-1);