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

Compare commits

base: TimeCrafters:c9c654cf0633faa5413134d691c8f2e7cd8f5d89
Branches Tags
TimeCrafters:master
..
compare: TimeCrafters:master
Branches Tags
TimeCrafters:master

15 Commits
c9c654cf06 ... master

Author SHA1 Message Date
NerdyBirdy460
ad83dc5e0c Merge remote-tracking branch 'origin/master' 2023-03-25 12:08:19 -05:00
NerdyBirdy460
151c866ade Minibot program- day 3 2023-03-25 12:08:04 -05:00
Cyberarm
fd3d6cb44d Changes 2023-03-25 11:36:43 -05:00
Cyberarm
1b148ad75b Added telemetry for servos to MentorBot implementation of Phoenix 2023-03-25 08:33:36 -05:00
cyberarm
c5484131bb Added theoretical servo controller that provides a position estimate, added drivetrain control for MentorBot Phoenix implementation, misc. tweaks. 2023-03-18 15:08:41 -05:00
NerdyBirdy460
2d1f930593 Minibot program- day 2 2023-03-08 16:07:18 -06:00
Cyberarm
647568b406 Possibly fix broken build (Github Actions only) 2023-03-05 07:18:42 -06:00
Cyberarm
c36a8e0312 Stop moving Phoenix stuff around! Refactored Phoenix stuff into its own package. 2023-03-05 07:15:09 -06:00
Cyberarm
fa54f5f209 Added FTCLib, implemented Arm PID(f) controller 2023-03-04 12:42:51 -06:00
NerdyBirdy460
d59e7a54f7 Minibot program- day 1 2023-03-04 12:35:34 -06:00
Cyberarm
fbb0645283 Enable IMU reset from TeleOp, fix robot centric drive 2023-02-10 15:07:34 -06:00
SpencerPiha
2d8ea6d431 Autonomous work 2023-02-09 20:45:44 -06:00
SpencerPiha
6e1e3981c6 Autonomous work 2023-02-09 18:13:48 -06:00
SpencerPiha
32556f9c1e Autonomous work 2023-02-07 20:33:52 -06:00
Sodi
f2bd08a69a Autonomous config 2023-02-06 20:23:13 -06:00
58 changed files with 2056 additions and 390 deletions
Expand all files Collapse all files

2
FtcRobotController/build.gradle
View File

@@ -8,7 +8,7 @@ apply plugin: 'com.android.library'
android {
defaultConfig {
minSdkVersion 23
minSdkVersion 24
//noinspection ExpiredTargetSdkVersion
targetSdkVersion 28
buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"'

52
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java
View File

@@ -1,52 +0,0 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@Autonomous(name = "Right Side")
public class RightStateAutoEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void setup() {
robot = new PhoenixBot1(this);
robot.imu.resetYaw();
// setupFromConfig(
// robot.configuration,
// "org.timecrafters.Autonomous.States",
// robot,
// PhoenixBot1.class,
// "Right State Auto");
// // driving forward to low junction
addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
// // rotate left towards low junction
// addState(new RotationState(robot, "Right State Auto", "02-1"));
// // driving forward/ backwards to adjust
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
// // drive forwards or backwards to adjust to pole
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
// // counteract distance driven
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
// // rotate towards opposing alliance
// addState(new RotationState(robot, "Right State Auto", "04-1"));
// // drive to stack column
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
// // rotate at stack
// addState(new RotationState(robot, "Right State Auto", "05-1"));
// // drive at stack
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
}
}

109
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java
View File

@@ -1,109 +0,0 @@
package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class DriverParkPlaceState extends CyberarmState {
private final boolean stateDisabled;
PhoenixBot1 robot;
private int RampUpDistance;
private int RampDownDistance;
private String intendedPlacement;
public DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
@Override
public void start() {
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
String placement = engine.blackboardGetString("parkPlace");
if (placement != null) {
if (!placement.equals(intendedPlacement)){
setHasFinished(true);
}
if (placement.equals(intendedPlacement)) {
double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.25;
} else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
} else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
} // else ending
} // placement equals if statement
// setHasFinished(true);
} // end of placement doesn't equal null
} // end of exec
@Override
public void telemetry() {
engine.telemetry.addData("Position", intendedPlacement);
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("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
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);
} // end of telemetry
} // end of class

27
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftSideAutonomousEngine.java
View File

@@ -1,22 +1,21 @@
package org.timecrafters.Autonomous.Engines;
package org.timecrafters.Phoenix.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Phoenix.Autonomous.States.DriverState;
import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
import org.timecrafters.Phoenix.Autonomous.States.RotationState;
import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Phoenix.Autonomous.States.TopArm;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous (name = "Left Side")
//@Autonomous (name = "Left Side")
@Disabled
public class LeftSideAutonomousEngine extends CyberarmEngine {

32
TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftStateAutoEngine.java Normal file
View File

@@ -0,0 +1,32 @@
package org.timecrafters.Phoenix.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous(name = "Left Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
public class LeftStateAutoEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
}
@Override
public void setup() {
robot = new PhoenixBot1(this);
robot.imu.resetYaw();
setupFromConfig(
robot.configuration,
"org.timecrafters.Autonomous.States",
robot,
PhoenixBot1.class,
"Left State Auto");
}
}

32
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftTwoConeAutonomousEngine.java
View File

@@ -1,23 +1,21 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
package org.timecrafters.Phoenix.Autonomous.Engines;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Phoenix.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentDistanceState;
import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
import org.timecrafters.Phoenix.Autonomous.States.RotationState;
import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Phoenix.Autonomous.States.TopArm;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous (name = "left 2 cone auto")
//@Autonomous (name = "left 2 cone auto")
public class LeftTwoConeAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;

22
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightSideAutonomousEngine.java
View File

@@ -1,19 +1,19 @@
package org.timecrafters.Autonomous.Engines;
package org.timecrafters.Phoenix.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Phoenix.Autonomous.States.DriverState;
import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
import org.timecrafters.Phoenix.Autonomous.States.RotationState;
import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Phoenix.Autonomous.States.TopArm;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous (name = "Right ")
@Disabled

128
TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightStateAutoEngine.java Normal file
View File

