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

Compare commits

base: TimeCrafters:c4b404ee199ad4a16fb2ec095605f3d0d5e9f2d1
Branches Tags
TimeCrafters:master
...
compare: TimeCrafters:365ed7a602c2d019436e7809da9934cb7ccf4c70
Branches Tags
TimeCrafters:master

2 Commits
c4b404ee19 ... 365ed7a602

Author SHA1 Message Date
Sodi
365ed7a602 Merge remote-tracking branch 'origin/master' 
# Conflicts:
#	TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
 2023-02-04 12:55:07 -06:00
Sodi
0217495975 Debugging arm motor 2023-02-04 12:54:14 -06:00
7 changed files with 346 additions and 451 deletions
Expand all files Collapse all files

349
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -5,6 +5,7 @@ import android.util.Log;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState {
@@ -15,21 +16,10 @@ public class DriverStateWithOdometer extends CyberarmState {
private double maximumTolerance;
private float direction;
private boolean targetAchieved = false;
private double CurrentPosition;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final int COUNTS_PER_REVOLUTION = 8192;
public final double distanceMultiplier;
public double startOfRampUpRight;
public double startOfRampDownRight;
public double startOfRampUpLeft;
public double startOfRampDownLeft;
public double endOfRampUpRight;
public double endOfRampDownRight;
public double endOfRampUpLeft;
public double endOfRampDownLeft;
public int driveStage;
public float currentAngle;
public double currentHorizontalEncoder;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -42,7 +32,6 @@ public class DriverStateWithOdometer extends CyberarmState {
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
@@ -55,8 +44,6 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
@@ -64,277 +51,163 @@ public class DriverStateWithOdometer extends CyberarmState {
maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
if (targetDrivePower > 0) {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
} else {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
}
driveStage = 0;
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
// Driving Forward
if (targetDrivePower > 0 && driveStage == 0) {
if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up
if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
(LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
}
// Driving Normal
else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
(LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
drivePower = targetDrivePower;
}
// Ramping down going forward
else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
(LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
} else if (driveStage == 0){
driveStage = 1;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
// double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
if (targetDrivePower > 0) {
drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
} else {
drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
}
}
// Driving Backwards .................................................................................................................................Backwards
if (targetDrivePower < 0 && driveStage == 0) {
// ramping up
if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
(LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
// ramping down
if (targetDrivePower > 0){
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
} else {
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
}
// Driving Normal
else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
(LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
drivePower = targetDrivePower;
}
// Ramping down going backward
else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
(LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
} else if (driveStage == 0){
driveStage = 1;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
}
// end of ramp down
} else {
// middle ground
drivePower = targetDrivePower;
}
// Forwards distance adjust...............................................................................................................................STAGE 1
if (driveStage == 1 && targetDrivePower > 0) {
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
if (targetDrivePower < 0 && drivePower > 0) {
drivePower = drivePower * -1;
}
drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){
if (targetAchieved) {
drivePower = drivePower * 0.25;
} else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
drivePower = 0.25;
}
}
}
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.frontRightDrive.setPower(drivePower);
drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
}
else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
targetAchieved = true;
drivePower = targetDrivePower * -0.25;
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
driveStage = 2;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
drivePower = 0.25;
}
}
// backwards distance adjust
if (driveStage == 1 && targetDrivePower < 0) {
if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
} else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
} else {
driveStage = 2;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
}
}
if (driveStage == 0 || driveStage == 1) {
robot.frontRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
// Heading adjustment
if (driveStage == 2 || driveStage == 4) {
currentAngle = robot.imu.getAngularOrientation().firstAngle;
else {
if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER );
robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER );
robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
}
else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
else {
robot.backLeftDrive.setPower(0);
driveStage ++;
}
}
// .................................................................................................................................................Strafe Adjustment
if ( driveStage == 3 ){
currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
if (currentHorizontalEncoder > 200){
robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
}
else if (currentHorizontalEncoder < -200){
robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
driveStage = 4;
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
}
}
if (driveStage == 5) {
// setHasFinished(true);
}
//
}
@Override
public void telemetry () {
engine.telemetry.addData("Stage", driveStage);
engine.telemetry.addData("maximumTolerance", maximumTolerance);
engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
engine.telemetry.addData("endOfRampDownRight", endOfRampDownRight);
engine.telemetry.addData("startOfRampUpLeft", startOfRampUpLeft);
engine.telemetry.addData("endOfRampUpLeft", endOfRampUpLeft);
engine.telemetry.addData("startOfRampDownLeft", startOfRampDownLeft);
engine.telemetry.addData("endOfRampDownLeft", endOfRampDownLeft);
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
@Override
public void telemetry() {
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("imu yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("imu pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
engine.telemetry.addData("imu roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
engine.telemetry.addData("Target Achieved", targetAchieved);
engine.telemetry.addData("Target Achieved", targetAchieved);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
Log.i("TELEMETRY", "imu yaw:: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.i("TELEMETRY", "imu pitch:: " + robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
Log.i("TELEMETRY", "imu roll:: " + robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
}
}
}

3
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java
View File

@@ -3,6 +3,7 @@ package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
@@ -88,7 +89,7 @@ public class JunctionAllignmentAngleState extends CyberarmState {
@Override
public void exec() {
currentAngle = robot.imu.getAngularOrientation().firstAngle;
currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (stateDisabled){

3
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
public class RotationState extends CyberarmState {
@@ -40,7 +41,7 @@ public class RotationState extends CyberarmState {
return;
} // end of if
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.4 * drivePower;

21
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -3,9 +3,11 @@ package org.timecrafters.TeleOp.states;
import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmEngine;
@@ -60,7 +62,7 @@ public class PhoenixBot1 {
public CRServo collectorLeft, collectorRight;
public BNO055IMU imu;
public IMU imu;
public TimeCraftersConfiguration configuration;
@@ -131,17 +133,20 @@ public class PhoenixBot1 {
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.UP
));
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
// parameters.mode = BNO055IMU.SensorMode.IMU;
// parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
// parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
// parameters.loggingEnabled = false;
this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
this.imu = engine.hardwareMap.get(IMU.class, "imu");
imu.initialize(parameters);
imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
// imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
configuration = new TimeCraftersConfiguration("Phoenix");
// AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit");

33
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class PhoenixTeleOPState extends CyberarmState {
@@ -52,7 +53,7 @@ public class PhoenixTeleOPState extends CyberarmState {
public void telemetry() {
engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation);
engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
@@ -131,7 +132,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.dpad_right) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 90;
CalculateDeltaRotation();
if (RobotRotation > -90 && RobotRotation < 89) {//CCW
@@ -158,7 +159,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.dpad_left) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = -90;
CalculateDeltaRotation();
if (RobotRotation > -89 && RobotRotation <= 90) {//CW
@@ -185,7 +186,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.y) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 180;
CalculateDeltaRotation();
if (RobotRotation < 0 && RobotRotation > -179) {
@@ -210,7 +211,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.a && !engine.gamepad1.start) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 0;
CalculateDeltaRotation();
if (RobotRotation < -1) {
@@ -237,7 +238,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = -45;
CalculateDeltaRotation();
if (RobotRotation < -46 || RobotRotation > 135) {//CCW
@@ -264,7 +265,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 45;
CalculateDeltaRotation();
if (RobotRotation > -135 && RobotRotation < 44) {//CCW
@@ -470,24 +471,10 @@ public class PhoenixTeleOPState extends CyberarmState {
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad1 == gamepad && button.equals("start")) {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
robot.imu.initialize(parameters);
robot.imu.resetYaw();
}
}
// public double downSensor() {
// double Distance, Distance_1, Distance_2, Distance_3, Distance_4, Distance_5;
// Distance_1 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_2 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_3 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_4 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance_5 = robot.downSensor.getDistance(DistanceUnit.MM);
// Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
// return Distance;
}

281
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java
View File

@@ -9,10 +9,10 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class TeleOPArmDriver extends CyberarmState {
private final PhoenixBot1 robot;
private long lastStepTime = 0;
private long lastStepTime = 0, Spirit;
private int CyclingArmUpAndDown = 0;
private GamepadChecker gamepad2Checker;
private int Opportunity, Endeavour, Peanut;
private int Opportunity, JunctionHeight, Ingenuity;
private double drivePower, armPower;
private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
private double servoCollectLow = 0.40; //Low servos, A button
@@ -42,7 +42,7 @@ public class TeleOPArmDriver extends CyberarmState {
engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
engine.telemetry.addData("Target (Endeavour)", Endeavour);
engine.telemetry.addData("Target Junction Height", JunctionHeight);
}
@Override
@@ -52,9 +52,9 @@ public class TeleOPArmDriver extends CyberarmState {
robot.LowRiserLeft.setPosition(0.45);
robot.LowRiserRight.setPosition(0.45);
robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setPower(0.25);
robot.ArmMotor.setPower(0.5);
Opportunity = 0;
Endeavour = 0;
JunctionHeight = 0;
}
@@ -62,167 +62,176 @@ public class TeleOPArmDriver extends CyberarmState {
@Override
public void exec() {
if (Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) > 46) {
ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
armPower = ArmNeededPower;
Spirit = System.currentTimeMillis();
double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) / 400.0 + 0.1;
// ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
armPower = ratio;
robot.ArmMotor.setPower(armPower);
if (engine.gamepad2.y) {
JunctionHeight = 4;
}
if (engine.gamepad2.b) {
JunctionHeight = 3;
}
if (engine.gamepad2.x) {
JunctionHeight = 2;
}
if (engine.gamepad2.a) {
JunctionHeight = 1;
}
if (engine.gamepad2.y) {
Endeavour = 4;
if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(armMotorHigh);
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ {
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
if (engine.gamepad2.b) {
Endeavour = 3;
if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
if (engine.gamepad2.x) {
Endeavour = 2;
}
if (engine.gamepad2.a) {
Endeavour = 1;
if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
robot.LowRiserLeft.getPosition() >= servoHighLow) {
JunctionHeight = 0;
}
}
if (Endeavour == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(armMotorHigh);
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ {
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
if (JunctionHeight == 3) {
robot.ArmMotor.setTargetPosition(armMotorMed);
if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
}
if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
robot.LowRiserLeft.getPosition() >= servoHighLow) {
Endeavour = 0;
}
if (robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
}
if (Endeavour == 3) {
robot.ArmMotor.setTargetPosition(armMotorMed);
if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow &&
robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
Endeavour = 0;
if (robot.LowRiserLeft.getPosition() < servoMedLow &&
robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() > servoMedLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
JunctionHeight = 0;
}
}
if (Endeavour == 2) {
robot.ArmMotor.setTargetPosition(armMotorLow);
if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
}
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
Endeavour = 0;
if (JunctionHeight == 2) {
robot.ArmMotor.setTargetPosition(armMotorLow);
if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
if (Endeavour == 1) {
robot.ArmMotor.setTargetPosition(armMotorCollect);
if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
Endeavour = 0;
if (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (engine.gamepad2.dpad_left && Peanut != 1) {
Peanut = 1;
if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
}
if (engine.gamepad2.dpad_right && Peanut != 2) {
Peanut = 2;
if (robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
}
if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
Peanut = 0;
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
JunctionHeight = 0;
}
}
if (Peanut == 1) {
robot.collectorRight.setPower(1);
robot.collectorLeft.setPower(-1);
if (JunctionHeight == 1) {
robot.ArmMotor.setTargetPosition(armMotorCollect);
if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
JunctionHeight = 0;
}
}
if (Peanut == 2) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(-1);
}
if (engine.gamepad2.dpad_left && Ingenuity != 1) {
Ingenuity = 1;
}
if (engine.gamepad2.right_trigger > 0.1) {
if (engine.gamepad2.dpad_right && Ingenuity != 2) {
Ingenuity = 2;
}
if (engine.gamepad2.dpad_left && Ingenuity == 1 || engine.gamepad2.dpad_right && Ingenuity == 2) {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
Ingenuity = 0;
}
if (Ingenuity == 1) {
robot.collectorRight.setPower(1);
robot.collectorLeft.setPower(-1);
}
if (Ingenuity == 2) {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(-1);
}
if (engine.gamepad2.right_trigger > 0.1) {
JunctionHeight = 0;
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() + 50);
robot.ArmMotor.setPower(armPower);
} else if (engine.gamepad2.left_trigger > 0.1) {
} else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 0) {
JunctionHeight = 0;
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getCurrentPosition() - 50);
robot.ArmMotor.setPower(armPower);
}
}
}
}
}

107
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java
View File

@@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
public class TeleOPTankDriver extends CyberarmState {
@@ -14,7 +15,7 @@ public class TeleOPTankDriver extends CyberarmState {
private double RobotRotation;
private double RotationTarget, DeltaRotation;
private double MinimalPower = 0.2;
private int DeltaOdometerR, Endeavour, Spirit;
private int DeltaOdometerR, Spirit;
private GamepadChecker gamepad1Checker;
public TeleOPTankDriver(PhoenixBot1 robot) {
@@ -24,19 +25,20 @@ public class TeleOPTankDriver extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addLine("Tank Driver");
engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("IMU", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation);
}
@Override
public void init() {
}
@Override
public void exec() {
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative //ORLY?
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
@@ -45,44 +47,61 @@ public class TeleOPTankDriver extends CyberarmState {
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
drivePower = engine.gamepad1.left_stick_y;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) {
drivePower = engine.gamepad1.left_stick_x;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
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);
if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.backLeftDrive.setPower(backLeftPower * drivePower);
robot.frontRightDrive.setPower(frontRightPower * drivePower);
robot.backRightDrive.setPower(backRightPower * drivePower);
}
frontLeftPower = (rotY + rotX + rx) / denominator;
backLeftPower = (rotY - rotX + rx) / denominator;
frontRightPower = (rotY - rotX - rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
drivePower = engine.gamepad1.right_stick_x;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
robot.backLeftDrive.setPower(backLeftPower * drivePower);
robot.backRightDrive.setPower(backRightPower * drivePower);
robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.frontRightDrive.setPower(frontRightPower * drivePower);
if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
// if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
// drivePower = engine.gamepad1.left_stick_y;
// robot.backLeftDrive.setPower(drivePower);
// robot.backRightDrive.setPower(drivePower);
// robot.frontLeftDrive.setPower(drivePower);
// robot.frontRightDrive.setPower(drivePower);
// }
//
// if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) {
// drivePower = engine.gamepad1.left_stick_x;
// robot.backLeftDrive.setPower(drivePower);
// robot.backRightDrive.setPower(-drivePower);
// robot.frontLeftDrive.setPower(-drivePower);
// robot.frontRightDrive.setPower(drivePower);
// }
//
// if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
// robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
// robot.backLeftDrive.setPower(backLeftPower * drivePower);
// robot.frontRightDrive.setPower(frontRightPower * drivePower);
// robot.backRightDrive.setPower(backRightPower * drivePower);
// }
//
// if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
// drivePower = engine.gamepad1.right_stick_x;
// robot.backLeftDrive.setPower(-drivePower);
// robot.backRightDrive.setPower(drivePower);
// robot.frontLeftDrive.setPower(-drivePower);
// robot.frontRightDrive.setPower(drivePower);
// }
//
// if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) {
// drivePower = 0;
// robot.backLeftDrive.setPower(drivePower);
// robot.backRightDrive.setPower(drivePower);
// robot.frontLeftDrive.setPower(drivePower);
// robot.frontRightDrive.setPower(drivePower);
// }
@@ -111,14 +130,14 @@ public class TeleOPTankDriver extends CyberarmState {
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad1 == gamepad && button.equals("start")) {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
robot.imu.initialize(parameters);
robot.imu.resetYaw();
}
if (engine.gamepad1 == gamepad && button.equals("y")) {
drivePower = 1;
}
if (engine.gamepad1 == gamepad && button.equals("a")) {
drivePower = 0.5;
}
}