TimeCrafters/FreightFrenzy
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FreightFrenzy.git synced 2025-12-13 05:02:34 +00:00
Code Issues Projects Releases Wiki Activity

created auto. states and made toggle for teleop (not tested yet)

Browse Source
This commit is contained in:
Spencer
2021-11-30 20:26:31 -06:00
parent 166b8091b8
commit 8f2f471667
6 changed files with 127 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java
View File

@@ -9,6 +9,6 @@ public class RedWarehouseEngine extends CyberarmEngine {
public void setup() {
Robot robot = new Robot(this);
addState(new DriveState(robot, 2000, 2000,.25,.25));
addState(new DriveState(robot, 2000, 2000,1,1));
}
}

42
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java Normal file
View File

@@ -0,0 +1,42 @@
package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
public class FaceState extends CyberarmState {
private double faceDirection, tolerance, power;
private Robot robot;
public FaceState(Robot robot, double faceDirection, double tolerance, double power) {
this.robot = robot;
this.faceDirection = faceDirection;
this.tolerance = tolerance;
this.power = power;
}
@Override
public void exec() {
robot.updateRobotOrientation();
if (robot.heading() - tolerance > faceDirection) {
robot.driveGoalLeft.setPower(power);
robot.driveWarehouseLeft.setPower(power);
robot.driveGoalRight.setPower(0);
robot.driveWarehouseRight.setPower(0);
}
else if (robot.heading() + tolerance < faceDirection){
robot.driveGoalLeft.setPower(0);
robot.driveWarehouseLeft.setPower(0);
robot.driveGoalRight.setPower(power);
robot.driveWarehouseRight.setPower(power);
}
else {
robot.driveGoalLeft.setPower(0);
robot.driveWarehouseLeft.setPower(0);
robot.driveGoalRight.setPower(0);
robot.driveWarehouseRight.setPower(0);
setHasFinished(true);
}
}
}

26
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java Normal file
View File

@@ -0,0 +1,26 @@
package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
public class TurretArmExtension extends CyberarmState {
private Robot robot;
private DcMotor motor;
private int position;
private double power;
public TurretArmExtension(Robot robot, DcMotor motor, int position, double power) {
this.robot = robot;
this.motor = motor;
this.position = position;
this.power = power;
}
@Override
public void exec() {
}
}

31
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java Normal file
View File

@@ -0,0 +1,31 @@
package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
import com.qualcomm.robotcore.hardware.CRServo;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
public class TurretOrbit extends CyberarmState {
private Robot robot;
private CRServo servo;
private long time;
private double power;
public TurretOrbit(Robot robot, CRServo servo, long time, double power) {
this.robot = robot;
this.servo = servo;
this.time = time;
this. power = power;
}
@Override
public void exec() {
if (runTime() < time) {
servo.setPower(power);
}
else {
servo.setPower(0);
setHasFinished(true);
}
}
}

13
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java
View File

@@ -65,6 +65,7 @@ public class Robot {
.translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT)
.multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0));
private RobotLocation vuforiaLastLocation;
private Orientation orientation;
// Sensors
public BNO055IMU imu;
@@ -102,15 +103,19 @@ public class Robot {
}
public double heading() {
return imu.getAngularOrientation().firstAngle;
return orientation.firstAngle;
}
public double roll() {
return imu.getAngularOrientation().secondAngle;
return orientation.secondAngle;
}
public double pitch() {
return imu.getAngularOrientation().thirdAngle;
return orientation.thirdAngle;
}
public void updateRobotOrientation(){
orientation = imu.getAngularOrientation();
}
public void activateTensorflow() {
@@ -215,6 +220,8 @@ public class Robot {
orangeArmRiser.setDirection(DcMotorSimple.Direction.FORWARD);
orangeArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
collectorOrange = engine.hardwareMap.crservo.get("collectorOrange");
orangeDispenser.setPosition(0.5);
}
private void initDepositor(){

25
TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java
View File

@@ -12,7 +12,7 @@ public class TeleOpState extends CyberarmState {
double maxDriveSpeed, maxCollectorArmSpeed, maxDepositorArmSpeed;
int maxArmTravelDistance, minArmTravelDistance;
boolean armLimitToggle = false, orangeCollectorToggle = false, whiteCollectorToggle = false;
boolean allianceRed = true;
boolean allianceRedDriver1 = true, allianceRedDriver2 = true;
public TeleOpState(Robot robot) {
this.robot = robot;
@@ -48,13 +48,13 @@ public class TeleOpState extends CyberarmState {
// dispenser powered
if (engine.gamepad1.b){
robot.orangeDispenser.setPosition(.5);
}
// if not pressed dispenser off
else {
robot.orangeDispenser.setPosition(0);
}
// if not pressed dispenser off
else {
robot.orangeDispenser.setPosition(0.5);
}
// if one of triggers pressed arm extends or unextends, there is also a limit on how far arm can extend
if (robot.orangeArmBobbin.getCurrentPosition() <= maxArmTravelDistance && engine.gamepad1.left_trigger > 0) {
@@ -84,7 +84,7 @@ public class TeleOpState extends CyberarmState {
else {
robot.turretServoOrange.setPower(0);
}*/
turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRed);
turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRedDriver1);
// if dpad verticles pressed arm rises or lowers...
if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) {
@@ -154,7 +154,7 @@ public class TeleOpState extends CyberarmState {
// else {
// robot.turretServoWhite.setPower(0);
// }
turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRed);
turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRedDriver2);
// if dpad verticles pressed arm rises or lowers
if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) {
@@ -228,6 +228,12 @@ public class TeleOpState extends CyberarmState {
robot.collectorOrange.setPower(-1);
}
break;
case "guide":
allianceRedDriver1 = !allianceRedDriver1;
break;
}
}
@@ -252,6 +258,9 @@ public class TeleOpState extends CyberarmState {
robot.collectorWhite.setPower(-1);
}
break;
case "guide":
allianceRedDriver2 = !allianceRedDriver2;
break;
}
}
}