@@ -0,0 +1,128 @@
package org.timecrafters.Phoenix.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous(name = "Right Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
public class RightStateAutoEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
}
@Override
public void setup() {
robot = new PhoenixBot1(this);
robot.imu.resetYaw();
setupFromConfig(
robot.configuration,
"org.timecrafters.Autonomous.States",
robot,
PhoenixBot1.class,
"Right State Auto");
//// addState(new TopArmv2(robot, "Right State Auto", "06-0"));
//
//// // forward to low junction ..........................................................................(I have PreLoaded Cone)
//// addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
//
// // rotate left towards low junction (angle = 45, direction = CCW) (I have PreLoaded Cone)
// addState(new RotationState(robot, "Right State Auto", "02-1"));
//
// // driving forward / backwards to adjust (I have PreLoaded Cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
//
// // counteract distance driven .........................................................................(I'm going for 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
//
// // rotate towards opposing alliance (angle = 0, direction = CW) (I'm going for 2nd cone)
// addState(new RotationState(robot, "Right State Auto", "05-0"));
//
// // drive to tall junction (to adjust to cone stack) (I'm going for 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
//
// // rotate at stack (angle = -90, direction = CW) (I'm going for 2nd cone)
// addState(new RotationState(robot, "Right State Auto", "07-0"));
//
// // drive at stack (I'm going for 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "08-0"));
//
//
// // drive away from stack slightly....................................................................... (I have a 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "09-0"));
//
// //drive away to low (I have a 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "10-0"));
//
// // rotate at low junction (angle = -135, direction = CW) (I have a 2nd cone)
// addState(new RotationState(robot, "Right State Auto", "11-0"));
//
// // driving forward / backwards to adjust (I have a 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "12-0"));
//
// // counteract distance driven .......................................................................(I'm going for 3rd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "13-0"));
//
// // rotate at stack (angle = -90, direction = CCW) (I'm going for 3rd cone)
// addState(new RotationState(robot, "Right State Auto", "14-0"));
//
// // drive at stack (I'm going for 3rd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "15-0"));
//
// // drive away from stack slightly................................................................... (I have a 3rd and final cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "16-0"));
//
// //drive away to mid (I have a 3rd and final cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "17-0"));
//
// // rotate at mid junction (angle = -135, direction = CW) (I have a 3rd and final cone)
// addState(new RotationState(robot, "Right State Auto", "18-0"));
//
// // driving forward / backwards to adjust (I have a 3rd and final cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "19-0"));
//
// // counteract distance driven.............................................................................. (I'm parking)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "20-0"));
//
// // rotate at opposing alliance (angle = 0, direction = CCW) (I'm parking)
// addState(new RotationState(robot, "Right State Auto", "21-0"));
//
// // Drive back one tile (I'm parking)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "22-0"));
//
// // rotate towards stack side (-90 CW) (I'm parking)
// addState(new RotationState(robot, "Right State Auto", "23-0"));
//
// // Choose Parking Spot (I'm parking)
// addState(new PathDecision(robot, "RightSideAutonomous", "24-0"));
//
// // case 1 drive forward (I'm parking)
// addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-1"));
//
// // case 2 drive forward (I'm parking)
// addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-2"));
//
// // case 3 drive forward (I'm parking)
// addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-3"));
//
// // rotate towards opposing alliance (angle = 0, direction = CCW) (I'm parking)
// addState(new RotationState(robot, "RightSideAutonomous", "25-0"));
}
}

32
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightTwoConeAutonomousEngine.java
View File

@@ -1,23 +1,21 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
package org.timecrafters.Phoenix.Autonomous.Engines;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.Autonomous.States.BottomArm;
import org.timecrafters.Phoenix.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Phoenix.Autonomous.States.CollectorState;
import org.timecrafters.Phoenix.Autonomous.States.ConeIdentification;
import org.timecrafters.Phoenix.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Phoenix.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentAngleState;
import org.timecrafters.Phoenix.Autonomous.States.JunctionAllignmentDistanceState;
import org.timecrafters.Phoenix.Autonomous.States.PathDecision;
import org.timecrafters.Phoenix.Autonomous.States.RotationState;
import org.timecrafters.Phoenix.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Phoenix.Autonomous.States.TopArm;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous (name = "Right 2 cone auto")
//@Autonomous (name = "Right 2 cone auto")
public class RightTwoConeAutonomousEngine extends CyberarmEngine {
PhoenixBot1 robot;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/BottomArm.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class BottomArm extends CyberarmState {

110
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/CollectorDistanceState.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class CollectorDistanceState extends CyberarmState {
@@ -12,7 +12,6 @@ public class CollectorDistanceState extends CyberarmState {
private int traveledDistance;
private int RampUpDistance;
private int RampDownDistance;
private double drivePower;
private double targetDrivePower;
private double lastMeasuredTime;
private double currentDistance;
@@ -24,6 +23,11 @@ public class CollectorDistanceState extends CyberarmState {
private float collectTime;
private double inRangeTime;
private boolean stateDisabled;
private double distanceLimit;
private long maximumLookTime;
private long startOfSequencerTime;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final double COUNTS_PER_REVOLUTION = 8192;
public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -33,6 +37,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.distanceLimit = robot.configuration.variable(groupName, actionName, "distanceLimit").value();
this.maximumLookTime = robot.configuration.variable(groupName, actionName, "maximumLookTime").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
@@ -40,25 +46,21 @@ public class CollectorDistanceState extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("Time left", System.currentTimeMillis() - startOfSequencerTime);
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("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addLine();
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addLine();
engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Current Distance", currentDistance);
engine.telemetry.addData("Current Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("last Distance", LastDistanceRead);
engine.telemetry.addLine();
@@ -75,25 +77,21 @@ public class CollectorDistanceState extends CyberarmState {
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
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.collectorLeft.setPower(1);
robot.collectorRight.setPower(1);
lastMeasuredTime = System.currentTimeMillis();
startOfSequencerTime = System.currentTimeMillis();
LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM);
traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
}
@@ -102,13 +100,14 @@ public class CollectorDistanceState extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
if (stateDisabled || System.currentTimeMillis() - startOfSequencerTime > maximumLookTime) {
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
return;
}
if (System.currentTimeMillis() - lastMeasuredTime > 150) {
@@ -136,65 +135,44 @@ public class CollectorDistanceState extends CyberarmState {
robot.backLeftDrive.setPower(0);
setHasFinished(true);
return;
}
}
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > distanceLimit) {
double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.15;
} else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.15);
} else {
// middle ground
drivePower = targetDrivePower;
}
robot.backLeftDrive.setPower(targetDrivePower);
robot.backRightDrive.setPower(targetDrivePower);
robot.frontLeftDrive.setPower(targetDrivePower);
robot.frontRightDrive.setPower(targetDrivePower);
if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
if (!inRange){
inRange = true;
inRangeTime = runTime();
} else {
if (runTime() - inRangeTime >= collectTime){
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
if (!inRange){
inRange = true;
inRangeTime = runTime();
} else {
if (runTime() - inRangeTime >= collectTime){
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
}
}
}
}
}
}
}

8
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/CollectorState.java
View File

@@ -1,8 +1,8 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class CollectorState extends CyberarmState {
@@ -52,8 +52,8 @@ public class CollectorState extends CyberarmState {
}
} else {
// robot.collectorLeft.setPower(0);
// robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
setHasFinished(true);
}

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/ConeIdentification.java
View File

@@ -1,8 +1,8 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
import java.util.List;

318
TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverParkPlaceState.java Normal file
View File

@@ -0,0 +1,318 @@
package org.timecrafters.Phoenix.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.Phoenix.PhoenixBot1;
public class DriverParkPlaceState extends CyberarmState {
private final boolean stateDisabled;
PhoenixBot1 robot;
private int RampUpDistance;
private int RampDownDistance;
private String intendedPlacement;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final double COUNTS_PER_REVOLUTION = 8192;
private double maximumTolerance;
private float direction;
private boolean targetAchieved = false;
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 DriverParkPlaceState(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
@Override
public void start() {
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
if (targetDrivePower > 0) {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
} else {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
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;
}
String placement = engine.blackboardGetString("parkPlace");
if (placement == null || !placement.equals(intendedPlacement)) {
setHasFinished(true);
} else {
double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
// Driving Forward
if (targetDrivePower > 0 && driveStage == 0) {
// 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);
}
}
// 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;
}
// 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
}
// Forwards distance adjust...............................................................................................................................STAGE 1
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);
}
}
// 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.VEER_COMPENSATION_DBL);
robot.backRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
}
// Heading adjustment
if (driveStage == 2 || driveStage == 4) {
currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
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 );
}
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);
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
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;
}
if (driveStage == 5) {
setHasFinished(true);
}
} // placement equals if statement
// setHasFinished(true);
} // end of placement doesn't equal null
} // end of exec
@Override
public void telemetry() {
engine.telemetry.addData("Position", intendedPlacement);
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("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
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);
} // end of telemetry
} // end of class

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverState.java
View File

