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

Debugging arm motor

Browse Source
This commit is contained in:
Sodi
2023-02-04 12:54:14 -06:00
parent 6a7d9c6de9
commit 0217495975
7 changed files with 242 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
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 com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1; import org.timecrafters.TeleOp.states.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState { public class DriverStateWithOdometer extends CyberarmState {
@@ -189,9 +190,9 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower()); engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition()); engine.telemetry.addData("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle); engine.telemetry.addData("imu yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle); engine.telemetry.addData("imu pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle); engine.telemetry.addData("imu roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
engine.telemetry.addData("Target Achieved", targetAchieved); engine.telemetry.addData("Target Achieved", targetAchieved);
@@ -204,9 +205,9 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("RampUpDistance", RampUpDistance); engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance); engine.telemetry.addData("RampDownDistance", RampDownDistance);
Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle); Log.i("TELEMETRY", "imu yaw:: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle); Log.i("TELEMETRY", "imu pitch:: " + robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle); 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 com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1; import org.timecrafters.TeleOp.states.PhoenixBot1;
@@ -88,7 +89,7 @@ public class JunctionAllignmentAngleState extends CyberarmState {
@Override @Override
public void exec() { public void exec() {
currentAngle = robot.imu.getAngularOrientation().firstAngle; currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (stateDisabled){ if (stateDisabled){

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

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.States; package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1; import org.timecrafters.TeleOp.states.PhoenixBot1;
public class RotationState extends CyberarmState { public class RotationState extends CyberarmState {
@@ -40,7 +41,7 @@ public class RotationState extends CyberarmState {
return; return;
} // end of if } // 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){ if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.4 * drivePower; 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.adafruit.AdafruitI2cColorSensor;
import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmEngine; import org.cyberarm.engine.V2.CyberarmEngine;
@@ -55,7 +57,7 @@ public class PhoenixBot1 {
public CRServo collectorLeft, collectorRight; public CRServo collectorLeft, collectorRight;
public BNO055IMU imu; public IMU imu;
public TimeCraftersConfiguration configuration; public TimeCraftersConfiguration configuration;
@@ -121,17 +123,20 @@ public class PhoenixBot1 {
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance"); leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right 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.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; // parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; // parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false; // parameters.loggingEnabled = false;
this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu"); this.imu = engine.hardwareMap.get(IMU.class, "imu");
imu.initialize(parameters); imu.initialize(parameters);
imu.startAccelerationIntegration(new Position(), new Velocity(), 10); // imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
configuration = new TimeCraftersConfiguration("Phoenix"); configuration = new TimeCraftersConfiguration("Phoenix");
// AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit"); // 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.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker; import org.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.DistanceUnit;
public class PhoenixTeleOPState extends CyberarmState { public class PhoenixTeleOPState extends CyberarmState {
@@ -52,7 +53,7 @@ public class PhoenixTeleOPState extends CyberarmState {
public void telemetry() { public void telemetry() {
engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition()); engine.telemetry.addData("Low Riser Right Position", robot.LowRiserRight.getPosition());
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.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("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation); engine.telemetry.addData("Delta Rotation", DeltaRotation);
engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM)); engine.telemetry.addData("Cone Distance", robot.collectorDistance.getDistance(DistanceUnit.MM));
@@ -131,7 +132,7 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.dpad_right) { if (engine.gamepad1.dpad_right) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 90; RotationTarget = 90;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation > -90 && RobotRotation < 89) {//CCW if (RobotRotation > -90 && RobotRotation < 89) {//CCW
@@ -158,7 +159,7 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.dpad_left) { if (engine.gamepad1.dpad_left) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = -90; RotationTarget = -90;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation > -89 && RobotRotation <= 90) {//CW if (RobotRotation > -89 && RobotRotation <= 90) {//CW
@@ -185,7 +186,7 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.y) { if (engine.gamepad1.y) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 180; RotationTarget = 180;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation < 0 && RobotRotation > -179) { if (RobotRotation < 0 && RobotRotation > -179) {
@@ -210,7 +211,7 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.a && !engine.gamepad1.start) { if (engine.gamepad1.a && !engine.gamepad1.start) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 0; RotationTarget = 0;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation < -1) { if (RobotRotation < -1) {
@@ -237,7 +238,7 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.x) { if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = -45; RotationTarget = -45;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation < -46 || RobotRotation > 135) {//CCW if (RobotRotation < -46 || RobotRotation > 135) {//CCW
@@ -264,7 +265,7 @@ public class PhoenixTeleOPState extends CyberarmState {
} }
if (engine.gamepad1.b) { if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle; RobotRotation = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
RotationTarget = 45; RotationTarget = 45;
CalculateDeltaRotation(); CalculateDeltaRotation();
if (RobotRotation > -135 && RobotRotation < 44) {//CCW if (RobotRotation > -135 && RobotRotation < 44) {//CCW
@@ -470,24 +471,10 @@ public class PhoenixTeleOPState extends CyberarmState {
@Override @Override
public void buttonUp(Gamepad gamepad, String button) { public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad1 == gamepad && button.equals("start")) { if (engine.gamepad1 == gamepad && button.equals("start")) {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU; robot.imu.resetYaw();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
robot.imu.initialize(parameters);
} }
} }
// 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 { public class TeleOPArmDriver extends CyberarmState {
private final PhoenixBot1 robot; private final PhoenixBot1 robot;
private long lastStepTime = 0; private long lastStepTime = 0, Spirit;
private int CyclingArmUpAndDown = 0; private int CyclingArmUpAndDown = 0;
private GamepadChecker gamepad2Checker; private GamepadChecker gamepad2Checker;
private int Opportunity, Endeavour, Peanut; private int Opportunity, JunctionHeight, Ingenuity;
private double drivePower, armPower; private double drivePower, armPower;
private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05; private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
private double servoCollectLow = 0.40; //Low servos, A button 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 Right Position", robot.LowRiserRight.getPosition());
engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition()); engine.telemetry.addData("Low Riser Left Position", robot.LowRiserLeft.getPosition());
engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition()); engine.telemetry.addData("Arm Motor TargetPosition", robot.ArmMotor.getTargetPosition());
engine.telemetry.addData("Target (Endeavour)", Endeavour); engine.telemetry.addData("Target Junction Height", JunctionHeight);
} }
@Override @Override
@@ -52,9 +52,9 @@ public class TeleOPArmDriver extends CyberarmState {
robot.LowRiserLeft.setPosition(0.45); robot.LowRiserLeft.setPosition(0.45);
robot.LowRiserRight.setPosition(0.45); robot.LowRiserRight.setPosition(0.45);
robot.ArmMotor.setTargetPosition(0); robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setPower(0.25); robot.ArmMotor.setPower(0.5);
Opportunity = 0; Opportunity = 0;
Endeavour = 0; JunctionHeight = 0;
} }
@@ -62,167 +62,176 @@ public class TeleOPArmDriver extends CyberarmState {
@Override @Override
public void exec() { public void exec() {
if (Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) > 46) { Spirit = System.currentTimeMillis();
ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25; double ratio = Math.abs(robot.ArmMotor.getCurrentPosition() - robot.ArmMotor.getTargetPosition()) / 400.0 + 0.1;
armPower = ArmNeededPower;
// ArmNeededPower = Math.abs((robot.ArmMotor.getTargetPosition() - robot.ArmMotor.getCurrentPosition()) / 920) + 0.25;
armPower = ratio;
robot.ArmMotor.setPower(armPower); 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) { if (JunctionHeight == 4 && System.currentTimeMillis() - lastStepTime >= 100) {
Endeavour = 4; 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) { if (robot.LowRiserLeft.getPosition() < servoHighLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
Endeavour = 3; robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} }
if (engine.gamepad2.x) { if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh &&
Endeavour = 2; robot.LowRiserLeft.getPosition() >= servoHighLow) {
} JunctionHeight = 0;
if (engine.gamepad2.a) {
Endeavour = 1;
} }
}
if (Endeavour == 4 && System.currentTimeMillis() - lastStepTime >= 100) { if (JunctionHeight == 3) {
lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setTargetPosition(armMotorMed);
robot.ArmMotor.setTargetPosition(armMotorHigh); if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ {
if (robot.ArmMotor.getCurrentPosition() < armMotorHigh)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) {
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize); 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.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} }
if (robot.ArmMotor.getCurrentPosition() >= armMotorHigh && }
robot.LowRiserLeft.getPosition() >= servoHighLow) { if (robot.LowRiserLeft.getPosition() <= servoMedLow &&
Endeavour = 0; 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 &&
if (Endeavour == 3) { robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
robot.ArmMotor.setTargetPosition(armMotorMed); if (System.currentTimeMillis() - lastStepTime >= 100) {
if (robot.LowRiserLeft.getPosition() > servoMedLow + 5)/* <-- low level too high*/ { lastStepTime = System.currentTimeMillis();
if (System.currentTimeMillis() - lastStepTime >= 100) { robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
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 - 5 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorMed) {
JunctionHeight = 0;
}
}
if (Endeavour == 2) { if (JunctionHeight == 2) {
robot.ArmMotor.setTargetPosition(armMotorLow); robot.ArmMotor.setTargetPosition(armMotorLow);
if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ { if (robot.LowRiserLeft.getPosition() > servoLowLow + 5)/* <-- low level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.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 (robot.LowRiserLeft.getPosition() < servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() >= armMotorLow)/* <-- low level too low*/ {
if (Endeavour == 1) { if (System.currentTimeMillis() - lastStepTime >= 100) {
robot.ArmMotor.setTargetPosition(armMotorCollect); lastStepTime = System.currentTimeMillis();
if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ { robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
if (System.currentTimeMillis() - lastStepTime >= 100) { robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
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 &&
if (engine.gamepad2.dpad_left && Peanut != 1) { robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
Peanut = 1; 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 &&
if (engine.gamepad2.dpad_right && Peanut != 2) { robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
Peanut = 2; if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() + ArmMotorStepSize);
}
} }
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) { robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.collectorLeft.setPower(0); robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
robot.collectorRight.setPower(0); JunctionHeight = 0;
Peanut = 0;
} }
}
if (Peanut == 1) { if (JunctionHeight == 1) {
robot.collectorRight.setPower(1); robot.ArmMotor.setTargetPosition(armMotorCollect);
robot.collectorLeft.setPower(-1); 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) { if (engine.gamepad2.dpad_left && Ingenuity != 1) {
robot.collectorLeft.setPower(1); Ingenuity = 1;
robot.collectorRight.setPower(-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.left_trigger > 0.1) {
JunctionHeight = 0;
} else if (engine.gamepad2.right_trigger == 0 && engine.gamepad2.left_trigger == 0 && Endeavour == 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.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker; import org.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
public class TeleOPTankDriver extends CyberarmState { public class TeleOPTankDriver extends CyberarmState {
@@ -14,7 +15,7 @@ public class TeleOPTankDriver extends CyberarmState {
private double RobotRotation; private double RobotRotation;
private double RotationTarget, DeltaRotation; private double RotationTarget, DeltaRotation;
private double MinimalPower = 0.2; private double MinimalPower = 0.2;
private int DeltaOdometerR, Endeavour, Spirit; private int DeltaOdometerR, Spirit;
private GamepadChecker gamepad1Checker; private GamepadChecker gamepad1Checker;
public TeleOPTankDriver(PhoenixBot1 robot) { public TeleOPTankDriver(PhoenixBot1 robot) {
@@ -24,19 +25,20 @@ public class TeleOPTankDriver extends CyberarmState {
@Override @Override
public void telemetry() { public void telemetry() {
engine.telemetry.addLine("Tank Driver"); 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("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation); engine.telemetry.addData("Delta Rotation", DeltaRotation);
} }
@Override @Override
public void init() { public void init() {
} }
@Override @Override
public void exec() { 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 x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = engine.gamepad1.right_stick_x; double rx = engine.gamepad1.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); 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 frontRightPower = (y - x - rx) / denominator;
double backRightPower = (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) { double heading = -robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
drivePower = engine.gamepad1.left_stick_x; double rotX = x * Math.cos(heading) - y * Math.sin(heading);
robot.backLeftDrive.setPower(drivePower); double rotY = x * Math.sin(heading) + y * Math.cos(heading);
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) { frontLeftPower = (rotY + rotX + rx) / denominator;
robot.frontLeftDrive.setPower(frontLeftPower * drivePower); backLeftPower = (rotY - rotX + rx) / denominator;
robot.backLeftDrive.setPower(backLeftPower * drivePower); frontRightPower = (rotY - rotX - rx) / denominator;
robot.frontRightDrive.setPower(frontRightPower * drivePower); backRightPower = (rotY + rotX - rx) / denominator;
robot.backRightDrive.setPower(backRightPower * drivePower);
}
if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { robot.backLeftDrive.setPower(backLeftPower * drivePower);
drivePower = engine.gamepad1.right_stick_x; robot.backRightDrive.setPower(backRightPower * drivePower);
robot.backLeftDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.backRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(frontRightPower * 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); // if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
robot.backRightDrive.setPower(drivePower); // drivePower = engine.gamepad1.left_stick_y;
robot.frontLeftDrive.setPower(drivePower); // robot.backLeftDrive.setPower(drivePower);
robot.frontRightDrive.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 @Override
public void buttonUp(Gamepad gamepad, String button) { public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad1 == gamepad && button.equals("start")) { if (engine.gamepad1 == gamepad && button.equals("start")) {
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU; robot.imu.resetYaw();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; }
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; if (engine.gamepad1 == gamepad && button.equals("y")) {
parameters.loggingEnabled = false; drivePower = 1;
}
robot.imu.initialize(parameters); if (engine.gamepad1 == gamepad && button.equals("a")) {
drivePower = 0.5;
} }
} }