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

autonomous for scrimmage and tele op

Browse Source
This commit is contained in:
SpencerPiha
2022-11-27 13:02:52 -06:00
parent b4567a3f17
commit 0cbb6f2792
7 changed files with 95 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
View File

@@ -24,7 +24,7 @@ public class RightSideAutonomousEngine extends CyberarmEngine {
public void setup() {
robot = new PhoenixBot1(this);
addState(new ServoCameraRotate(robot, "RightSideAutonomous", "00-0"));
addState(new ConeIdentification(robot, "RightSideAutonomous", "00-1"));
// addState(new ConeIdentification(robot, "RightSideAutonomous", "00-1"));
addState(new ServoCameraRotate(robot, "RightSideAutonomous", "00-2"));
//drive to high pole
addState(new DriverState(robot, "RightSideAutonomous", "01-0"));

7
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
View File

@@ -23,6 +23,7 @@ public class CollectorDistanceState extends CyberarmState {
private boolean inRange = false;
private float collectTime;
private double inRangeTime;
private boolean stateDisabled;
public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -32,6 +33,8 @@ public class CollectorDistanceState extends CyberarmState {
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@@ -92,6 +95,10 @@ public class CollectorDistanceState extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
setHasFinished(true);
return;
}
if (System.currentTimeMillis() - lastMeasuredTime > 150) {
// checking to see if time is greater than 150 milliseconds

3
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java
View File

@@ -1,11 +1,12 @@
package org.timecrafters.minibots.cyberarm.engines;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
import org.timecrafters.minibots.cyberarm.states.Mecanum_Fancy_Drive_State;
@Disabled
@TeleOp(name = "Fancy Drive TeleOp")
public class Mecanum_fancy_drive_TeleOp extends CyberarmEngine {

2
TeamCode/src/main/java/org/timecrafters/testing/engine/PhoenixTeleOP.java
View File

@@ -7,7 +7,7 @@ import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.testing.states.PhoenixBot1;
import org.timecrafters.testing.states.PhoenixTeleOPState;
@TeleOp (name = "PhoenixTeleOP")
@TeleOp (name = "APhoenixTeleOP")
public class PhoenixTeleOP extends CyberarmEngine {

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

@@ -60,7 +60,7 @@ public class PhoenixBot1 {
}
private void setupRobot () {
// collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();

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

@@ -80,7 +80,7 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(drivePower);
}
if (engine.gamepad1.left_trigger > 0) {
else if (engine.gamepad1.left_trigger > 0) {
drivePower = engine.gamepad1.left_trigger;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower);
@@ -88,14 +88,14 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-drivePower);
}
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
drivePower = engine.gamepad1.left_stick_y;
if (Math.abs(engine.gamepad1.left_stick_y) > 0.05) {
drivePower = engine.gamepad1.left_stick_y * 0.3;
robot.backRightDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
drivePower = engine.gamepad1.right_stick_y;
if (Math.abs(engine.gamepad1.right_stick_y) > 0.05) {
drivePower = engine.gamepad1.right_stick_y * 0.3;
robot.backLeftDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
}
@@ -117,7 +117,7 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontRightDrive.setPower(-drivePower);
}
if (engine.gamepad1.a) {
if (engine.gamepad1.y) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 180;
CalculateDeltaRotation();
@@ -128,7 +128,7 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
else if (RobotRotation > 0) {
else if (RobotRotation >= 0) {
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
@@ -144,7 +144,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
}
if (engine.gamepad1.y) {
if (engine.gamepad1.a) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 0;
CalculateDeltaRotation();
@@ -199,23 +199,23 @@ public class PhoenixTeleOPState extends CyberarmState {
if (engine.gamepad1.x) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = -90;
RotationTarget = -45;
CalculateDeltaRotation();
if (RobotRotation < 90 && RobotRotation < -89) {//CCW
if (RobotRotation < -46 || RobotRotation > 135) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > 90 || RobotRotation < -91) {//CW
if (RobotRotation > -44 && RobotRotation < 135) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation > -91 && RobotRotation < -89) {
if (RobotRotation > -46 && RobotRotation < -44) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
@@ -253,23 +253,23 @@ public class PhoenixTeleOPState extends CyberarmState {
if (engine.gamepad1.b) {
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
RotationTarget = 90;
RotationTarget = 45;
CalculateDeltaRotation();
if (RobotRotation > -90 && RobotRotation < 89) {//CCW
if (RobotRotation > -135 && RobotRotation < 44) {//CCW
drivePower = (-1 * DeltaRotation/180) - MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < -90 || RobotRotation > 91) {//CW
if (RobotRotation < -135 || RobotRotation > 44) {//CW
drivePower = (1 * DeltaRotation/180) + MinimalPower;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
if (RobotRotation < 91 && RobotRotation > 89) {
if (RobotRotation < 46 && RobotRotation > 44) {
drivePower = 0;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
@@ -310,7 +310,7 @@ public class PhoenixTeleOPState extends CyberarmState {
}
if (engine.gamepad2.y) {
if (robot.HighRiserLeft.getPosition() < 0.9) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
@@ -391,62 +391,6 @@ public class PhoenixTeleOPState extends CyberarmState {
if (engine.gamepad2.start) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
switch (CyclingArmUpAndDown) {
// upper arm up
case 0:
if (robot.HighRiserLeft.getPosition() < 1) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
}
break;
// lower arm up
case 1:
if (robot.LowRiserLeft.getPosition() < 1) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
}
break;
// lower arm down
case 2:
if (robot.LowRiserLeft.getPosition() >= 0.44) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
} else {
CyclingArmUpAndDown = CyclingArmUpAndDown + 1;
}
break;
// upper arm down
case 3:
if (robot.HighRiserLeft.getPosition() >= 0.45) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
} else {
CyclingArmUpAndDown = 0;
}
break;
default:
break;
} // end of switch
}// end of time if statement
}// end of start button press
gamepad1Checker.update();
gamepad2Checker.update();
}

66
TeamCode/src/main/java/org/timecrafters/testing/states/SteeringDriveExperiment.java Normal file
View File

@@ -0,0 +1,66 @@
package org.timecrafters.testing.states;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
public class SteeringDriveExperiment extends CyberarmState {
private final PhoenixBot1 robot;
public boolean A;
public boolean X;
public boolean Y;
private double drivePower = 1;
private boolean bprev;
public SteeringDriveExperiment(PhoenixBot1 robot) {
this.robot = robot;
}
@Override
public void exec() {
//Gamepad Read
A = engine.gamepad1.a;
X = engine.gamepad1.x;
Y = engine.gamepad1.y;
//Drivetrain Variables
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
double rx = engine.gamepad1.right_stick_x;
//toggle for drive speed
boolean b = engine.gamepad1.b;
if (b && !bprev) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
bprev = b;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(rx), 1);
double frontLeftPower = (y + rx) / denominator;
double backLeftPower = (y + rx) / denominator;
double frontRightPower = (y - rx) / denominator;
double backRightPower = (y - rx) / denominator;
robot.frontLeftDrive.setPower(frontLeftPower * drivePower);
robot.backLeftDrive.setPower(backLeftPower * drivePower);
robot.frontRightDrive.setPower(frontRightPower * drivePower);
robot.backRightDrive.setPower(backRightPower * drivePower);
//------------------------------------------------------------------------------------------------------------------------------------
}
}