@@ -1,9 +1,9 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class DriverState extends CyberarmState {
private final boolean stateDisabled;

46
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState {
private final boolean stateDisabled;
@@ -15,7 +15,7 @@ public class DriverStateWithOdometer extends CyberarmState {
private float direction;
private boolean targetAchieved = false;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final int COUNTS_PER_REVOLUTION = 8192;
public final double COUNTS_PER_REVOLUTION = 8192;
public double startOfRampUpRight;
public double startOfRampDownRight;
public double startOfRampUpLeft;
@@ -40,8 +40,7 @@ public class DriverStateWithOdometer extends CyberarmState {
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
private double drivePower, targetDrivePower, traveledDistance;
@Override
public void start() {
@@ -62,24 +61,24 @@ public class DriverStateWithOdometer extends CyberarmState {
if (targetDrivePower > 0) {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
} else {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
@@ -232,18 +231,18 @@ public class DriverStateWithOdometer extends CyberarmState {
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 );
robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05);
robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
}
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);
robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05 );
robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER + 0.05 );
robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER - 0.05);
} else {
robot.frontRightDrive.setPower(0);
@@ -255,7 +254,7 @@ public class DriverStateWithOdometer extends CyberarmState {
}
}
// .................................................................................................................................................Strafe Adjustment
// ...........................................................................................................................................Strafe Adjustment
if ( driveStage == 3 ){
currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
@@ -287,14 +286,17 @@ public class DriverStateWithOdometer extends CyberarmState {
}
if (driveStage == 5) {
// setHasFinished(true);
setHasFinished(true);
}
}
@Override
public void telemetry () {
engine.telemetry.addData("Stage", driveStage);
engine.telemetry.addData("maximumTolerance", maximumTolerance);
engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
@@ -311,9 +313,7 @@ public class DriverStateWithOdometer extends CyberarmState {
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("maximumTolerance", maximumTolerance);
engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Target Achieved", targetAchieved);

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentAngleState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/JunctionAllignmentAngleState.java
View File

@@ -1,11 +1,11 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.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.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class JunctionAllignmentAngleState extends CyberarmState {
private final boolean stateDisabled;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/JunctionAllignmentDistanceState.java
View File

@@ -1,8 +1,8 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class JunctionAllignmentDistanceState extends CyberarmState {
private final boolean stateDisabled;

15
TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/PathDecision.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class PathDecision extends CyberarmState {
PhoenixBot1 robot;
@@ -17,6 +17,17 @@ public class PathDecision extends CyberarmState {
@Override
public void exec() {
String placement = engine.blackboardGetString("parkPlace");
if (placement.equals("1")) {
addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 1"));
}
else if (placement.equals("3")){
addState(new DriverStateWithOdometer(robot, "" + groupName + " Parking", "Park 3"));
}
setHasFinished(true);
}

58
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/RotationState.java
View File

@@ -1,12 +1,13 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class RotationState extends CyberarmState {
private final boolean stateDisabled;
PhoenixBot1 robot;
public RotationState(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
@@ -52,13 +53,17 @@ public class RotationState extends CyberarmState {
drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER;
if (ClockWiseRotation) { rotationDirection = 1;} else { rotationDirection = -1;}
if (ClockWiseRotation) {
rotationDirection = 1;
} else {
rotationDirection = -1;
}
robot.backLeftDrive.setPower( drivePowerVariable * rotationDirection );
robot.backRightDrive.setPower( -drivePowerVariable * rotationDirection );
robot.frontLeftDrive.setPower( drivePowerVariable * rotationDirection );
robot.frontRightDrive.setPower( -drivePowerVariable * rotationDirection );
robot.backLeftDrive.setPower(drivePowerVariable * rotationDirection);
robot.backRightDrive.setPower(-drivePowerVariable * rotationDirection);
robot.frontLeftDrive.setPower(drivePowerVariable * rotationDirection);
robot.frontRightDrive.setPower(-drivePowerVariable * rotationDirection);
if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE &&
(RotationStage == 0) &&
@@ -66,14 +71,14 @@ public class RotationState extends CyberarmState {
RotationStage = 1;
lastStepTime = System.currentTimeMillis();
}
}
}
if (RotationStage == 1){
robot.backLeftDrive.setPower( 0 );
robot.backRightDrive.setPower( 0 );
robot.frontLeftDrive.setPower( 0 );
robot.frontRightDrive.setPower( 0 );
if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION ){
if (RotationStage == 1) {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
RotationStage = 2;
}
}
@@ -83,13 +88,14 @@ public class RotationState extends CyberarmState {
if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) {
// CW
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 );
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);
}
else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
lastStepTime = System.currentTimeMillis();
} else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
// CCW
robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
@@ -97,18 +103,24 @@ public class RotationState extends CyberarmState {
robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
lastStepTime = System.currentTimeMillis();
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
RotationStage ++;
// setHasFinished(true);
if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION) {
RotationStage = 3;
}
}
}
if (RotationStage == 3) {
RotationStage ++;
setHasFinished(true);
}
}

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/ServoCameraRotate.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/ServoCameraRotate.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class ServoCameraRotate extends CyberarmState {
private final boolean stateDisabled;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArm.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
package org.timecrafters.Phoenix.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class TopArm extends CyberarmState {

48
TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmResetState.java Normal file
View File

@@ -0,0 +1,48 @@
package org.timecrafters.Phoenix.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Phoenix.PhoenixBot1;
public class TopArmResetState extends CyberarmState {
private final PhoenixBot1 robot;
private double targetPower;
private int targetPosition;
private int tolerance;
private long lastMeasuredTime;
private long pausingTime;
public TopArmResetState(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
this.pausingTime = robot.configuration.variable(groupName, actionName, "pausingTime").value();
}
@Override
public void start() {
robot.ArmMotor.setTargetPosition(targetPosition);
robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.ArmMotor.setPower(targetPower);
lastMeasuredTime = System.currentTimeMillis();
}
@Override
public void exec() {
if (System.currentTimeMillis() - lastMeasuredTime > pausingTime) {
robot.ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.ArmMotor.setTargetPosition(0);
robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.ArmMotor.setPower(0.2);
setHasFinished(true);
}
}
}

37
TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmv2.java Normal file
View File

@@ -0,0 +1,37 @@
package org.timecrafters.Phoenix.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Phoenix.PhoenixBot1;
public class TopArmv2 extends CyberarmState {
private final PhoenixBot1 robot;
private double targetPower;
private int targetPosition;
private int tolerance;
public TopArmv2(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value();
this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value();
this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value();
}
@Override
public void start() {
robot.ArmMotor.setTargetPosition(targetPosition);
robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.ArmMotor.setPower(targetPower);
}
@Override
public void exec() {
if (robot.ArmMotor.getTargetPosition() == targetPosition) {
setHasFinished(true);
}
}
}

8
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java → TeamCode/src/main/java/org/timecrafters/Phoenix/PhoenixBot1.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix;
import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
import com.qualcomm.hardware.bosch.BNO055IMU;
@@ -37,6 +37,7 @@ public class PhoenixBot1 {
public double ROTATION_TOLERANCE;
public long PAUSE_ON_ROTATION;
public double DISTANCE_MULTIPLIER;
public double CAMERA_INITiAL_POS;
// private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -104,6 +105,7 @@ public class PhoenixBot1 {
PAUSE_ON_ROTATION = configuration.variable("Robot", "Tuning", "PAUSE_ON_ROTATION").value();
WHEEL_CIRCUMFERENCE = configuration.variable("Robot", "Tuning", "WHEEL_CIRCUMFERENCE").value();
DISTANCE_MULTIPLIER = configuration.variable("Robot", "Tuning", "DISTANCE_MULTIPLIER").value();
CAMERA_INITiAL_POS = configuration.variable("Robot", "Tuning", "CAMERA_INITiAL_POS").value();
}
private void initVuforia(){
@@ -238,7 +240,7 @@ public class PhoenixBot1 {
ArmMotor.setDirection(DcMotorSimple.Direction.FORWARD);
ArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ArmMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
ArmMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
CameraServo.setDirection(Servo.Direction.FORWARD);
@@ -247,7 +249,7 @@ public class PhoenixBot1 {
// HighRiserLeft.setPosition(0.40);
// HighRiserRight.setPosition(0.40);
CameraServo.setPosition(0.775);
CameraServo.setPosition(CAMERA_INITiAL_POS);

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/CameraTestEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/CameraTestEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import org.cyberarm.engine.V2.CyberarmEngine;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DriveDebugEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/DriveDebugEngine.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class DriveDebugEngine extends CyberarmEngine {

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DynamicSetupEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/DynamicSetupEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

8
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixTeleOP.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixTeleOP.java
View File

@@ -1,11 +1,11 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.TeleOPTankDriver;
import org.timecrafters.Autonomous.TeleOp.states.TeleOPArmDriver;
import org.timecrafters.Phoenix.PhoenixBot1;
import org.timecrafters.Phoenix.TeleOp.states.TeleOPArmDriver;
import org.timecrafters.Phoenix.TeleOp.states.TeleOPTankDriver;
@TeleOp (name = "APhoenixTeleOP")

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixWingEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixWingEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.CRServo;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SodiEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SodiEngine.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.TeleOp.states.LaserState;
import org.timecrafters.Phoenix.TeleOp.states.LaserState;
@Disabled
@TeleOp(name = "Wheel")

6
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SpeedrunEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SpeedrunEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -6,9 +6,9 @@ import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.GamepadChecker;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.TeleOp.states.TeleOPSpeedrunState;
import org.timecrafters.Phoenix.PhoenixBot1;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.Autonomous.TeleOp.states.TeleOPSpeedrunState;
@Disabled
@TeleOp (name = "Speedrun Engine")

6
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SterringDriveEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/SterringDriveEngine.java
View File

@@ -1,11 +1,11 @@
package org.timecrafters.Autonomous.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.SteeringDriveExperiment;
import org.timecrafters.Phoenix.TeleOp.states.SteeringDriveExperiment;
import org.timecrafters.Phoenix.PhoenixBot1;
@Disabled
@TeleOp(name = "Steering Drive Test")

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestCommon.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/CameraTestCommon.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/CameraTestState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/DynamicSetupState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/DynamicSetupState.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Autonomous.TeleOp.engine.DynamicSetupEngine;
import org.timecrafters.Phoenix.TeleOp.engine.DynamicSetupEngine;
public class DynamicSetupState extends CyberarmState {
private long delay;

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserServoHeight.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserServoHeight.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

3
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import android.annotation.SuppressLint;
@@ -10,6 +10,7 @@ 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;
import org.timecrafters.Phoenix.PhoenixBot1;
public class PhoenixTeleOPState extends CyberarmState {

3
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPv2.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPv2.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Phoenix.PhoenixBot1;
public class PhoenixTeleOPv2 extends CyberarmState {
private double drivePower = 1;

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixWingState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixWingState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiLEDState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SodiLEDState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

2
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SodiState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;

3
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SteeringDriveExperiment.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/SteeringDriveExperiment.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Phoenix.PhoenixBot1;
public class SteeringDriveExperiment extends CyberarmState {

5
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPArmDriver.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPArmDriver.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker;
import org.timecrafters.Phoenix.PhoenixBot1;
public class TeleOPArmDriver extends CyberarmState {
private final PhoenixBot1 robot;
@@ -21,7 +22,7 @@ public class TeleOPArmDriver extends CyberarmState {
// private double servoLowHigh = 0.75; //High servos, X button
private double servoMed = 0.45; //Low servos, B button
// private double servoMedHigh = 0.9; //High servos, B button
private double servoHigh = 0.7; //Low servos, Y button
private double servoHigh = 0.8; //Low servos, Y button
// private double servoHighHigh = 0.9; //High servos, Y button
private double ArmNeededPower;
private int armMotorCollect = -100;

3
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPSpeedrunState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPSpeedrunState.java
View File

@@ -1,8 +1,9 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.Phoenix.PhoenixBot1;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
public class TeleOPSpeedrunState extends CyberarmState {

3
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPTankDriver.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/TeleOPTankDriver.java
View File

@@ -1,10 +1,11 @@
package org.timecrafters.Autonomous.TeleOp.states;
package org.timecrafters.Phoenix.TeleOp.states;
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;
import org.timecrafters.Phoenix.PhoenixBot1;
public class TeleOPTankDriver extends CyberarmState {

14
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DrivetrainDriverControl.java
View File

@@ -10,6 +10,7 @@ import org.timecrafters.minibots.cyberarm.chiron.Robot;
public class DrivetrainDriverControl extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final double robotCentricRotation;
private Gamepad controller;
@@ -23,6 +24,8 @@ public class DrivetrainDriverControl extends CyberarmState {
this.actionName = actionName;
this.controller = engine.gamepad1;
this.robotCentricRotation = robot.tuningConfig("robot_centric_rotation").value();
}
@Override
@@ -50,6 +53,13 @@ public class DrivetrainDriverControl extends CyberarmState {
double y = invertRobotForward ? controller.left_stick_y : -controller.left_stick_y;
double x = (invertRobotForward && !fieldCentricControl ? controller.left_stick_x : -controller.left_stick_x);
if (!fieldCentricControl) {
Vector2d v = new Vector2d(x, y).rotated(robotCentricRotation);
x = v.getX();
y = v.getY();
}
// Improve control?
if (y < 0) {
y = -Math.sqrt(-y);
@@ -160,6 +170,10 @@ public class DrivetrainDriverControl extends CyberarmState {
if (button.equals("right_stick_button")) {
robotSlowMode = !robotSlowMode;
}
if (button.equals("left_stick_button") && robot.hardwareFault) {
robot.imu.resetYaw();
}
}
@Override

86
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/PositionalServoController.java Normal file
View File

@@ -0,0 +1,86 @@
package org.timecrafters.minibots.cyberarm.phoenix;
import com.qualcomm.robotcore.hardware.Servo;
public class PositionalServoController {
final private Servo servo;
final private double targetDegreesPerSecond, servoDegreesPerSecond, servoRangeInDegrees;
private double lastEstimatedPosition, estimatedPosition, workingPosition, targetPosition;
private long lastUpdatedAt;
private boolean travelling = false;
public PositionalServoController(Servo servo, double targetDegreesPerSecond, double servoDegreesPerSecond, double servoRangeInDegrees) {
this.servo = servo;
this.targetDegreesPerSecond = targetDegreesPerSecond;
this.servoDegreesPerSecond = servoDegreesPerSecond;
this.servoRangeInDegrees = servoRangeInDegrees;
this.workingPosition = servo.getPosition();
this.estimatedPosition = workingPosition;
this.lastEstimatedPosition = estimatedPosition;
this.lastUpdatedAt = System.currentTimeMillis();
}
public Servo getServo() {
return servo;
}
public void update() {
double error = targetPosition - estimatedPosition;
double delta = (System.currentTimeMillis() - lastUpdatedAt) / 1000.0;
double estimatedTravel = servoDegreesPerSecond * delta;
double targetTravel = targetDegreesPerSecond * delta;
if (targetTravel < estimatedTravel) {
estimatedTravel = targetTravel;
}
if (travelling) {
this.lastEstimatedPosition = estimatedPosition;
if (error < 0.0) {
this.estimatedPosition -= estimatedTravel;
} else {
this.estimatedPosition += estimatedTravel;
}
}
if (error < 0.0 - estimatedTravel) {
workingPosition -= estimatedTravel;
travelling = true;
} else if (error > 0.0 + estimatedTravel) {
workingPosition += estimatedTravel;
travelling = true;
} else {
workingPosition = targetPosition;
travelling = false;
}
servo.setPosition(workingPosition);
this.lastUpdatedAt = System.currentTimeMillis();
}
public void setTargetPosition(double targetPosition) {
this.targetPosition = targetPosition;
}
public double getEstimatedPosition() {
return estimatedPosition;
}
public double getWorkingPosition() { return workingPosition; }
public double getTargetPosition() { return targetPosition; }
public double getEstimatedAngle() {
return estimatedPosition * servoRangeInDegrees;
}
public double getError() { return targetPosition - estimatedPosition; }
private double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
}

724
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java Normal file
View File

@@ -0,0 +1,724 @@
package org.timecrafters.minibots.cyberarm.phoenix;
import android.annotation.SuppressLint;
import android.util.Log;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.Blinker;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import java.util.ArrayList;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.concurrent.TimeUnit;
public class Robot {
private static final String TAG = "Phoenix | Robot";
public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm;
public final PositionalServoController leftRiserServoController, rightRiserServoController;
public final Servo cameraServo;
public final CRServo collectorLeftServo, collectorRightServo;
// public final ServoImplEx gripper;
public final IMU imu;
public LynxModule expansionHub;
public final double imuAngleOffset, initialFacing;
public boolean armManuallyControlled = false;
public boolean automaticAntiTipActive = false;
public boolean hardwareFault = false;
public String hardwareFaultMessage = "";
private Status status = Status.OKAY, lastStatus = Status.OKAY;
private final CopyOnWriteArrayList<Status> reportedStatuses = new CopyOnWriteArrayList<>();
private final ConcurrentHashMap<String, Double> motorVelocityError = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorVelocityLastTiming = new ConcurrentHashMap<>();
private final ConcurrentHashMap<String, Double> motorTargetVelocity = new ConcurrentHashMap<>();
private ArmPosition armTargetPosition;
public enum ArmPosition {
COLLECT,
GROUND,
LOW,
MEDIUM,
HIGH
}
public enum Status {
OKAY,
MONITORING,
WARNING,
DANGER
}
private final CyberarmEngine engine;
private TimeCraftersConfiguration configuration;
private final double radius, diameter;
private final double wheelRadius, wheelGearRatio, armGearRatio;
private final int wheelTicksPerRevolution, armTicksPerRevolution;
private final VuforiaLocalizer vuforia;
private final TFObjectDetector tfod;
private boolean LEDStatusToggle = false;
private double lastLEDStatusAnimationTime = 0;
public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) {
this.engine = engine;
this.configuration = configuration;
radius = tuningConfig("field_localizer_robot_radius").value();
diameter = radius * 2;
imuAngleOffset = hardwareConfig("imu_angle_offset").value();
wheelRadius = tuningConfig("wheel_radius").value();
wheelGearRatio = tuningConfig("wheel_gear_ratio").value();
wheelTicksPerRevolution = tuningConfig("wheel_ticks_per_revolution").value();
armGearRatio = tuningConfig("arm_gear_ratio").value();
armTicksPerRevolution = tuningConfig("arm_ticks_per_revolution").value();
// FIXME: Rename motors in configuration
// Define hardware
backLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Left"); // MOTOR PORT: 2 - CONTROL HUB
frontRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Right"); // MOTOR PORT: 1 - CONTROL HUB
frontLeftDrive = engine.hardwareMap.get(DcMotorEx.class, "Front Left"); // MOTOR PORT: 3 - CONTROL HUB
backRightDrive = engine.hardwareMap.get(DcMotorEx.class, "Back Right"); // MOTOR PORT: 0 - CONTROL HUB
// FIXME: Rename lift_drive to arm in hardware config
arm = engine.hardwareMap.get(DcMotorEx.class, "ArmMotor"); // MOTOR PORT: 2 - EXPANSION HUB
// gripper = engine.hardwareMap.get(ServoImplEx.class, "gripper"); // SERVO PORT: ?
imu = engine.hardwareMap.get(IMU.class, "imu");
// Configure hardware
// MOTORS
// DIRECTION
frontLeftDrive.setDirection(hardwareConfig("front_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
frontRightDrive.setDirection(hardwareConfig("front_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
backLeftDrive.setDirection(hardwareConfig("back_left_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
backRightDrive.setDirection(hardwareConfig("back_right_drive_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
arm.setDirection(hardwareConfig("arm_direction_forward").value() ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
// RUNMODE
frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
armTargetPosition = ArmPosition.COLLECT;
// ZERO POWER BEHAVIOR
frontLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// MOTOR POWER
arm.setPower(tuningConfig("arm_automatic_power").value());
// SERVOS
Servo leftRiser = engine.hardwareMap.servo.get("LowRiserLeft");
leftRiser.setDirection(Servo.Direction.FORWARD);
Servo rightRiser = engine.hardwareMap.servo.get("LowRiserRight");
rightRiser.setDirection(Servo.Direction.REVERSE);
// NOTES: 428.57142858° per second = no load speed of 60° per 0.14s at 6V, see: https://docs.revrobotics.com/duo-build/actuators/servos/smart-robot-servo#mechanical-specifications
leftRiserServoController = new PositionalServoController(
leftRiser,
5.0,
428.57142858,
270.0
); // SERVER PORT: ? - ? HUB
rightRiserServoController = new PositionalServoController(
rightRiser,
5.0,
428.57142858,
270.0
); // SERVER PORT: ? - ? HUB
cameraServo = engine.hardwareMap.servo.get("Camera Servo"); // SERVER PORT: ? - ? HUB
collectorLeftServo = engine.hardwareMap.crservo.get("Collector Left"); // SERVER PORT: ? - ? HUB
collectorRightServo = engine.hardwareMap.crservo.get("Collector Right"); // SERVER PORT: ? - ? HUB
// SERVO DIRECTIONS
cameraServo.setDirection(Servo.Direction.REVERSE);
collectorLeftServo.setDirection(DcMotorSimple.Direction.REVERSE);
collectorRightServo.setDirection(DcMotorSimple.Direction.FORWARD);
// SENSORS
// IMU
IMU.Parameters parameters = new IMU.Parameters(
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.UP
)
);
imu.initialize(parameters);
// Preserve our "initial" facing, since we transform it from zero.
initialFacing = facing();
// BulkRead from Hubs
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
if (!hub.isParent() && expansionHub == null) {
expansionHub = hub;
}
}
// Set LED pattern
if (expansionHub != null) {
expansionHub.setPattern(ledPatternOkay());
}
// Webcam
vuforia = initVuforia();
tfod = initTfod();
// Drive Encoder Error Setup
engine.blackboardSet("left_drive_error", 0);
engine.blackboardSet("right_drive_error", 0);
}
private VuforiaLocalizer initVuforia() {
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
parameters.vuforiaLicenseKey = hardwareConfig("vuforia_license_key").value();
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
return ClassFactory.getInstance().createVuforia(parameters);
}
private TFObjectDetector initTfod() {
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minResultConfidence = 0.75f;
tfodParameters.isModelTensorFlow2 = true;
tfodParameters.inputSize = 300;
TFObjectDetector tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset("PowerPlay.tflite", "1 Bolt", "2 Bulb", "3 Panel");
return tfod;
}
public void reloadConfig() {
String name = configuration.getConfig().getName();
configuration = new TimeCraftersConfiguration(name);
}
public void standardTelemetry() {
engine.telemetry.addLine();
// STATUS
engine.telemetry.addLine("DATA");
engine.telemetry.addData(" Robot Status", status);
engine.telemetry.addData(" Hardware Fault", hardwareFault);
engine.telemetry.addData(" Hardware Fault Message", hardwareFaultMessage);
engine.telemetry.addLine();
// Motor Powers
engine.telemetry.addLine("Motor Powers");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getPower());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getPower());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getPower());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getPower());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getPower());
engine.telemetry.addLine();
// Motor Positions
engine.telemetry.addLine("Motor Positions");
engine.telemetry.addData(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getCurrentPosition()));
engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getCurrentPosition()));
engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getCurrentPosition()));
engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getCurrentPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getCurrentPosition()));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getCurrentPosition(), ticksToAngle(arm.getCurrentPosition()));
engine.telemetry.addLine();
// Motor Target Positions
engine.telemetry.addLine("Motor Target Positions");
engine.telemetry.addData(" Front Left Drive", "%d (%8.2f in)", frontLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontLeftDrive.getTargetPosition()));
engine.telemetry.addData(" Front Right Drive", "%d (%8.2f in)", frontRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, frontRightDrive.getTargetPosition()));
engine.telemetry.addData(" Back Left Drive", "%d (%8.2f in)", backLeftDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backLeftDrive.getTargetPosition()));
engine.telemetry.addData(" Back Right Drive", "%d (%8.2f in)", backRightDrive.getTargetPosition(), ticksToUnit(DistanceUnit.INCH, backRightDrive.getTargetPosition()));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%d (%8.2f degrees)", arm.getTargetPosition(), ticksToAngle(arm.getTargetPosition()));
engine.telemetry.addLine();
// Motor Velocity
engine.telemetry.addLine("Motor Velocity");
engine.telemetry.addData(" Front Left Drive", "%8.2f (%8.2f in)", frontLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontLeftDrive.getVelocity()));
engine.telemetry.addData(" Front Right Drive", "%8.2f (%8.2f in)", frontRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) frontRightDrive.getVelocity()));
engine.telemetry.addData(" Back Left Drive", "%8.2f (%8.2f in)", backLeftDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backLeftDrive.getVelocity()));
engine.telemetry.addData(" Back Right Drive", "%8.2f (%8.2f in)", backRightDrive.getVelocity(), ticksToUnit(DistanceUnit.INCH, (int) backRightDrive.getVelocity()));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", "%8.2f (%8.2f degrees)", arm.getVelocity(), ticksToAngle((int)arm.getVelocity()));
engine.telemetry.addLine();
// Motor Currents
engine.telemetry.addLine("Motor Currents (AMPS)");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addData(" Back Right Drive", backRightDrive.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getCurrent(CurrentUnit.AMPS));
engine.telemetry.addLine();
// Motor Directions
engine.telemetry.addLine("Motor Directions");
engine.telemetry.addData(" Front Left Drive", frontLeftDrive.getDirection());
engine.telemetry.addData(" Front Right Drive", frontRightDrive.getDirection());
engine.telemetry.addData(" Back Left Drive", backLeftDrive.getDirection());
engine.telemetry.addData(" Back Right Drive", backRightDrive.getDirection());
engine.telemetry.addLine();
engine.telemetry.addData(" Arm", arm.getDirection());
engine.telemetry.addLine();
// Servo Positions
engine.telemetry.addLine("Servo Positions");
engine.telemetry.addData(" Left Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", leftRiserServoController.getEstimatedPosition(), leftRiserServoController.getError(), leftRiserServoController.getTargetPosition(), leftRiserServoController.getServo().getPosition());
engine.telemetry.addData(" Right Riser (Est)/(Target)/(Blind)", "%.4f [error: %.4f] / %.4f / %.4f", rightRiserServoController.getEstimatedPosition(), rightRiserServoController.getError(), rightRiserServoController.getTargetPosition(), rightRiserServoController.getServo().getPosition());
engine.telemetry.addData(" Camera (Blind)", cameraServo.getPosition());
engine.telemetry.addLine();
// Continuous Servo Powers
engine.telemetry.addLine("Servo Powers");
engine.telemetry.addData(" Collector Left", collectorLeftServo.getPower());
engine.telemetry.addData(" Collector Right", collectorRightServo.getPower());
engine.telemetry.addLine();
// Servo Directions
engine.telemetry.addLine("Servo Directions");
engine.telemetry.addData(" Left Riser", leftRiserServoController.getServo().getDirection());
engine.telemetry.addData(" Right Riser", rightRiserServoController.getServo().getDirection());
engine.telemetry.addData(" Camera", cameraServo.getDirection());
engine.telemetry.addData(" Collector Left", collectorLeftServo.getDirection());
engine.telemetry.addData(" Collector Right", collectorRightServo.getDirection());
engine.telemetry.addLine();
// Sensors / IMU
engine.telemetry.addLine("IMU");
engine.telemetry.addData(" Facing (Degrees)", facing());
engine.telemetry.addData(" Heading (Radians)", heading());
engine.telemetry.addData(" Turn Rate", turnRate());
engine.telemetry.addData(" Angle Offset (Degrees)", imuAngleOffset);
engine.telemetry.addLine();
}
private ArrayList<Blinker.Step> ledPatternStandby() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0x008000, 750, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x005000, 750, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x002000, 750, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x001000, 250, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternOkay() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0x00aa00, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x00aa44, 500, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternMonitoring() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xaaaaaa, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x000000, 250, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternWarning() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xffff00, 500, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0xff8800, 500, TimeUnit.MILLISECONDS));
return steps;
}
private ArrayList<Blinker.Step> ledPatternDanger() {
final ArrayList<Blinker.Step> steps = new ArrayList<>();
steps.add(new Blinker.Step(0xff0000, 250, TimeUnit.MILLISECONDS));
steps.add(new Blinker.Step(0x000000, 100, TimeUnit.MILLISECONDS));
return steps;
}
public void reportStatus(Status status) {
reportedStatuses.add(status);
}
public void update() {
for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
hub.clearBulkCache();
}
double voltage = getVoltage();
if (voltage < 9.75) {
reportStatus(Status.DANGER);
hardwareFaultMessage = "Battery voltage to low! (" + voltage + " volts)";
hardwareFault = true;
}
status = Status.OKAY;
for (Status s : reportedStatuses) {
if (s.ordinal() > status.ordinal()) {
status = s;
}
}
reportedStatuses.clear();
if (status != lastStatus) {
lastStatus = status;
if (expansionHub != null) {
if (lastStatus == Status.OKAY) { expansionHub.setPattern(ledPatternOkay()); }
if (lastStatus == Status.MONITORING) { expansionHub.setPattern(ledPatternMonitoring()); }
if (lastStatus == Status.WARNING) { expansionHub.setPattern(ledPatternWarning()); }
if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
}
}
manageArmAndRiserServos();
}
public void stop() {
if (expansionHub != null) {
expansionHub.setPattern(ledPatternStandby());
}
}
public void armPosition(ArmPosition position) {
if (hardwareFault) {
return;
}
armTargetPosition = position;
reportStatus(Status.WARNING);
}
private void manageArmAndRiserServos() {
boolean lowerServos = true;
switch (armTargetPosition) {
case COLLECT:
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
}
break;
// case GROUND:
// if (areRiserServosInLoweredPosition()) {
// arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").value()));
// }
// break;
case GROUND: // DISABLE GROUND JUNCTION SUPPORT FOR NOW
case LOW:
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
}
break;
case MEDIUM:
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
}
break;
case HIGH:
double angleHigh = tuningConfig("arm_position_angle_high").value();
arm.setTargetPosition(angleToTicks(angleHigh));
if (arm.getCurrentPosition() >= angleToTicks(angleHigh - 10.0)) {
lowerServos = false;
}
break;
default:
throw new RuntimeException("Unexpected arm position!");
}
if (lowerServos) {
leftRiserServoController.setTargetPosition(0.45);
rightRiserServoController.setTargetPosition(0.45);
} else {
leftRiserServoController.setTargetPosition(0.8);
rightRiserServoController.setTargetPosition(0.8);
}
leftRiserServoController.update();
rightRiserServoController.update();
}
private boolean areRiserServosInLoweredPosition() {
return leftRiserServoController.getEstimatedPosition() < 0.5 && rightRiserServoController.getEstimatedPosition() < 0.5;
}
// Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38
public double angleDiff(double from, double to) {
double value = (to - from + 180);
double fmod = (value - 0.0) % (360.0 - 0.0);
return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180;
}
public double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
public Status getStatus() { return status; }
public double getRadius() { return radius; }
public double getDiameter() { return diameter; }
public double getVoltage() {
return engine.hardwareMap.voltageSensor.iterator().next().getVoltage();
}
public TFObjectDetector getTfod() { return tfod; }
public VuforiaLocalizer getVuforia() { return vuforia; }
public TimeCraftersConfiguration getConfiguration() { return configuration; }
// For: Drive Wheels
public int unitToTicks(DistanceUnit unit, double distance) {
double fI = (wheelGearRatio * wheelTicksPerRevolution) / (wheelRadius * 2 * Math.PI * (wheelGearRatio * wheelTicksPerRevolution) / (wheelGearRatio * wheelTicksPerRevolution));
double inches = unit.toInches(unit.fromUnit(unit, distance));
double ticks = fI * inches;
return (int)ticks;
}
// For: Drive Wheels
public double ticksToUnit(DistanceUnit unit, int ticks) {
// Convert to inches, then to unit.
double inches = wheelRadius * 2 * Math.PI * ticks / (wheelGearRatio * wheelTicksPerRevolution);
return unit.fromUnit(DistanceUnit.INCH, inches);
}
// For: Arm
public int angleToTicks(double angle) {
double d = (armGearRatio * armTicksPerRevolution) / 360.0;
// Casting to float so that the int version of Math.round is used.
return Math.round((float)d * (float)angle);
}
// For: Arm
public double ticksToAngle(int ticks) {
double oneDegree = 360.0 / (armGearRatio * armTicksPerRevolution);
return oneDegree * ticks;
}
public Variable hardwareConfig(String variableName) {
Action hardwareConfiguration = configuration.action("Robot", "Hardware");
for (Variable v : hardwareConfiguration.getVariables()) {
if (variableName.trim().equals(v.name)) {
return v;
}
}
throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Hardware");
}
public Variable tuningConfig(String variableName) {
Action action = configuration.action("Robot", "Tuning");
for (Variable v : action.getVariables()) {
if (variableName.trim().equals(v.name)) {
return v;
}
}
throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning");
}
@SuppressLint("NewApi")
public void controlMotorPIDF(DcMotorEx motor, String motorName, double targetVelocity, double feedForward) {
Action action = configuration.action("Robot", "Tuning_PIDF_" + motorName);
double proportional = 0, integral = 0, derivative = 0;
for (Variable v : action.getVariables()) {
switch (v.name.trim()) {
case "proportional":
proportional = v.value();
break;
case "integral":
integral = v.value();
break;
case "derivative":
derivative = v.value();
break;
}
}
double interval = (engine.getRuntime() - motorVelocityLastTiming.getOrDefault(motorName, 0.0));
double distance = motor.getTargetPosition() - motor.getCurrentPosition();
if (Math.abs(distance) < Math.abs(targetVelocity)) {
if ((targetVelocity < 0 && distance > 0) || (targetVelocity > 0 && distance < 0)) {
targetVelocity = -distance;
} else {
targetVelocity = distance;
}
}
double error = targetVelocity - motor.getVelocity();
double deltaError = error - motorVelocityError.getOrDefault(motorName, 0.0);
double kIntegral = error * interval;
double kDerivative = deltaError / interval;
double kp = proportional * error;
double ki = integral * kIntegral;
double kd = derivative * kDerivative;
motorVelocityError.put(motorName, error);
motorVelocityLastTiming.put(motorName, engine.getRuntime());
double newTargetVelocity = kp + ki + kd + targetVelocity;
motor.setVelocity(newTargetVelocity);
Log.d(TAG, "Interval: " + interval + "ms, Error: " + error + " ticks, deltaError: " + deltaError + " ticks, distance: " +
distance + " ticks, kIntegral: " + kIntegral + ", kDerivative: " + kDerivative + ", proportional: " + proportional +
", integral: " + integral + ", derivative: " + derivative + ", kp: " + kp + ", ki: " + ki + ", kd: " + kd +
", targetVelocity: " + targetVelocity + " ticks, new Target Velocity: " + newTargetVelocity + " ticks, + motorVelocity: " + motor.getVelocity() + " ticks.");
}
@SuppressLint("NewApi")
public void controlArmMotor(double targetVelocity) {
// double time = System.currentTimeMillis();
// double newTargetVelocity = motorTargetVelocity.getOrDefault("Arm", targetVelocity);
// double lastTiming = motorVelocityLastTiming.getOrDefault("Arm", time);
// double deltaTime = (time - lastTiming) * 0.001;
//
// double distanceToTarget = arm.getTargetPosition() - arm.getCurrentPosition();
// double adjustedTargetVelocity = Math.abs(distanceToTarget) < targetVelocity ? Math.abs(distanceToTarget) : targetVelocity;
//
// double error = adjustedTargetVelocity - arm.getVelocity();
// double kp = 0.9;
//
// newTargetVelocity += error * kp * deltaTime;
//
// motorTargetVelocity.put("Arm", newTargetVelocity);
// motorVelocityLastTiming.put("Arm", time);
// arm.setVelocity(newTargetVelocity);
arm.setPower(tuningConfig("arm_automatic_power").value());
}
public double initialFacing() {
return initialFacing;
}
public double facing() {
double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0;
}
public double heading() {
return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0);
// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
}
public double turnRate() {
return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED
}
public boolean isBetween(double value, double min, double max) {
return value >= min && value <= max;
}
}

