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

Compare commits

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

8 Commits
fa54f5f209 ... 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
51 changed files with 553 additions and 167 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())) + '"'

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

@@ -1,20 +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.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.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")
@Disabled

4
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftStateAutoEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/LeftStateAutoEngine.java
View File

@@ -1,9 +1,9 @@
package org.timecrafters.Autonomous.Engines;
package org.timecrafters.Phoenix.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous(name = "Left Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
public class LeftStateAutoEngine extends CyberarmEngine {

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

@@ -1,19 +1,19 @@
package org.timecrafters.Autonomous.Engines;
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.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")

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

9
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/Engines/RightStateAutoEngine.java
View File

@@ -1,14 +1,9 @@
package org.timecrafters.Autonomous.Engines;
package org.timecrafters.Phoenix.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.TopArmv2;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
@Autonomous(name = "Right Side", group = "A Phoenix", preselectTeleOp = "APhoenixTeleOP")
public class RightStateAutoEngine extends CyberarmEngine {

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

@@ -1,21 +1,19 @@
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")

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 {

4
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 {

4
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 {

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;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/DriverParkPlaceState.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 DriverParkPlaceState extends CyberarmState {
private final boolean stateDisabled;

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;

4
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;

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.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.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class JunctionAllignmentDistanceState extends CyberarmState {
private final boolean stateDisabled;

4
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.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class PathDecision extends CyberarmState {
PhoenixBot1 robot;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/RotationState.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.AngleUnit;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Phoenix.PhoenixBot1;
public class RotationState extends CyberarmState {
private final boolean stateDisabled;

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 {

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmResetState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmResetState.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 TopArmResetState extends CyberarmState {
private final PhoenixBot1 robot;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java → TeamCode/src/main/java/org/timecrafters/Phoenix/Autonomous/States/TopArmv2.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 TopArmv2 extends CyberarmState {
private final PhoenixBot1 robot;

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

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Phoenix;
import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
import com.qualcomm.hardware.bosch.BNO055IMU;

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/TeleOp/engine/PhoenixTeleOP.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixTeleOP.java
View File

@@ -1,11 +1,11 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Phoenix.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.TeleOp.states.TeleOPArmDriver;
import org.timecrafters.TeleOp.states.TeleOPTankDriver;
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/TeleOp/engine/PhoenixWingEngine.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/engine/PhoenixWingEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.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/TeleOp/states/LaserServoHeight.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserServoHeight.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.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/TeleOp/states/LaserState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/LaserState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.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/TeleOp/states/PhoenixTeleOPState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.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/TeleOp/states/PhoenixTeleOPv2.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixTeleOPv2.java
View File

@@ -1,6 +1,7 @@
package org.timecrafters.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/TeleOp/states/PhoenixWingState.java → TeamCode/src/main/java/org/timecrafters/Phoenix/TeleOp/states/PhoenixWingState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.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 {

3
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;

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 {

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;
}
}

131
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/phoenix/Robot.java
View File

@@ -3,18 +3,15 @@ package org.timecrafters.minibots.cyberarm.phoenix;
import android.annotation.SuppressLint;
import android.util.Log;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.Blinker;
import com.qualcomm.robotcore.hardware.CRServoImplEx;
import com.qualcomm.robotcore.hardware.ColorSensor;
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 com.qualcomm.robotcore.hardware.ServoImplEx;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
@@ -27,7 +24,6 @@ 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 org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer;
import java.util.ArrayList;
import java.util.concurrent.ConcurrentHashMap;
@@ -37,6 +33,9 @@ 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;
@@ -52,6 +51,7 @@ public class Robot {
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,
@@ -134,6 +134,7 @@ public class Robot {
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);
@@ -147,11 +148,45 @@ public class Robot {
// 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.UP,
RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD));
new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
RevHubOrientationOnRobot.UsbFacingDirection.UP
)
);
imu.initialize(parameters);
@@ -259,6 +294,8 @@ public class Robot {
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()));
@@ -301,6 +338,31 @@ public class Robot {
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());
@@ -394,6 +456,8 @@ public class Robot {
if (lastStatus == Status.DANGER) { expansionHub.setPattern(ledPatternDanger()); }
}
}
manageArmAndRiserServos();
}
public void stop() {
@@ -407,32 +471,67 @@ public class Robot {
return;
}
armTargetPosition = position;
reportStatus(Status.WARNING);
}
switch (position) {
private void manageArmAndRiserServos() {
boolean lowerServos = true;
switch (armTargetPosition) {
case COLLECT:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_collect").value()));
}
break;
case GROUND:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_ground").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:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_low").value()));
}
break;
case MEDIUM:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
if (areRiserServosInLoweredPosition()) {
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_medium").value()));
}
break;
case HIGH:
arm.setTargetPosition(angleToTicks(tuningConfig("arm_position_angle_high").value()));
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

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

@@ -11,6 +11,7 @@ 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;
@@ -20,6 +21,8 @@ public class ArmController extends CyberarmState {
this.groupName = groupName;
this.actionName = actionName;
this.controller = engine.gamepad1;
pidController = new PIDController(p, i, d);
}
@@ -38,7 +41,7 @@ public class ArmController extends CyberarmState {
case "derivative":
d = v.value();
break;
case "feed":
case "feed": // feedback
f = v.value();
break;
}
@@ -56,7 +59,7 @@ public class ArmController extends CyberarmState {
@Override
public void buttonDown(Gamepad gamepad, String button) {
if (gamepad != engine.gamepad2) {
if (gamepad != controller) {
return;
}
@@ -64,17 +67,55 @@ public class ArmController extends CyberarmState {
case "guide":
robot.reloadConfig();
break;
// Arm control
case "a":
robot.armPosition(Robot.ArmPosition.COLLECT);
break;
case "x":
robot.armPosition(Robot.ArmPosition.GROUND);
robot.armPosition(Robot.ArmPosition.LOW); // DISABLED GROUND JUNCTION
break;
case "b":
robot.armPosition(Robot.ArmPosition.LOW);
robot.armPosition(Robot.ArmPosition.MEDIUM);
break;
case "y":
robot.armPosition(Robot.ArmPosition.MEDIUM);
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);
}
}

21
TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023Bot.java
View File

@@ -1,7 +1,10 @@
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;
@@ -11,8 +14,10 @@ public class Mini2023Bot {
private final Mini2023Engine engine;
public TimeCraftersConfiguration configuration;
public DcMotor Opportunity, Spirit, Ingenuity;
public CRServo servoA, servoB, servoC;
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;
@@ -21,13 +26,21 @@ public class Mini2023Bot {
private void setupRobot() {
Opportunity = engine.hardwareMap.get(DcMotor.class, "Left Wheel");
Spirit = engine.hardwareMap.get(DcMotor.class, "Right Wheel");
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");

61
TeamCode/src/main/java/org/timecrafters/minibots/states/Mini2023State.java
View File

@@ -1,43 +1,74 @@
package org.timecrafters.minibots.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.engines.Mini2023Engine;
public class Mini2023State extends CyberarmState {
private final Mini2023Bot robot;
public double driveSpeed, orbitSpeed;
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() {
driveSpeed = 0;
lThrust = 0;
rThrust = 0;
orbitSpeed = 0;
robot.servoA.setPower(0);
robot.servoB.setPower(0);
robot.servoC.setPower(0);
robot.Opportunity.setPower(driveSpeed);
robot.Spirit.setPower(driveSpeed);
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 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) {
orbitSpeed = engine.gamepad2.left_stick_y;
robot.Opportunity.setPower(driveSpeed);
robot.Spirit.setPower(driveSpeed);
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_x) > 0.1) {
orbitSpeed = engine.gamepad1.right_stick_x * 0.75;
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);
}