TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 06:22:33 +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 org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState {
@@ -189,9 +190,9 @@ public class DriverStateWithOdometer extends CyberarmState {
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 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);
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);
@@ -204,9 +205,9 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
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);
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;
@@ -55,7 +57,7 @@ public class PhoenixBot1 {
public CRServo collectorLeft, collectorRight;
public BNO055IMU imu;
public IMU imu;
public TimeCraftersConfiguration configuration;
@@ -121,17 +123,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;
}
}