47
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/engines/TeleOp.java Normal file
View File

@@ -0,0 +1,47 @@
package org.timecrafters.minibots.cyberarm.phoenix.engines;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.cyberarm.phoenix.Robot;
@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Mentor Phoenix | TeleOp", group = "Mentor Phoenix")
public class TeleOp extends CyberarmEngine {
private Robot robot;
private TimeCraftersConfiguration configuration;
final private String configurationName = "MentorPhoenix", actionsGroupName = "TeleOpStates";
@Override
public void setup() {
configuration = new TimeCraftersConfiguration(configurationName);
robot = new Robot(
this,
configuration
);
robot.imu.resetYaw();
setupFromConfig(
robot.getConfiguration(),
"org.timecrafters.minibots.cyberarm.phoenix.states.teleop",
robot,
Robot.class,
actionsGroupName);
}
@Override
public void loop() {
robot.update();
super.loop();
robot.standardTelemetry();
}
@Override
public void stop() {
robot.stop();
super.stop();
}
}

122
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/ArmController.java Normal file
View File

@@ -0,0 +1,122 @@
package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import org.timecrafters.minibots.cyberarm.phoenix.Robot;
public class ArmController extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final Gamepad controller;
private PIDController pidController;
private double p = 0, i = 0, d = 0, f = 0;
private final double ticksInDegree = 700 / 180;
public ArmController(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.controller = engine.gamepad1;
pidController = new PIDController(p, i, d);
}
@Override
public void exec() {
Action action = robot.getConfiguration().action("Robot", "Tuning_PIDF_Arm");
for (Variable v : action.getVariables()) {
switch (v.name.trim()) {
case "proportional":
p = v.value();
break;
case "integral":
i = v.value();
break;
case "derivative":
d = v.value();
break;
case "feed": // feedback
f = v.value();
break;
}
}
pidController.setPID(p, i, d);
int armPos = robot.arm.getCurrentPosition();
double pid = pidController.calculate(armPos, robot.arm.getTargetPosition());
double ff = Math.cos(Math.toRadians(robot.arm.getTargetPosition() / ticksInDegree)) * f;
double power = pid + ff;
robot.arm.setPower(power);
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != controller) {
return;
}
switch (button) {
case "guide":
robot.reloadConfig();
break;
// Arm control
case "a":
robot.armPosition(Robot.ArmPosition.COLLECT);
break;
case "x":
robot.armPosition(Robot.ArmPosition.LOW); // DISABLED GROUND JUNCTION
break;
case "b":
robot.armPosition(Robot.ArmPosition.MEDIUM);
break;
case "y":
robot.armPosition(Robot.ArmPosition.HIGH);
break;
// Manual stepping arm
case "left_bumper":
robot.arm.setTargetPosition(
robot.arm.getCurrentPosition() - robot.angleToTicks(5.0)
);
break;
case "right_bumper":
robot.arm.setTargetPosition(
robot.arm.getCurrentPosition() + robot.angleToTicks(5.0)
);
break;
// Collector control
case "dpad_up":
robot.collectorLeftServo.setPower(-1);
robot.collectorRightServo.setPower(-1);
break;
case "dpad_down":
robot.collectorLeftServo.setPower(1);
robot.collectorRightServo.setPower(1);
}
}
@Override
public void buttonUp(Gamepad gamepad, String button) {
if (gamepad != controller) {
return;
}
switch (button) {
// Collector control
case "dpad_up":
case "dpad_down":
robot.collectorLeftServo.setPower(0);
robot.collectorRightServo.setPower(0);
break;
}
}
}

118
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/states/teleop/DriveController.java Normal file
View File

@@ -0,0 +1,118 @@
package org.timecrafters.minibots.cyberarm.phoenix.states.teleop;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.minibots.cyberarm.phoenix.Robot;
public class DriveController extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final Gamepad controller;
private boolean robotSlowMode;
public DriveController(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.controller = engine.gamepad1;
this.robotSlowMode = true;
}
@Override
public void exec() {
move();
}
@Override
public void stop() {
stopDrive();
}
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != controller) {
return;
}
// DEBUG: Toggle hardware fault
if (button.equals("guide")) {
robot.hardwareFault = !robot.hardwareFault;
if (robot.hardwareFault) {
robot.hardwareFaultMessage = "Manually triggered.";
} else {
robot.hardwareFaultMessage = "";
}
}
if (button.equals("right_stick_button")) {
robotSlowMode = !robotSlowMode;
}
if (button.equals("left_stick_button") && robot.hardwareFault) {
robot.imu.resetYaw();
}
}
// REF: https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
private void move() {
if (robot.automaticAntiTipActive || robot.hardwareFault) {
return;
}
double x = controller.left_stick_x;
double y = -controller.left_stick_y;
// Improve control?
if (y < 0) {
y = -Math.sqrt(-y);
} else {
y = Math.sqrt(y);
}
if (x < 0) {
x = -Math.sqrt(-x) * 1.1; // Counteract imperfect strafing;
} else {
x = Math.sqrt(x) * 1.1; // Counteract imperfect strafing;
}
double rx = -controller.right_stick_x;
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0;
double heading = robot.heading();
double rotX = x * Math.cos(heading) - y * Math.sin(heading);
double rotY = x * Math.sin(heading) + y * Math.cos(heading);
frontLeftPower = (rotY + rotX + rx) / denominator;
backLeftPower = (rotY - rotX - rx) / denominator;
frontRightPower = (rotY - rotX + rx) / denominator;
backRightPower = (rotY + rotX - rx) / denominator;
double maxVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_max_velocity_in_inches").value());
double slowVelocity = robot.unitToTicks(DistanceUnit.INCH, robot.tuningConfig("drivetrain_slow_velocity_in_inches").value());
double velocity = robotSlowMode ? slowVelocity : maxVelocity;
// Power is treated as a ratio here
robot.frontLeftDrive.setVelocity(frontLeftPower * velocity);
robot.frontRightDrive.setVelocity(frontRightPower * velocity);
robot.backLeftDrive.setVelocity(backLeftPower * velocity);
robot.backRightDrive.setVelocity(backRightPower * velocity);
}
private void stopDrive() {
robot.backLeftDrive.setVelocity(0); robot.backLeftDrive.setPower(0);
robot.frontRightDrive.setVelocity(0); robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setVelocity(0); robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setVelocity(0); robot.backRightDrive.setPower(0);
}
}

22
TeamCode/src/main/java/org/timecrafters/minibots/engines/Mini2023Engine.java Normal file
View File

@@ -0,0 +1,22 @@
package org.timecrafters.minibots.engines;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.minibots.states.Mini2023Bot;
import org.timecrafters.minibots.states.Mini2023State;
@TeleOp (name= "2023Mini")
public class Mini2023Engine extends CyberarmEngine {
Mini2023Bot robot;
@Override
public void setup() {
robot = new Mini2023Bot(this);
addState(new Mini2023State(robot));
}
}

49
TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java Normal file
View File

@@ -0,0 +1,49 @@
package org.timecrafters.minibots.states;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.IMU;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.minibots.engines.Mini2023Engine;
public class Mini2023Bot {
private final Mini2023Engine engine;
public TimeCraftersConfiguration configuration;
public DcMotor Opportunity, Spirit; //Don't ask. Just don't.
public CRServo servoA, servoB, servoC; //Just be thankful the servos have 'normal' names.
public IMU imu;
public ModernRoboticsI2cRangeSensor hazcam;
public Mini2023Bot(Mini2023Engine engine) {
this.engine = engine;
setupRobot();
}
private void setupRobot() {
Spirit = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
Opportunity = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
servoA = engine.hardwareMap.get(CRServo.class, "Servo 1");
servoB = engine.hardwareMap.get(CRServo.class, "Servo 2");
servoC = engine.hardwareMap.get(CRServo.class, "Servo 3");
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.UP
));
this.imu = engine.hardwareMap.get(IMU.class, "imu");
imu.initialize(parameters);
// configuration = new TimeCraftersConfiguration("2023 Mini");
}
}

77
TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java Normal file
View File

@@ -0,0 +1,77 @@
package org.timecrafters.minibots.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
public class Mini2023State extends CyberarmState {
private final Mini2023Bot robot;
public double lThrust, rThrust, orbitSpeed;
public double deltaServo;
public Mini2023State(Mini2023Bot robot) {this.robot = robot;}
public double getDeltaServo() {
return deltaServo;
}
@Override public void telemetry() {
engine.telemetry.addData("Right Drive Power", robot.Opportunity.getPower());
engine.telemetry.addData("Left Drive Power", robot.Spirit.getPower());
}
@Override
public void init() {
lThrust = 0;
rThrust = 0;
orbitSpeed = 0;
robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed);
robot.Spirit.setPower(lThrust);
robot.Opportunity.setPower(rThrust);
}
@Override
public void exec() {
if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) {
lThrust = engine.gamepad1.left_stick_y * 0.5;
robot.Spirit.setPower(lThrust);
} else {
lThrust = 0;
robot.Spirit.setPower(lThrust);
}
if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) {
rThrust = engine.gamepad1.right_stick_y * 0.5;
robot.Opportunity.setPower(rThrust);
} else {
rThrust = 0;
robot.Opportunity.setPower(rThrust);
}
if (engine.gamepad1.right_trigger > 0.1) {
orbitSpeed = engine.gamepad1.right_trigger * 0.5;
robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed);
} else if (engine.gamepad1.left_trigger > 0.1) {
orbitSpeed = engine.gamepad1.left_trigger * 0.5;
robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed);
} else {
orbitSpeed = 0;
robot.servoA.setPower(orbitSpeed);
robot.servoB.setPower(orbitSpeed);
robot.servoC.setPower(orbitSpeed);
}
}
}

2
build.common.gradle
View File

@@ -51,7 +51,7 @@ android {
defaultConfig {
signingConfig signingConfigs.debug
applicationId 'com.qualcomm.ftcrobotcontroller'
minSdkVersion 23
minSdkVersion 24
//noinspection ExpiredTargetSdkVersion
targetSdkVersion 28

1
build.dependencies.gradle
View File

@@ -22,5 +22,6 @@ dependencies {
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.8'
implementation 'org.ftclib.ftclib:core:2.1.1'
}