From 26a16c50b599fdb9562bb41201d291a4ccab7212 Mon Sep 17 00:00:00 2001 From: KOOLPOOL <141171610+Kooolpool@users.noreply.github.com> Date: Tue, 17 Jun 2025 10:43:55 -0700 Subject: [PATCH 1/4] added some subsystems --- Osprey/build.gradle | 39 +++ Osprey/lib/OpModeAnnotationProcessor.jar | Bin 0 -> 6032 bytes Osprey/src/main/AndroidManifest.xml | 11 + .../ftc/ptechnodactyl/Hardware.java | 91 +++++++ .../ftc/ptechnodactyl/Robot.java | 33 +++ .../ftc/ptechnodactyl/Setup.java | 52 ++++ .../commands/DrivingCommands.java | 39 +++ .../ftc/ptechnodactyl/commands/EZCmd.java | 27 ++ .../commands/JoystickDriveCommand.java | 139 +++++++++++ .../commands/JoystickIncDecCmd.java | 0 .../controllers/DriverController.java | 52 ++++ .../controllers/OneController.java | 0 .../controllers/OperatorController.java | 52 ++++ .../controllers/TestController.java | 0 .../ptechnodactyl/helpers/HeadingHelper.java | 75 ++++++ .../helpers/StartingPosition.java | 7 + .../ftc/ptechnodactyl/opmodes/auto/Basic.java | 62 +++++ .../opmodes/tele/JustDrivingTeleOp.java | 42 ++++ .../ptechnodactyl/opmodes/tele/MotorTest.java | 0 .../opmodes/tele/PlainDrive.java | 28 +++ .../opmodes/tele/oneController.java | 0 .../firstinspires/ftc/ptechnodactyl/readme.md | 140 +++++++++++ .../subsystems/ClawSubsystem.java | 0 .../subsystems/DrivebaseSubsystem.java | 234 ++++++++++++++++++ Osprey/src/main/res/raw/readme.md | 1 + Osprey/src/main/res/values/strings.xml | 4 + .../main/res/xml/teamwebcamcalibrations.xml | 161 ++++++++++++ .../ftc/ptechnodactyl/Robot.java | 9 +- .../controllers/OperatorController.java | 16 +- .../opmodes/tele/JustDrivingTeleOp.java | 5 +- .../subsystems/ArmSubsystem.java | 149 +++++++++++ .../subsystems/BarcodePipeline.java | 192 ++++++++++++++ .../subsystems/BrakeSubsystem.java | 38 +++ .../subsystems/CapSubsystem.java | 85 +++++++ .../subsystems/CarouselSubsystem.java | 70 ++++++ 35 files changed, 1830 insertions(+), 23 deletions(-) create mode 100644 Osprey/build.gradle create mode 100644 Osprey/lib/OpModeAnnotationProcessor.jar create mode 100644 Osprey/src/main/AndroidManifest.xml create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java rename {Ptechnodactyl => Osprey}/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java (100%) create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java rename {Ptechnodactyl => Osprey}/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java (100%) create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java rename {Ptechnodactyl => Osprey}/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java (100%) create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java rename {Ptechnodactyl => Osprey}/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java (100%) create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java rename {Ptechnodactyl => Osprey}/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java (100%) create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md rename {Ptechnodactyl => Osprey}/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java (100%) create mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java create mode 100644 Osprey/src/main/res/raw/readme.md create mode 100644 Osprey/src/main/res/values/strings.xml create mode 100644 Osprey/src/main/res/xml/teamwebcamcalibrations.xml create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java diff --git a/Osprey/build.gradle b/Osprey/build.gradle new file mode 100644 index 00000000..40bc5902 --- /dev/null +++ b/Osprey/build.gradle @@ -0,0 +1,39 @@ +// +// build.gradle in Ptechnodactyl +// +// Most of the definitions for building your module reside in a common, shared +// file 'build.common.gradle'. Being factored in this way makes it easier to +// integrate updates to the FTC into your code. If you really need to customize +// the build definitions, you can place those customizations in this file, but +// please think carefully as to whether such customizations are really necessary +// before doing so. + + +// Custom definitions may go here + +// Include common definitions from above. +apply from: '../build.common.gradle' +apply from: '../build.dependencies.gradle' + +android { + namespace = 'org.firstinspires.ftc.learnbot' + androidResources { + noCompress 'tflite' + } + + packagingOptions { + jniLibs { + pickFirsts += ['**/*.so'] + } + jniLibs.useLegacyPackaging true + } +} + +dependencies { + implementation project(':FtcRobotController') + // Uncomment this to use a local version of TechnoLib + // implementation project(':RobotLibrary') // FLIP: TechnoLibLocal + // implementation project(':Path') // FLIP: TechnoLibLocal + // implementation project(':Vision') // FLIP: TechnoLibLocal + annotationProcessor files('lib/OpModeAnnotationProcessor.jar') +} diff --git a/Osprey/lib/OpModeAnnotationProcessor.jar b/Osprey/lib/OpModeAnnotationProcessor.jar new file mode 100644 index 0000000000000000000000000000000000000000..4825cc36d9606badc3c0eac0f61079f9c5a4d196 GIT binary patch literal 6032 zcmbVQ2UL?;(+*83p-BxzlwKk&NReKJgwU&k8X!OblRy9)0@8(msC0-@1f+)|MG>X< zBH$`gY&1cTZUOaAR##Ye@n645a?X2BX6D{^X6}9F88a9aH9LTomKK1zJZ=WqA?yHZ z09@Zn`;d`|fec*R#K=J3(n=a`@TCg?;OLfP3O)d&86bqIVkx3Zi~EWfV>yTPjt};K z&T063*y0U7^^AWM3hrs|vD|!Hc7VwW=< z^USZe+R{J+$(@&@(xgzGcK*h3phqME+wC~xVm~4Yd|4%D)xvuo6tduY12Xnl0ytqU zxrXyh{-f-AUiDB9>U^0ioy2BWCPcWSz0G8Z$C!z>+dmA!72djV%N`src3M8*kvwj! z8ZK4mT;=98(Ukdt0hs=Z5LtydWbYGpqsJ$|mE-6&OO0y{C9-?2gDPDnhuVllRMSv7>=Eo06tDbqH+jLqAri*`wQp}j|{^x&)vPV!+G;`Q5W4eh9^reuRZI? zPm!>hCzRFWt*`r4jlJYQ_9z@4X+nWKPT&ZQm`(sg^;%}qIu};K8HZsuaTwN$kk#&M zhK9Dl_0(%GqbT2TR3E4{lfE0M#I->yGYdI<4!waD{uTGKZYxf? z^wqg~t=cd14-1tqadBi=E$@qitc{^KyB$ut+Nn#$5fTcyS=65Aa?}dfMO&WLZ!J0t zZ=FA@+nR0>0PCA*Qt{X-a{e3XtWqyNR4 zVH_8d6IkafIz&Ll-AND69vB~XqT=Dd6d=f#gko;(xny_7doRspP(m1gH!gA}N~D}( zC|X4~!@yDL%Dzx1m3NdLOzpF7i3`lWX?X{H-wR}q(`#6JbKdNv4D2HY#>FDs?}D-{ zKUX`WRPuWo{G*fjnQ9b{O7dc69JSvs9XJyZGNK`LHn0X8G0U|4+!kFZ?Rks`UCZf_ zCo!AGa@}z5)zmZ_?bPAJw?-TTTD;4A&)d^5+Sa-?H56A5rm>*83~n4;x;+Cs>?&W$be#pGtaO!;J`zjZ6RkOi$qkig* zr*a<)AfZS`?T%<2^O+M{W_^&PPX(1G?|SV==RCTiAa{!N6+73DU_8bdEVHHx;vap8 zEx{f4jBy@YfLwI70zxD7Uh_jXR0NsFSZdT))YWGKdjjDwt<|zJzKho%jmWr%c{y*j z-CZOcEK4mEARsWhGD*%f7el*m0#fRYXF*~TMnXAqRi7xIzF+jAQkQa^a<+q;K88Hh zb-#6Wg2^zR<22J&h@|+fBj;EzH89+s^(bbH6Jmd;ED1^rT^E+XpD+sbv1!H9J?_o= zG@#rBbdJhs-H0*U6!(SWkQb2`XZKCVzLu_2{Hn%j*sJVuaTzTE{raA|gE`hbV)eFX zVHsa@)A&p&0afg{rn&dzpH8eccHcAgtf1S06lIbYE!=vN&IqQuUF7q2gkr5lrGs)KePp*LABX z!o7|4f&=QNdmrSqQIEHQ>~#8FJ&dIe7sk4#6G0MduZ*x&L+Us4JU1okLzn_$7HWg} zYw^-saps+%4ocX?TF2l(6b3&M1bWyc6!7+Vd=D!*I|8sAnptE^(vkcVl9bw zI-Enpk-%zpl&62ryJF>!XYLDh$=!3$?QWmC9x}|q_lep|=?NYcPiQqg6uvg@z2g_k)T} zef9tVlT6=$_us+I=6?q_VO=t~X`@}Vf553B#>)li^9}V%ERUPAX|sRX1LaDRC}|ap z=CT^D7LT#ji`O#5i%w;Wi#DWW@jq)^K#vpBg5{Pm9|Qula#B9LU|u>3>i4oez*w)a z=;}Z1Fs(YHI`j6Ore+XDZD{00qW6e_W}DX$$8v9uCwS@SF**X#+)7P;yh^eN#9$LV z;;rkD-%Z8vBE`gF@Leaj$b{TO%O=XDOND)DObNCG#H>UOo~mgz;84}deWZCZ(8$s9 zOuKXpmxl@MWj3aedN4Y%0!i`&W0#W z|GKtI^5nEgg>?~HaDh8h+4q+^_d1(1ln;!OWu zWlwyCZ0|Av!s!aK`Kn|OT@3E+YD)rpMl9BY;*3?{h0y`9W>Z4%6l~#1b_IV!CBKKg znCt_i_`0MAD+KG1L27Y*!kpsSu(U=G?#}gRl&y~yJK=tIwD_j26|E4?qx93DsP|PP z5YQ0cwYw;!9POM6VR}Gc<`h@^gHVp3%y|YQ$BsuzMjF8}2R||0smId02^y>FKsH!2 zV#}wU!5R&bi-INU!WuPMZ*<)Q1Y%$F5%7dIQG*L`4Fkxrt%w_5*N7SiH#s;j-@h;m z87-AtFv##>m1+)5ADFNVV@?=p7w5seYrTG3jM9R6D>NSRN5>TPbYQabJ|*JG0W`g%7P)0QC!+; zEi{75FACU3%aM7iYrfQ)FlPGwTL4XHF^EcX(n(;>ysYrVgA*DW&nGC}BARtsA<`Zx zhY)?uYMgxFBX&;W`wc}F4(gVck03e(-taccKE4LcV0=|7n_SP!YKeNY|H_fE4h4a0 z=Prn`MT{pYBCt|$r(Ec#!6@N@&nXGD7SDX7?#%Iyvl(T>v0b?ygI>lRvZJ-=@zuTi z^&G;rM|ldGT&Kg**+sR;JNYNHRG<5Uyvf6S^7I%HZI zwQ>8ffg|;t_`vI2Pas%bKtH)6IG&ZW`b6W6ii)Gu+9W zeUC4PF4BAypt!;^qLF)2P`WEc^wyPlKbr?6-msUpj$(%DGz9?QKuYTFEP;-c&>!q( zcQrDZ_@RbF`uL+rEMNx((EF)Ql>SxU1L2Q2E4@u7q`$KX6xvPt2LujGHRRuUa)CSz6cuPZ+ z$#k*~q4e+X!{mFE-ZEEA5=ut#o}_m_VGlD=fmg@Kbn%J-sI6Y(10*vQ1#jw~En$QW z>nc1oRjtDZH)qwDh<&*2cHRqH0Aa(fbVENYxFieS%=A-6%;Y$UTG`{{HXql?-*7aI zsk_nWAn9OJ@r12S|N$^^D?{81gdDjudb^4MM?69gpX7(jzCr`mE$jq)k1?A zih}5SN~eT}GyBS0ar1mi+#>;dE)i)M-KRlKif;q{EYq>H$v6$tq2&2~D8Ka>Kj%mJ zH}a$H=ZisOPGkIV+c|31Vu^6uL{tSUPg8E2c6+EJ^9J=WKH6MsX6>ziDJRqe zsiRVka%qyHSZI)QNsR3PE?KLbrU?!{pK=XGtspu%2q+Wk-PhYt=^`$gOqj8tWNPbs zD&Cea9+StzP-6UWSw?kzD4vSm0pIotKhE3@I+=U$*y0l|sH(q)#7qPaP9iAvK=a|R z=M>nnR()}gaY|g3KbTS z>owz4-8UfVwLcFBJX*ztW^|ir}0)qo|CQ zuUMy~%%VHwKJ#aqsK~d;sEZGqHYrVlf2N6ok{z%;8`?RaAoXNAF zZILI%@OM1_V-iFT+h4&aFSWBvA$Q{2!b}>H{f|riKYjVmJ&^mMoxn=&hqi^0;Wzj2 zqdWRld5|G`TPA=zkoZ?-f3Fc560rTK%^d)~9bPho??h2Dl5Wf4f3^Qlfc~pZ{{8s$ zkQVs{&%2i{8KAdi@w>$ixZd5A4A|QOBGEkZVn0lG!uRe@$N;`ACc8V?3Fo`JBLn)j z#Qqm|Kj3{kcVqzB2`yxR-*csNk*u literal 0 HcmV?d00001 diff --git a/Osprey/src/main/AndroidManifest.xml b/Osprey/src/main/AndroidManifest.xml new file mode 100644 index 00000000..3705b315 --- /dev/null +++ b/Osprey/src/main/AndroidManifest.xml @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java new file mode 100644 index 00000000..930868d6 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java @@ -0,0 +1,91 @@ +package org.firstinspires.ftc.ptechnodactyl; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.hardware.motor.Motor; +import com.technototes.library.hardware.sensor.AdafruitIMU; +import com.technototes.library.hardware.sensor.ColorDistanceSensor; +import com.technototes.library.hardware.sensor.IGyro; +import com.technototes.library.hardware.sensor.IMU; +import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.logger.Loggable; +import com.technototes.vision.hardware.Webcam; +import java.util.List; +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; + +public class Hardware implements Loggable { + + public List hubs; + + public EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor, arm; + public Motor placeholder1; + public DcMotorEx liftMotor; + + public Servo placeholder2; + public Servo servo, clawServo; + + public IGyro imu; + public Webcam camera; + public Rev2MDistanceSensor distanceSensor; + public ColorDistanceSensor colorSensor; + + public Hardware(HardwareMap hwmap) { + hubs = hwmap.getAll(LynxModule.class); + if (Setup.Connected.EXTERNAL_IMU) { + imu = new AdafruitIMU(Setup.HardwareNames.EXTERNAL_IMU, AdafruitIMU.Orientation.Pitch); + Setup.HardwareNames.MOTOR = "External IMU is in use"; + } else { + imu = new IMU( + Setup.HardwareNames.IMU, + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD + ); + Setup.HardwareNames.MOTOR = "Internal IMU is being used"; + } + if (Setup.Connected.DRIVEBASE) { + this.frMotor = new EncodedMotor<>(Setup.HardwareNames.FRMOTOR); + this.flMotor = new EncodedMotor<>(Setup.HardwareNames.FLMOTOR); + this.rrMotor = new EncodedMotor<>(Setup.HardwareNames.RRMOTOR); + this.rlMotor = new EncodedMotor<>(Setup.HardwareNames.RLMOTOR); + if (Setup.Connected.DISTANCE_SENSOR) { + this.distanceSensor = new Rev2MDistanceSensor(Setup.HardwareNames.DISTANCE); + } + } + if (Setup.Connected.MOTOR) { + this.theMotor = new EncodedMotor<>(Setup.HardwareNames.MOTOR); + } + if (Setup.Connected.FLYWHEEL) { + this.placeholder1 = new Motor<>(Setup.HardwareNames.FLYWHEELMOTOR); + } + if (Setup.Connected.WEBCAM) { + camera = new Webcam(Setup.HardwareNames.CAMERA); + } + if (Setup.Connected.TESTSUBSYSTEM) { + if (Setup.Connected.SERVO) { + this.servo = new Servo(Setup.HardwareNames.SERVO); + } + // if (Setup.Connected.COLOR_SENSOR) { + // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); + // } + } + if (Setup.Connected.CLAWSUBSYSTEM) { + this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO); + this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM); + } + } + + // We can read the voltage from the different hubs for fun... + public double voltage() { + double volt = 0; + double count = 0; + for (LynxModule lm : hubs) { + count += 1; + volt += lm.getInputVoltage(VoltageUnit.VOLTS); + } + return volt / count; + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java new file mode 100644 index 00000000..0a34a722 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.ptechnodactyl; + +import com.technototes.library.logger.Loggable; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +public class Robot implements Loggable { + + public StartingPosition position; + public Alliance alliance; + public double initialVoltage; + public DrivebaseSubsystem drivebaseSubsystem; + public ClawSubsystem clawSubsystem; + + public Robot(Hardware hw) { + this.initialVoltage = hw.voltage(); + if (Setup.Connected.DRIVEBASE) { + this.drivebaseSubsystem = new DrivebaseSubsystem( + hw.flMotor, + hw.frMotor, + hw.rlMotor, + hw.rrMotor, + hw.imu + ); + } + if (Setup.Connected.CLAWSUBSYSTEM) { + this.clawSubsystem = new ClawSubsystem(hw); + } + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java new file mode 100644 index 00000000..39a497ac --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.ptechnodactyl; + +import com.acmerobotics.dashboard.config.Config; + +public class Setup { + + @Config + public static class Connected { + + public static boolean DRIVEBASE = true; + public static boolean TESTSUBSYSTEM = false; + public static boolean CLAWSUBSYSTEM = true; + public static boolean MOTOR = false; + public static boolean SERVO = false; + public static boolean ARM = true; + public static boolean CLAWSERVO = true; + public static boolean DISTANCE_SENSOR = false; + public static boolean COLOR_SENSOR = false; + public static boolean FLYWHEEL = false; + public static boolean WEBCAM = false; + public static boolean EXTERNAL_IMU = false; + } + + @Config + public static class HardwareNames { + + public static String MOTOR = "motor"; + public static String FLMOTOR = "fl"; + public static String FRMOTOR = "fr"; + public static String RLMOTOR = "rl"; + public static String RRMOTOR = "rr"; + public static String FLYWHEELMOTOR = "fly"; + public static String SERVO = "s"; + public static String IMU = "imu"; + public static String EXTERNAL_IMU = "adafruit-imu"; + public static String DISTANCE = "d"; + public static String COLOR = "c"; + public static String CAMERA = "camera"; + public static String ARM = "arm"; + public static String CLAWSERVO = "clawServo"; + } + + @Config + public static class GlobalSettings { + + public static double STICK_DEAD_ZONE = 0.1; + public static double STRAIGHTEN_SCALE_FACTOR = 0.25; + public static double STRAIGHTEN_RANGE = .15; // Fraction of 45 degrees... + public static double TRIGGER_THRESHOLD = 0.7; + public static double STRAIGHTEN_DEAD_ZONE = 0.08; + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java new file mode 100644 index 00000000..2e11e466 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.ptechnodactyl.commands; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +public class DrivingCommands { + + public static Command NormalDriving(DrivebaseSubsystem ds) { + return Command.create(ds::setNormalMode); + } + + public static Command SnailDriving(DrivebaseSubsystem ds) { + return Command.create(ds::setSnailMode); + } + + public static Command TurboDriving(DrivebaseSubsystem ds) { + return Command.create(ds::setTurboMode); + } + + public static Command NormalSpeed(DrivebaseSubsystem ds) { + return Command.create(ds::auto); + } + + public static Command SlowSpeed(DrivebaseSubsystem ds) { + return Command.create(ds::slow); + } + + public static Command FastSpeed(DrivebaseSubsystem ds) { + return Command.create(ds::fast); + } + + public static Command ResetGyro(DrivebaseSubsystem ds) { + return Command.create(ds::setExternalHeading, 0.0); + } + + public static Command RecordHeading(DrivebaseSubsystem drive) { + return Command.create(drive::saveHeading); + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java new file mode 100644 index 00000000..934521a4 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.ptechnodactyl.commands; + +import com.technototes.library.command.Command; +import com.technototes.library.command.MethodCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +public class EZCmd { + + public static class Drive { + + public static Command TurboMode(DrivebaseSubsystem db) { + return new MethodCommand(db::setTurboMode); + } + + public static Command NormalMode(DrivebaseSubsystem db) { + return new MethodCommand(db::setNormalMode); + } + + public static Command SnailMode(DrivebaseSubsystem db) { + return new MethodCommand(db::setSnailMode); + } + + public static Command ResetGyro(DrivebaseSubsystem db) { + return new MethodCommand(db::setExternalHeading, 0.0); + } + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java new file mode 100644 index 00000000..74ef7baf --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java @@ -0,0 +1,139 @@ +package org.firstinspires.ftc.ptechnodactyl.commands; + +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.technototes.library.command.Command; +import com.technototes.library.control.Stick; +import com.technototes.library.logger.Loggable; +import com.technototes.library.util.MathUtils; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +public class JoystickDriveCommand implements Command, Loggable { + + public DrivebaseSubsystem subsystem; + public DoubleSupplier x, y, r; + public BooleanSupplier watchTrigger; + public double targetHeadingRads; + public DoubleSupplier driveStraighten; + public DoubleSupplier drive45; + public boolean driverDriving; + public boolean operatorDriving; + + public JoystickDriveCommand( + DrivebaseSubsystem sub, + Stick xyStick, + Stick rotStick, + DoubleSupplier strtDrive, + DoubleSupplier angleDrive + ) { + addRequirements(sub); + subsystem = sub; + x = xyStick.getXSupplier(); + y = xyStick.getYSupplier(); + r = rotStick.getXSupplier(); + targetHeadingRads = -sub.getExternalHeading(); + driveStraighten = strtDrive; + drive45 = angleDrive; + driverDriving = true; + operatorDriving = false; + } + + // Use this constructor if you don't want auto-straightening + public JoystickDriveCommand(DrivebaseSubsystem sub, Stick xyStick, Stick rotStick) { + this(sub, xyStick, rotStick, null, null); + } + + // This will make the bot snap to an angle, if the 'straighten' button is pressed + // Otherwise, it just reads the rotation value from the rotation stick + private double getRotation(double headingInRads) { + // Check to see if we're trying to straighten the robot + double normalized = 0.0; + boolean straightTrigger; + boolean fortyfiveTrigger; + straightTrigger = isTriggered(driveStraighten); + fortyfiveTrigger = isTriggered(drive45); + if (!straightTrigger && !fortyfiveTrigger) { + // No straighten override: return the stick value + // (with some adjustment...) + return -Math.pow(r.getAsDouble(), 3) * subsystem.speed; + } + if (straightTrigger) { + // headingInRads is [0-2pi] + double heading = -Math.toDegrees(headingInRads); + // Snap to the closest 90 or 270 degree angle (for going through the depot) + double close = MathUtils.closestTo(heading, 0, 90, 180, 270, 360); + double offBy = close - heading; + // Normalize the error to -1 to 1 + normalized = Math.max(Math.min(offBy / 45, 1.), -1.); + // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem) + if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) { + return 0.0; + } + } else { + // headingInRads is [0-2pi] + double heading45 = -Math.toDegrees(headingInRads); + // Snap to the closest 90 or 270 degree angle (for going through the depot) + double close45 = MathUtils.closestTo(heading45, 45, 135, 225, 315); + double offBy45 = close45 - heading45; + // Normalize the error to -1 to 1 + normalized = Math.max(Math.min(offBy45 / 45, 1.), -1.); + // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem) + if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) { + return 0.0; + } + } + // Scale it by the cube root, the scale that down by 30% + // .9 (about 40 degrees off) provides .96 power => .288 + // .1 (about 5 degrees off) provides .46 power => .14 + return Math.cbrt(normalized) * 0.3; + } + + public static boolean isTriggered(DoubleSupplier ds) { + if (ds == null || ds.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD) { + return false; + } + return true; + } + + @Override + public void execute() { + // If subsystem is busy it is running a trajectory. + if (!subsystem.isBusy()) { + double curHeading = -subsystem.getExternalHeading(); + + // The math & signs looks wonky, because this makes things field-relative + // (Remember that "3 O'Clock" is zero degrees) + double yvalue = y.getAsDouble(); + double xvalue = x.getAsDouble(); + if (driveStraighten != null) { + if (driveStraighten.getAsDouble() > 0.7) { + if (Math.abs(yvalue) > Math.abs(xvalue)) xvalue = 0; + else yvalue = 0; + } + } + Vector2d input = new Vector2d( + yvalue * subsystem.speed, + xvalue * subsystem.speed + ).rotated(curHeading); + + subsystem.setWeightedDrivePower( + new Pose2d(input.getX(), input.getY(), getRotation(curHeading)) + ); + } + subsystem.update(); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + if (cancel) subsystem.setDriveSignal(new DriveSignal()); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java similarity index 100% rename from Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java rename to Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java new file mode 100644 index 00000000..2727dddc --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandAxis; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; + +public class DriverController { + + public Robot robot; + public CommandGamepad gamepad; + + public Stick driveLeftStick, driveRightStick; + public CommandButton resetGyroButton, turboButton, snailButton; + public CommandButton override; + + public DriverController(CommandGamepad g, Robot r) { + this.robot = r; + gamepad = g; + override = g.leftTrigger.getAsButton(0.5); + + AssignNamedControllerButton(); + if (Setup.Connected.DRIVEBASE) { + bindDriveControls(); + } + } + + private void AssignNamedControllerButton() { + resetGyroButton = gamepad.ps_options; + driveLeftStick = gamepad.leftStick; + driveRightStick = gamepad.rightStick; + turboButton = gamepad.leftBumper; + snailButton = gamepad.rightBumper; + } + + public void bindDriveControls() { + CommandScheduler.scheduleJoystick( + new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) + ); + + turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); + turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); + snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java similarity index 100% rename from Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java rename to Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java new file mode 100644 index 00000000..0cd94954 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; + +public class OperatorController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton openClaw; + public CommandButton closeClaw; + public Stick armStick; + public CommandButton ArmHorizontal; + public CommandButton ArmVertical; + public CommandButton intake; + + public OperatorController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + openClaw = gamepad.leftBumper; + closeClaw = gamepad.rightBumper; + armStick = gamepad.rightStick; + ArmHorizontal = gamepad.ps_circle; + ArmVertical = gamepad.ps_triangle; + intake = gamepad.dpadRight; + } + + public void BindControls() { + if (Setup.Connected.CLAWSUBSYSTEM) { + bindClawSubsystemControls(); + } + } + + public void bindClawSubsystemControls() { + openClaw.whenPressed(robot.clawSubsystem::openClaw); + closeClaw.whenPressed(robot.clawSubsystem::closeClaw); + ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); + ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); + intake.whenPressed(robot.clawSubsystem::setArmIntake); + CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java similarity index 100% rename from Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java rename to Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java new file mode 100644 index 00000000..af82fa17 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java @@ -0,0 +1,75 @@ +package org.firstinspires.ftc.ptechnodactyl.helpers; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity; + +@Config +public class HeadingHelper { + + public static double DEFAULT_START_HEADING = 180; + public static double DEFAULT_START_X = 53; + public static double DEFAULT_START_Y = 63; + public static int EXPIRATION_TIME = 20; + + public double headingUpdateTime; + public double lastHeading; + public double lastXPosition; + public double lastYPosition; + + public HeadingHelper(double lastX, double lastY, double heading) { + headingUpdateTime = System.currentTimeMillis() / 1000.0; + lastXPosition = lastX; + lastYPosition = lastY; + lastHeading = heading; + } + + public static void saveHeading(double x, double y, double h) { + FtcRobotControllerActivity.SaveBetweenRuns = new HeadingHelper(x, y, h); + } + + public static void savePose(Pose2d p) { + saveHeading(p.getX(), p.getY(), p.getHeading()); + } + + public static void clearSavedInfo() { + FtcRobotControllerActivity.SaveBetweenRuns = null; + } + + public static boolean validHeading() { + HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; + if (hh == null) { + return false; + } + double now = System.currentTimeMillis() / 1000.0; + return now < hh.headingUpdateTime + EXPIRATION_TIME; + } + + public static Pose2d getSavedPose() { + return new Pose2d(getSavedX(), getSavedY(), getSavedHeading()); + } + + public static double getSavedHeading() { + HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; + if (hh != null) { + return hh.lastHeading; + } + return DEFAULT_START_HEADING; + } + + public static double getSavedX() { + HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; + if (hh != null) { + return hh.lastXPosition; + } + return DEFAULT_START_X; + } + + public static double getSavedY() { + HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; + if (hh != null) { + return hh.lastYPosition; + } + return DEFAULT_START_Y; + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java new file mode 100644 index 00000000..48f4bea7 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.ptechnodactyl.helpers; + +public enum StartingPosition { + Backstage, + Wing, + Unspecified, +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java new file mode 100644 index 00000000..a77ee77b --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.path.command.TrajectorySequenceCommand; +import com.technototes.path.geometry.ConfigurablePoseD; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; + +/* + * Some Emojis for opmode names: + * Copy them and paste them in the 'name' section of the @Autonomous tag + * Red alliance: 🟥 + * Blue alliance: 🟦 + * Wing position: 🪶 + * Backstage pos: 🎦 + */ +//@Config +@Autonomous(name = "Basic Auto", preselectTeleOp = "PlainDrive") +@SuppressWarnings("unused") +public class Basic extends CommandOpMode implements Loggable { + + public static int AUTO_TIME = 1; + public Hardware hardware; + public Robot robot; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> + b + .apply(new ConfigurablePoseD(0, 0, 0).toPose()) + .lineTo(new Vector2d(5, 5)) + .build() + ), + // new TurboCommand(robot.drivebaseSubsystem), + // new StartSpinningCmd(robot.spinner), + new WaitCommand(AUTO_TIME), + new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> + b + .apply(new ConfigurablePoseD(5, 5, 0).toPose()) + .lineTo(new Vector2d(0, 0)) + .build() + ), + // new StopSpinningCmd(robot.spinner), + CommandScheduler::terminateOpMode + ), + CommandOpMode.OpModeState.RUN + ); + } +} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java new file mode 100644 index 00000000..6302c59d --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; +import org.firstinspires.ftc.ptechnodactyl.controllers.OperatorController; +import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; + +@TeleOp(name = "PT Driving w/Turbo!") +@SuppressWarnings("unused") +public class JustDrivingTeleOp extends CommandOpMode implements Loggable { + + public Robot robot; + public DriverController controlsDriver; + public OperatorController controlsOperator; + public Hardware hardware; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + controlsOperator = new OperatorController(codriverGamepad, robot); + if (Setup.Connected.DRIVEBASE) { + controlsDriver = new DriverController(driverGamepad, robot); + CommandScheduler.scheduleForState( + EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), + OpModeState.INIT + ); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java similarity index 100% rename from Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java rename to Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java new file mode 100644 index 00000000..a6501b33 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; + +@TeleOp(name = "PlainDrive") +@SuppressWarnings("unused") +public class PlainDrive extends CommandOpMode implements Loggable { + + public Hardware hardware; + public Robot robot; + + public DriverController driver; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + driver = new DriverController(driverGamepad, robot); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java similarity index 100% rename from Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java rename to Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md new file mode 100644 index 00000000..c2a5bed5 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md @@ -0,0 +1,140 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your +team's robot controller App. This module is currently empty (a clean slate) but +the process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it +your own. + +Sample opmodes exist in the FtcRobotController module. To locate these samples, +find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: +FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to +interpret the naming system, it will help to understand the conventions that +were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in +this folder. + +To summarize: A range of different samples classes will reside in the +java/external/samples. The class names will follow a naming convention which +indicates the purpose of each class. The prefix of the name will be one of the +following: + +Basic: This is a minimally functional OpMode used to illustrate the +skeleton/structure of a particular style of OpMode. These are bare bones +examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. It is +not intended to drive a functioning robot, it is simply showing the minimal code +required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) +drive base. It may be used to provide a common baseline driving OpMode, or to +demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function +or concept. These may be complex, but their operation should be explained +clearly in the comments, or the comments should reference an external doc, guide +or tutorial. Each OpMode should try to only demonstrate a single concept so they +are easy to locate based on their name. These OpModes may not produce a drivable +robot. + +After the prefix, other conventions will apply: + +- Sensor class names are constructed as: Sensor - Company - Type +- Robot class names are constructed as: Robot - Mode - Action - OpModetype +- Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to +be the basis for your own robot. In all cases, the desired sample(s) needs to be +copied into your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + +1. Locate the desired sample class in the Project/Android tree. + +2. Right click on the sample class and select "Copy" + +3. Expand the TeamCode/java folder + +4. Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + +5. You will be prompted for a class name for the copy. Choose something + meaningful based on the purpose of this class. Start with a capital letter, + and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on +the Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown +below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by +the code: `name="Template: Linear OpMode"` You can change what appears between +the quotes to better describe your opmode. The "group=" portion of the code can +be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list +because of the `@Disabled` annotation which has been included. This line can +simply be deleted , or commented out, to make the OpMode visible. + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to +all share a common code organization, with each being able to _see_ the others +code but each having their own team module with their own code that they +maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of +these teams. Each of the clones would then appear along side each other in the +Android Studio module list, together with the FtcRobotController module (and the +original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module +from the pulldown list prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. You will need to +be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios +before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is +intentional. + +1. Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2. In the new Team0417 folder, delete the TeamCode.iml file. + +3. the new Team0417 folder, rename the + "src/main/java/org/firstinspires/ftc/teamcode" folder to a matching name + with a lowercase 'team' eg: "team0417". + +4. In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, + change the line that contains package="org.firstinspires.ftc.teamcode" to be + package="org.firstinspires.ftc.team0417" + +5. Add: include ':Team0417' to the "/settings.gradle" file. +6. Open up Android Studios and clean out any old files by using the menu to + "Build/Clean Project"" diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java similarity index 100% rename from Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java rename to Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java new file mode 100644 index 00000000..5ec27188 --- /dev/null +++ b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java @@ -0,0 +1,234 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.hardware.sensor.IGyro; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.Loggable; +import com.technototes.path.subsystem.MecanumConstants; +import com.technototes.path.subsystem.PathingMecanumDrivebaseSubsystem; +import java.util.function.Supplier; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.helpers.HeadingHelper; + +public class DrivebaseSubsystem + extends PathingMecanumDrivebaseSubsystem + implements Supplier, Loggable { + + @Override + public Pose2d get() { + return getPoseEstimate(); + } + + @Config + public abstract static class DriveConstants implements MecanumConstants { + + public static double SLOW_MOTOR_SPEED = 0.6; + public static double FAST_MOTOR_SPEED = 1.0; + public static double AUTO_MOTOR_SPEED = 0.9; + + @TicksPerRev + public static final double TICKS_PER_REV = 537.7; // From GoBilda's website + + @MaxRPM + public static final double MAX_RPM = 312; + + public static double MAX_TICKS_PER_SEC = (TICKS_PER_REV * MAX_RPM) / 60.0; + + @UseDriveEncoder + public static final boolean RUN_USING_ENCODER = true; + + @MotorVeloPID + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( + 20, + 0, + 3, + MecanumConstants.getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) + ); + + @WheelRadius + public static double WHEEL_RADIUS = 1.88976; // inches "roughly" lol + + @GearRatio + public static double GEAR_RATIO = 1.0; // output (wheel) speed / input (motor) speed + + @TrackWidth + public static double TRACK_WIDTH = 9.25; // inches + + @WheelBase + public static double WHEEL_BASE = 9.25; // inches + + @KV + public static double kV = + 1.0 / MecanumConstants.rpmToVelocity(MAX_RPM, WHEEL_RADIUS, GEAR_RATIO); + + @KA + public static double kA = 0; + + @KStatic + public static double kStatic = 0; + + // This was 60, which was too fast. Things slid around a lot. + @MaxVelo + public static double MAX_VEL = 50; + + // This was 35, which also felt a bit too fast. The bot controls more smoothly now + @MaxAccel + public static double MAX_ACCEL = 30; + + // This was 180 degrees + @MaxAngleVelo + public static double MAX_ANG_VEL = Math.toRadians(180); + + // This was 90 degrees + @MaxAngleAccel + public static double MAX_ANG_ACCEL = Math.toRadians(90); + + @TransPID + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); + + @HeadPID + public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); + + @LateralMult + public static double LATERAL_MULTIPLIER = 1.0; // For Mecanum, this was by 1.14 (14% off) + + @VXWeight + public static double VX_WEIGHT = 1; + + @VYWeight + public static double VY_WEIGHT = 1; + + @OmegaWeight + public static double OMEGA_WEIGHT = 1; + + @PoseLimit + public static int POSE_HISTORY_LIMIT = 100; + + // Helps deal with tired motors + public static double AFR_SCALE = 0.9; + public static double AFL_SCALE = 0.9; + public static double ARR_SCALE = 0.9; + public static double ARL_SCALE = 0.9; + } + + private static final boolean ENABLE_POSE_DIAGNOSTICS = true; + + @Log(name = "Pose2d: ") + public String poseDisplay = ENABLE_POSE_DIAGNOSTICS ? "" : null; + + // @Log.Number(name = "FL") + public EncodedMotor fl2; + + // @Log.Number(name = "FR") + public EncodedMotor fr2; + + // @Log.Number(name = "RL") + public EncodedMotor rl2; + + // @Log.Number(name = "RR") + public EncodedMotor rr2; + + // @Log(name = "Turbo") + public boolean Turbo = false; + + // @Log(name = "Snail") + public boolean Snail = false; + + @Log.Number(name = "cur heading") + public double curHeading; + + public DrivebaseSubsystem( + EncodedMotor flMotor, + EncodedMotor frMotor, + EncodedMotor rlMotor, + EncodedMotor rrMotor, + IGyro imu + ) { + super(flMotor, frMotor, rlMotor, rrMotor, imu, () -> DriveConstants.class); + fl2 = flMotor; + fr2 = frMotor; + rl2 = rlMotor; + rr2 = rrMotor; + curHeading = imu.getHeading(); + Setup.HardwareNames.COLOR = imu.getClass().toString(); + } + + @Override + public void periodic() { + if (ENABLE_POSE_DIAGNOSTICS) { + updatePoseEstimate(); + Pose2d pose = getPoseEstimate(); + Pose2d poseVelocity = getPoseVelocity(); + poseDisplay = pose.toString() + + " : " + + (poseVelocity != null ? poseVelocity.toString() : "nullv"); + curHeading = this.gyro.getHeading(); + Setup.HardwareNames.FLYWHEELMOTOR = this.gyro.getClass().toString(); + } + } + + public void fast() { + speed = DriveConstants.FAST_MOTOR_SPEED; + } + + public void slow() { + speed = DriveConstants.SLOW_MOTOR_SPEED; + } + + public void auto() { + speed = DriveConstants.AUTO_MOTOR_SPEED; + } + + public void setSnailMode() { + Snail = true; + Turbo = false; + } + + public void saveHeading() { + HeadingHelper.saveHeading(get().getX(), get().getY(), gyro.getHeading()); + } + + public void setTurboMode() { + Turbo = true; + Snail = false; + } + + public void setNormalMode() { + Snail = false; + Turbo = false; + } + + @Override + public void setMotorPowers(double lfv, double lrv, double rrv, double rfv) { + // TODO: Use the stick position to determine how to scale these values + // in Turbo mode (If the robot is driving in a straight line, the values are + // going to max out at sqrt(2)/2, rather than: We can go faster, but we don't + // *always* want to scale faster, only when we're it turbo mode, and when one (or more) + // of the control sticks are at their limit + double maxlv = Math.max(Math.abs(lfv), Math.abs(lrv)); + double maxrv = Math.max(Math.abs(rfv), Math.abs(rrv)); + double maxall = Math.max(maxlv, maxrv); + if (Snail == true) { + maxall = 1.0 / DriveConstants.SLOW_MOTOR_SPEED; + } else if (Turbo == false) { + maxall = 1.0 / DriveConstants.AUTO_MOTOR_SPEED; + } + leftFront.setVelocity( + (lfv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.AFL_SCALE) / maxall + ); + leftRear.setVelocity( + (lrv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.ARL_SCALE) / maxall + ); + rightRear.setVelocity( + (rrv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.ARR_SCALE) / maxall + ); + rightFront.setVelocity( + (rfv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.AFR_SCALE) / maxall + ); + } +} diff --git a/Osprey/src/main/res/raw/readme.md b/Osprey/src/main/res/raw/readme.md new file mode 100644 index 00000000..cf732dc9 --- /dev/null +++ b/Osprey/src/main/res/raw/readme.md @@ -0,0 +1 @@ +Place your sound files in this folder to use them as project resources. diff --git a/Osprey/src/main/res/values/strings.xml b/Osprey/src/main/res/values/strings.xml new file mode 100644 index 00000000..d781ec5f --- /dev/null +++ b/Osprey/src/main/res/values/strings.xml @@ -0,0 +1,4 @@ + + + + diff --git a/Osprey/src/main/res/xml/teamwebcamcalibrations.xml b/Osprey/src/main/res/xml/teamwebcamcalibrations.xml new file mode 100644 index 00000000..22ae7a86 --- /dev/null +++ b/Osprey/src/main/res/xml/teamwebcamcalibrations.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java index 0a34a722..20189789 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java @@ -2,9 +2,8 @@ import com.technototes.library.logger.Loggable; import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Hardware; + import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; public class Robot implements Loggable { @@ -13,7 +12,7 @@ public class Robot implements Loggable { public Alliance alliance; public double initialVoltage; public DrivebaseSubsystem drivebaseSubsystem; - public ClawSubsystem clawSubsystem; + public Robot(Hardware hw) { this.initialVoltage = hw.voltage(); @@ -26,8 +25,6 @@ public Robot(Hardware hw) { hw.imu ); } - if (Setup.Connected.CLAWSUBSYSTEM) { - this.clawSubsystem = new ClawSubsystem(hw); - } + } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java index 0cd94954..22ab7690 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java @@ -1,12 +1,9 @@ package org.firstinspires.ftc.ptechnodactyl.controllers; -import com.technototes.library.command.CommandScheduler; import com.technototes.library.control.CommandButton; import com.technototes.library.control.CommandGamepad; import com.technototes.library.control.Stick; import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; public class OperatorController { @@ -36,17 +33,8 @@ private void AssignNamedControllerButton() { } public void BindControls() { - if (Setup.Connected.CLAWSUBSYSTEM) { - bindClawSubsystemControls(); - } - } - public void bindClawSubsystemControls() { - openClaw.whenPressed(robot.clawSubsystem::openClaw); - closeClaw.whenPressed(robot.clawSubsystem::closeClaw); - ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); - ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); - intake.whenPressed(robot.clawSubsystem::setArmIntake); - CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); } + + } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java index 6302c59d..e1562808 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java @@ -7,18 +7,17 @@ import com.technototes.library.logger.Loggable; import com.technototes.library.structure.CommandOpMode; import com.technototes.library.util.Alliance; + import org.firstinspires.ftc.ptechnodactyl.Hardware; import org.firstinspires.ftc.ptechnodactyl.Robot; import org.firstinspires.ftc.ptechnodactyl.Setup; import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; import org.firstinspires.ftc.ptechnodactyl.controllers.OperatorController; -import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; @TeleOp(name = "PT Driving w/Turbo!") @SuppressWarnings("unused") -public class JustDrivingTeleOp extends CommandOpMode implements Loggable { +public abstract class JustDrivingTeleOp extends CommandOpMode implements Loggable { public Robot robot; public DriverController controlsDriver; diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java new file mode 100644 index 00000000..9f0eb4a6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java @@ -0,0 +1,149 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.Range; +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoProfiler; +import com.technototes.library.subsystem.Subsystem; + +import java.util.function.Supplier; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.CARRY; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.COLLECT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.ARM_CONSTRAINTS; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DOWN; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DUMP; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DIFFERENTIAL; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.FAKE_CARRY; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.IN; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.OUT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.SLIGHT_CARRY; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.UP; + +@SuppressWarnings("unused") + +public class ArmSubsystem implements Subsystem, Supplier { + /** + * Deposit is am arm to hold and drop freight + * + * @Config allows you to mess with variables without messing with the code + * Especially good when trying to set the position of different mechanisms + */ + @Config + public static class ArmConstants { + //public static double MIN = 0, MAX = 0.5; + public static double DUMP = 0.55, CARRY = 0.25, FAKE_CARRY = 0.15, COLLECT = 0.03, AUTO_CARRY = 0.3, SLIGHT_CARRY = 0.15; + public static double IN = 0.02, UP = 0.3, OUT = 0.6, DOWN = 0.75; + public static double DIFFERENTIAL = 2.8; + public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints(5, 5, 5); + } + + public Servo dumpServo; + public Servo armServo; + public ServoProfiler armController; + + public ArmSubsystem(Servo l, Servo r) { + dumpServo = l; + armServo = r; + armController = new ServoProfiler(armServo).setConstraints(ARM_CONSTRAINTS).setTargetPosition(UP); + + } + + /** + * Sets the servo controlling the cup to a constant value which dumps the freight + * to a target + */ + public void dump() { + setDump(DUMP); + } + + /** + * Sets a servo controlling the cup to a constant value which carries the freight while + * the robot is moving + */ + public void carry() { + setDump(CARRY); + } + + public void fakeCarry() { + setDump(FAKE_CARRY); + } + + /** + * Sets a servo to a constant value which puts the cup in position to take the freight once + * it's intaked + */ + public void collect() { + setDump(COLLECT); + } + + /** + * Sets a servo to a custom constant value based off the specific position the driver wants the cup + * to be when dumping the freight + */ + public void setDump(double v) { + targetDumpPosition = Range.clip(v, COLLECT, DUMP); + } + + /** + * Sets the arm servo to a constant value to retract the arm of the robot + */ + public void up() { + setArm(UP); + } + + /** + * Sets the arm servo to a constant value to retract the arm of the robot + */ + public void in() { + setArm(IN); + } + + /** + * Sets the arm servo to a constant value to extend the arm of the robot + */ + public void out() { + setArm(OUT); + } + + public void down() { + setArm(DOWN); + } + + /** + * Sets the arm servo to a custom constant value to extend the arm of the robot + */ + public void setArm(double v) { + armController.setTargetPosition(Range.clip(v, IN, DOWN)); + } + + /** + * Sets a custom value that will add to the Arm servo's value, setting it to a new position + * for extension + */ + public void translateArm(double v) { + setArm(armServo.getPosition() + v); + } + + /** + * Method to display the extension and dump value on the telemetry to the driver knows + * how much they are extending the arm and positioning the cup + */ + @Override + public String get() { + return "ARM: " + armServo.getPosition() + ", DUMP: " + targetDumpPosition; + //return "EXTENSION: "+differential.getAverage()+", DUMP: "+differential.getDeviation(); + } + + public void slightCarry() { + setDump(SLIGHT_CARRY); + } + + private double targetDumpPosition = 0.2; + + @Override + public void periodic() { + armController.update(); + dumpServo.setPosition(targetDumpPosition + armServo.getPosition() / DIFFERENTIAL); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java new file mode 100644 index 00000000..50067076 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BarcodePipeline.java @@ -0,0 +1,192 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvPipeline; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.Supplier; +import com.acmerobotics.dashboard.config.Config; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.BarcodePipeline.BarcodeConstants.*; +public class BarcodePipeline extends OpenCvPipeline implements Supplier { + @Config + public static class BarcodeConstants { + public static boolean DISPLAY = true; + public static Scalar DISPLAY_COLOR = new Scalar(200, 0, 0); + public static Scalar LOWER_LIMIT = new Scalar(80.0, 0.0, 0.0, 0.0); + public static Scalar UPPER_LIMIT = new Scalar(255.0, 80.0, 80.0, 255.0); + public static int BORDER_LEFT_X = 0; //amount of pixels from the left side of the cam to skip + public static int BORDER_RIGHT_X = 0; //amount of pixels from the right of the cam to skip + public static int BORDER_TOP_Y = 90; //amount of pixels from the top of the cam to skip + public static int BORDER_BOTTOM_Y = 50; //amount of pixels from the bottom of the cam to skip + + //y is fot the outpiut + public static Point LEFT = new Point(50, 140); + public static Point CENTER = new Point(160, 140); + public static Point RIGHT = new Point(270, 140); + + public static int VARIANCE = 50; + public static double MIN_AREA = 1000; + + + } + + public Exception debug; + + + private int loopcounter = 0; + private int ploopcounter = 0; + + private final Mat mat = new Mat(); + private final Mat processed = new Mat(); + + private Rect maxRect = new Rect(); + + private double maxArea = 0; + private boolean first = false; + + public Telemetry telemetry; + + public BarcodePipeline(Telemetry t){ + telemetry = t; + } + public BarcodePipeline(){ + } + + @Override + public Mat processFrame(Mat input) + { + try + { + // Process Image + Imgproc.cvtColor(input, mat, Imgproc.COLOR_RGB2RGBA); + Core.inRange(mat, LOWER_LIMIT, UPPER_LIMIT, processed); + // Core.bitwise_and(input, input, output, processed); + + // Remove Noise + Imgproc.morphologyEx(processed, processed, Imgproc.MORPH_OPEN, new Mat()); + Imgproc.morphologyEx(processed, processed, Imgproc.MORPH_CLOSE, new Mat()); + // GaussianBlur + Imgproc.GaussianBlur(processed, processed, new Size(5.0, 15.0), 0.00); + // Find Contours + List contours = new ArrayList<>(); + Imgproc.findContours(processed, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); + + // Draw Contours + if(DISPLAY) Imgproc.drawContours(input, contours, -1, DISPLAY_COLOR); + + // Loop Through Contours + for (MatOfPoint contour : contours) + { + Point[] contourArray = contour.toArray(); + + // Bound Rectangle if Contour is Large Enough + if (contourArray.length >= 15) + { + MatOfPoint2f areaPoints = new MatOfPoint2f(contourArray); + Rect rect = Imgproc.boundingRect(areaPoints); + + // if rectangle is larger than previous cycle or if rectangle is not larger than previous 6 cycles > then replace + if (rect.area() > maxArea + && rect.x > BORDER_LEFT_X && rect.x + rect.width < input.width() - BORDER_RIGHT_X + && rect.y > BORDER_TOP_Y && rect.y + rect.height < input.height() - BORDER_BOTTOM_Y + || loopcounter - ploopcounter > 6) + { + maxArea = rect.area(); + maxRect = rect; + ploopcounter++; + loopcounter = ploopcounter; + first = true; + } + areaPoints.release(); + } + contour.release(); + } + mat.release(); + processed.release(); + if (contours.isEmpty()) + { + maxRect = new Rect(); + } + if (first && maxRect.area() > MIN_AREA) + { + if(DISPLAY) Imgproc.rectangle(input, maxRect, DISPLAY_COLOR, 2); + } + // Draw Borders + if(DISPLAY) { + Imgproc.rectangle(input, new Rect(BORDER_LEFT_X, BORDER_TOP_Y, input.width() - BORDER_RIGHT_X - BORDER_LEFT_X, input.height() - BORDER_BOTTOM_Y - BORDER_TOP_Y), DISPLAY_COLOR, 2); + Imgproc.circle(input, LEFT, VARIANCE, DISPLAY_COLOR); + Imgproc.circle(input, CENTER, VARIANCE, DISPLAY_COLOR); + Imgproc.circle(input, RIGHT, VARIANCE, DISPLAY_COLOR); + + // Display Data + + Imgproc.putText(input, "Area: " + getRectArea() + " Midpoint: " + getRectMidpointXY().x + " , " + getRectMidpointXY().y+ " Selection: "+get(), new Point(20, input.height() - 20), Imgproc.FONT_HERSHEY_PLAIN, 0.6, DISPLAY_COLOR, 1); + } + loopcounter++; + } catch (Exception e) { + debug = e; + boolean error = true; + } + if(telemetry != null){ + telemetry.addLine(get().toString()); + telemetry.update(); + } + + try { + Thread.sleep(100); + } catch (InterruptedException ignored) { + } + + return input; + } + public int getRectHeight(){return maxRect.height;} + public int getRectWidth(){ return maxRect.width; } + public int getRectX(){ return maxRect.x; } + public int getRectY(){ return maxRect.y; } + public double getRectMidpointX(){ return getRectX() + (getRectWidth()/2.0); } + public double getRectMidpointY(){ return getRectY() + (getRectHeight()/2.0); } + public Point getRectMidpointXY(){ return new Point(getRectMidpointX(), getRectMidpointY());} + public double getRectArea(){ return maxRect.area(); } + + private int last = -1; + @Override + public synchronized Integer get() { + if(getRectArea()>MIN_AREA) { + Point p = getRectMidpointXY(); + if (Math.abs(p.x - LEFT.x) < VARIANCE && Math.abs(p.y - LEFT.y) < VARIANCE) last = 0; + if (Math.abs(p.x - CENTER.x) < VARIANCE && Math.abs(p.y - CENTER.y) < VARIANCE) last = 1; + if (Math.abs(p.x - RIGHT.x) < VARIANCE && Math.abs(p.y - RIGHT.y) < VARIANCE) last = 2; + } + return last; + } + + public boolean none(){ + return get() == -1; + } + public boolean zero(){ + return get() == 0; + } + public boolean one(){ + return get() == 1; + } + public boolean two(){ + return get() == 2; + } + + public boolean twoOrDefault(){ + return none() || two(); + } + + +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java new file mode 100644 index 00000000..01dc57c5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.subsystem.Subsystem; + +import java.util.function.Supplier; + +public class BrakeSubsystem implements Subsystem, Supplier { + @com.acmerobotics.dashboard.config.Config + public static class BrakeConstants { + public static double UP = 0.02, DOWN = 0.34; + } + + private boolean isBraking; + public Servo servo; + + public BrakeSubsystem(Servo s) { + servo = s; + isBraking = false; + } + + public void raise() { + servo.setPosition(BrakeConstants.UP); + isBraking = false; + } + + public void lower() { + servo.setPosition(BrakeConstants.DOWN); + isBraking = true; + } + + @Override + public Boolean get() { + return isBraking; + } +} + + diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java new file mode 100644 index 00000000..b1d6ae8b --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java @@ -0,0 +1,85 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem.CapConstants.*; + +import com.acmerobotics.dashboard.config.Config; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoProfiler; +import com.technototes.library.subsystem.Subsystem; + + +import java.util.function.Supplier; + + +public class CapSubsystem implements Subsystem, Supplier { + @Config + public static class CapConstants { + public static double TURRET_INIT = 0.8, TURRET_PICKUP = 0.1, TURRET_CARRY = 0.8, TURRET_CAP = 0.5; + public static double CLAW_OPEN = 0.1, CLAW_CLOSE = 0.53; + public static double ARM_UP = 1, ARM_CAP = 0.85, ARM_INIT = 0.4, ARM_DOWN = 0.05; + public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints(5, 5, 5); + public static ServoProfiler.Constraints TURRET_CONSTRAINTS = new ServoProfiler.Constraints(3, 2, 2); + + } + public Servo armServo; + public Servo clawServo; + + public Servo turretServo; + + public ServoProfiler armProfiler; + public ServoProfiler turretProfiler; + + + public CapSubsystem(Servo arm, Servo claw, Servo turret){ + CommandScheduler.register(this); + armServo = arm; + clawServo = claw; + turretServo = turret; + armProfiler = new ServoProfiler(armServo).setServoRange(0.4).setConstraints(ARM_CONSTRAINTS).setTargetPosition(ARM_INIT); + turretProfiler = new ServoProfiler(turretServo).setConstraints(TURRET_CONSTRAINTS).setTargetPosition(TURRET_INIT); + } + + public void open(){ + clawServo.setPosition(CLAW_OPEN); + } + + public void close(){ + clawServo.setPosition(CLAW_CLOSE); + } + + public void up(){ + armProfiler.setTargetPosition(ARM_UP); + turretProfiler.setTargetPosition(TURRET_CARRY); + clawServo.setPosition(CLAW_CLOSE); + } + public void raise(){ + armProfiler.setTargetPosition(ARM_CAP); + } + public void raise2(){ + turretProfiler.setTargetPosition(TURRET_CAP); + } + public void down(){ + armProfiler.setTargetPosition(ARM_DOWN); + turretProfiler.setTargetPosition(TURRET_PICKUP); + } + public void translateArm(double translation){ + armProfiler.translateTargetPosition(translation); + } + public void translateTurret(double translation){ + turretProfiler.translateTargetPosition(translation); + } + + @Override + public void periodic() { + turretProfiler.update(); + armProfiler.update(); + } + + @Override + public String get() { + return "CLAW: "+clawServo.getPosition()+", ARM: "+armProfiler.getTargetPosition()+", TURRET: "+turretProfiler.getTargetPosition(); + } + +} + diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java new file mode 100644 index 00000000..1f3c907f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CarouselSubsystem.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.technototes.library.hardware.motor.Motor; +import com.technototes.library.subsystem.Subsystem; + +import java.util.function.Supplier; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.AUTO_SPEED; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.CAROUSEL_STOP_SPEED; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.MAX_SPEED; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.MIN_SPEED; + + +@SuppressWarnings("unused") + +public class CarouselSubsystem implements Subsystem, Supplier { + + /** + * Carousel Constants for measuring speed of carousel motor + */ + + @Config + public static class CarouselConstants{ + public static double MAX_SPEED = 1; + public static double MIN_SPEED = 0.2; + public static double AUTO_SPEED = 0.2; + + public static double CAROUSEL_STOP_SPEED = 0; + public static double SPIN_OFFSET = 3.5; + } + + public Motor motor; + + public CarouselSubsystem(Motor m){ + motor = m; + } + + public void right(){motor.setSpeed(AUTO_SPEED);} + + public void right(double speed){motor.setSpeed(MAX_SPEED*Math.max(MIN_SPEED, speed));} + + /** + * when called by Carousel Right Command, sets motor to turn carousel right + */ + + public void left(){motor.setSpeed(-AUTO_SPEED);} + public void left(double speed){motor.setSpeed(-MAX_SPEED*Math.max(MIN_SPEED, speed));} + + /** + * when called by Carousel Left Command, sets motor to turn carousel left + */ + + public void stop(){motor.setSpeed(CAROUSEL_STOP_SPEED);} + + /** + * when called by Carousel Stop Command, stops carousel motor + */ + + @Override + public Double get() { + return motor.getSpeed(); + /** + * gets current motor speed + */ + } + +} From 4ba7ad1da82c35fa768fbe4cfa8a87e914cb7424 Mon Sep 17 00:00:00 2001 From: KOOLPOOL <141171610+Kooolpool@users.noreply.github.com> Date: Tue, 17 Jun 2025 16:10:26 -0700 Subject: [PATCH 2/4] added some more subsystems --- .../ftc/hoops/ClawAndWristBot.java | 8 +- .../org/firstinspires/ftc/hoops/Hardware.java | 8 +- .../subsystems/ClawAndWristSubsystem.java | 10 +- .../hoops/subsystems/PlacementSubsystem.java | 6 +- .../ftc/hoops/subsystems/TestSubsystem.java | 4 +- Osprey/build.gradle | 39 -- Osprey/lib/OpModeAnnotationProcessor.jar | Bin 6032 -> 0 bytes Osprey/src/main/AndroidManifest.xml | 11 - .../ftc/ptechnodactyl/Hardware.java | 91 ---- .../ftc/ptechnodactyl/Robot.java | 33 -- .../ftc/ptechnodactyl/Setup.java | 52 -- .../commands/DrivingCommands.java | 39 -- .../ftc/ptechnodactyl/commands/EZCmd.java | 27 - .../commands/JoystickDriveCommand.java | 139 ----- .../commands/JoystickIncDecCmd.java | 42 -- .../controllers/DriverController.java | 52 -- .../controllers/OneController.java | 77 --- .../controllers/OperatorController.java | 52 -- .../controllers/TestController.java | 42 -- .../ptechnodactyl/helpers/HeadingHelper.java | 75 --- .../helpers/StartingPosition.java | 7 - .../ftc/ptechnodactyl/opmodes/auto/Basic.java | 62 --- .../opmodes/tele/JustDrivingTeleOp.java | 42 -- .../ptechnodactyl/opmodes/tele/MotorTest.java | 29 - .../opmodes/tele/PlainDrive.java | 28 - .../opmodes/tele/oneController.java | 31 -- .../firstinspires/ftc/ptechnodactyl/readme.md | 140 ----- .../subsystems/ClawSubsystem.java | 201 ------- .../subsystems/DrivebaseSubsystem.java | 234 --------- Osprey/src/main/res/raw/readme.md | 1 - Osprey/src/main/res/values/strings.xml | 4 - .../main/res/xml/teamwebcamcalibrations.xml | 161 ------ .../ftc/ptechnodactyl/Controls.java | 291 ++++++++++ .../ftc/ptechnodactyl/Hardware.java | 178 ++++--- .../ftc/ptechnodactyl/Robot.java | 107 +++- .../ftc/ptechnodactyl/RobotConstants.java | 271 ++++++++++ .../ftc/ptechnodactyl/RobotState.java | 78 +++ .../commands/arm/ArmAllianceCommand.java | 17 + .../commands/arm/ArmBarcodeSelectCommand.java | 21 + .../commands/arm/ArmCommand.java | 19 + .../commands/arm/ArmInCommand.java | 22 + .../commands/arm/ArmRaiseCommand.java | 16 + .../commands/arm/ArmRaiseInCommand.java | 21 + .../commands/arm/ArmSharedCommand.java | 26 + .../commands/arm/ArmTranslateCommand.java | 29 + .../commands/arm/BucketCarryCommand.java | 19 + .../commands/arm/BucketCollectCommand.java | 24 + .../commands/arm/BucketDumpCommand.java | 24 + .../arm/BucketDumpVariableCommand.java | 38 ++ .../autonomous/AutoCarouselCommand.java | 30 ++ .../commands/autonomous/AutoCycleCommand.java | 50 ++ .../autonomous/AutoCyclePreloadCommand.java | 32 ++ .../AutoDepositAllianceCommand.java | 44 ++ .../autonomous/AutoDepositDuckCommand.java | 35 ++ .../commands/autonomous/AutoDuckCommand.java | 40 ++ .../autonomous/AutoDuckPreloadCommand.java | 30 ++ .../autonomous/AutoIntakeDuckCommand.java | 22 + .../AutoIntakeWarehouseCommand.java | 32 ++ .../autonomous/AutoParkBarrierCommand.java | 28 + .../autonomous/AutoParkSquareCommand.java | 26 + .../autonomous/AutoParkWarehouseCommand.java | 26 + .../TeleopDepositAllianceCommand.java | 57 ++ .../TeleopDepositSharedCommand.java | 58 ++ .../TeleopIntakeAllianceWarehouseCommand.java | 36 ++ .../TeleopIntakeSharedWarehouseCommand.java | 36 ++ .../commands/cap/CapArmTranslateCommand.java | 29 + .../commands/cap/CapCloseCommand.java | 19 + .../commands/cap/CapDownCommand.java | 25 + .../commands/cap/CapOpenCommand.java | 19 + .../commands/cap/CapOutCommand.java | 25 + .../commands/cap/CapStoreCommand.java | 25 + .../cap/CapTurretTranslateCommand.java | 29 + .../carousel/CarouselLeftCommand.java | 31 ++ .../carousel/CarouselRightCommand.java | 31 ++ .../carousel/CarouselSpinCommand.java | 32 ++ .../carousel/CarouselStopCommand.java | 19 + .../deposit/DepositAllianceCommand.java | 26 + .../deposit/DepositCollectCommand.java | 32 ++ .../deposit/DepositCycleAllianceCommand.java | 26 + .../deposit/DepositOppositeSharedCommand.java | 33 ++ .../deposit/DepositPreloadCommand.java | 26 + .../deposit/DepositSharedCommand.java | 24 + .../ExtensionBarcodeSelectCommand.java | 24 + .../extension/ExtensionCollectCommand.java | 17 + .../ExtensionCollectSafeCommand.java | 23 + .../commands/extension/ExtensionCommand.java | 23 + .../extension/ExtensionLeftOutCommand.java | 10 + .../extension/ExtensionLeftSideCommand.java | 25 + .../extension/ExtensionOutCommand.java | 31 ++ .../extension/ExtensionRightOutCommand.java | 10 + .../extension/ExtensionRightSideCommand.java | 25 + .../extension/ExtensionSideCommand.java | 34 ++ .../extension/ExtensionTranslateCommand.java | 23 + .../extension/TurretTranslateCommand.java | 33 ++ .../commands/intake/IntakeInCommand.java | 19 + .../commands/intake/IntakeOutCommand.java | 30 ++ .../commands/intake/IntakeSafeCommand.java | 21 + .../commands/intake/IntakeStopCommand.java | 19 + .../lift/LiftBarcodeSelectCommand.java | 17 + .../commands/lift/LiftCollectCommand.java | 10 + .../commands/lift/LiftCommand.java | 34 ++ .../commands/lift/LiftLevel1Command.java | 10 + .../commands/lift/LiftLevel2Command.java | 10 + .../commands/lift/LiftLevel2TeleCommand.java | 10 + .../commands/lift/LiftLevel3Command.java | 10 + .../commands/lift/LiftLevelCommand.java | 19 + .../commands/lift/LiftSharedCommand.java | 10 + .../commands/lift/LiftTranslateCommand.java | 23 + .../commands/vision/VisionBarcodeCommand.java | 32 ++ .../controllers/DriverController.java | 52 -- .../controllers/OperatorController.java | 40 -- .../opmodes/auto/AutoCycleBase.java | 56 ++ .../opmodes/auto/AutoDuckBase.java | 56 ++ .../ftc/ptechnodactyl/opmodes/auto/Basic.java | 62 --- .../opmodes/auto/LoopTimeTest.java | 52 ++ .../opmodes/auto/rr/BackAndForth.java | 59 +++ .../auto/rr/DriveVelocityPIDTuner.java | 174 ++++++ .../opmodes/auto/rr/FollowerPIDTuner.java | 55 ++ .../opmodes/auto/rr/LocalizationTest.java | 55 ++ .../auto/rr/ManualFeedforwardTuner.java | 156 ++++++ .../opmodes/auto/rr/MaxAngularVeloTuner.java | 74 +++ .../opmodes/auto/rr/MaxVelocityTuner.java | 101 ++++ .../auto/rr/MotorDirectionDebugger.java | 105 ++++ .../opmodes/auto/rr/OpenCVTest.java | 373 +++++++++++++ .../opmodes/auto/rr/SampleMecanumDrive.java | 496 ++++++++++++++++++ .../opmodes/auto/rr/SplineTest.java | 46 ++ .../opmodes/auto/rr/StrafeTest.java | 50 ++ .../opmodes/auto/rr/StraightTest.java | 50 ++ .../opmodes/auto/rr/TurnTest.java | 34 ++ .../opmodes/tele/JustDrivingTeleOp.java | 41 -- .../opmodes/tele/PlainDrive.java | 28 - .../opmodes/tele/TeleOpBase.java | 46 ++ .../subsystems/ArmSubsystem.java | 42 +- .../subsystems/BrakeSubsystem.java | 11 +- .../subsystems/CapSubsystem.java | 74 ++- .../subsystems/DrivebaseSubsystem.java | 135 ++++- .../subsystems/ExtensionSubsystem.java | 82 +++ .../subsystems/IntakeSubsystem.java | 94 ++++ .../subsystems/LiftSubsystem.java | 104 ++++ .../subsystems/VisionSubsystem.java | 56 ++ .../ftc/sixteen750/Hardware.java | 22 +- .../subsystems/HorizontalSlidesSubsystem.java | 8 +- .../subsystems/VerticalSlidesSubsystem.java | 6 +- .../firstinspires/ftc/twenty403/Hardware.java | 10 +- .../twenty403/subsystems/HangSubsystem.java | 3 - .../subsystems/KidShampooSubsystem.java | 4 +- bun.lockb | Bin 20721 -> 20721 bytes 147 files changed, 5322 insertions(+), 2123 deletions(-) delete mode 100644 Osprey/build.gradle delete mode 100644 Osprey/lib/OpModeAnnotationProcessor.jar delete mode 100644 Osprey/src/main/AndroidManifest.xml delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java delete mode 100644 Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java delete mode 100644 Osprey/src/main/res/raw/readme.md delete mode 100644 Osprey/src/main/res/values/strings.xml delete mode 100644 Osprey/src/main/res/xml/teamwebcamcalibrations.xml create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java delete mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java delete mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java delete mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java delete mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java delete mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java index 6434cd9e..1818e6e3 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/ClawAndWristBot.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.hoops; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import org.firstinspires.ftc.hoops.subsystems.ClawAndWristSubsystem; //@Config @@ -13,6 +13,10 @@ public class ClawAndWristBot { public ClawAndWristSubsystem caw; public ClawAndWristBot() { - caw = new ClawAndWristSubsystem(new Servo(CLAW1), new Servo(CLAW2), new Servo(WRIST)); + caw = new ClawAndWristSubsystem( + new ServoGroup(CLAW1), + new ServoGroup(CLAW2), + new ServoGroup(WRIST) + ); } } diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java index 82b45b1d..6483ba2e 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/Hardware.java @@ -11,7 +11,7 @@ import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Loggable; import com.technototes.vision.hardware.Webcam; import java.util.List; @@ -25,8 +25,8 @@ public class Hardware implements Loggable { public Motor placeholder1; public DcMotorEx liftMotor; - public Servo placeholder2; - public Servo servo; + public ServoGroup placeholder2; + public ServoGroup servo; public IGyro imu; public Webcam camera; @@ -67,7 +67,7 @@ public Hardware(HardwareMap hwmap) { } if (Setup.Connected.TESTSUBSYSTEM) { if (Setup.Connected.SERVO) { - this.servo = new Servo(Setup.HardwareNames.SERVO); + this.servo = new ServoGroup(Setup.HardwareNames.SERVO); } // if (Setup.Connected.COLOR_SENSOR) { // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java index 17fb2d71..bf36ee12 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/ClawAndWristSubsystem.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.hoops.subsystems; import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -31,11 +31,11 @@ public class ClawAndWristSubsystem implements Subsystem, Loggable { @Log(name = "Wrist") public double WristPos; - private Servo claw1; - private Servo claw2; - private Servo wrist; + private ServoGroup claw1; + private ServoGroup claw2; + private ServoGroup wrist; - public ClawAndWristSubsystem(Servo c1, Servo c2, Servo w) { + public ClawAndWristSubsystem(ServoGroup c1, ServoGroup c2, ServoGroup w) { claw1 = c1; claw2 = c2; wrist = w; diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java index 89598a11..6370224a 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/PlacementSubsystem.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -17,7 +17,7 @@ public class PlacementSubsystem implements Subsystem, Loggable { public static double ScoreServo = 0.5; public static double ArmServo = 0.5; - public Servo armServo; + public ServoGroup armServo; public static double ScoreServoInput = 0.5; @@ -27,7 +27,7 @@ public class PlacementSubsystem implements Subsystem, Loggable { public static double ArmServoOutput = 0.5; - public Servo scoreServo; + public ServoGroup scoreServo; public DcMotorEx liftMotor; private boolean isHardware; diff --git a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java index 54dd2b82..4cc5857a 100644 --- a/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java +++ b/LearnBot/src/main/java/org/firstinspires/ftc/hoops/subsystems/TestSubsystem.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.util.Range; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -37,7 +37,7 @@ public class TestSubsystem implements Subsystem, Loggable { // but only while/if the the distance is greater than 10cm private EncodedMotor theMotor; - private Servo servo; + private ServoGroup servo; private Rev2MDistanceSensor theSensor; private boolean running; diff --git a/Osprey/build.gradle b/Osprey/build.gradle deleted file mode 100644 index 40bc5902..00000000 --- a/Osprey/build.gradle +++ /dev/null @@ -1,39 +0,0 @@ -// -// build.gradle in Ptechnodactyl -// -// Most of the definitions for building your module reside in a common, shared -// file 'build.common.gradle'. Being factored in this way makes it easier to -// integrate updates to the FTC into your code. If you really need to customize -// the build definitions, you can place those customizations in this file, but -// please think carefully as to whether such customizations are really necessary -// before doing so. - - -// Custom definitions may go here - -// Include common definitions from above. -apply from: '../build.common.gradle' -apply from: '../build.dependencies.gradle' - -android { - namespace = 'org.firstinspires.ftc.learnbot' - androidResources { - noCompress 'tflite' - } - - packagingOptions { - jniLibs { - pickFirsts += ['**/*.so'] - } - jniLibs.useLegacyPackaging true - } -} - -dependencies { - implementation project(':FtcRobotController') - // Uncomment this to use a local version of TechnoLib - // implementation project(':RobotLibrary') // FLIP: TechnoLibLocal - // implementation project(':Path') // FLIP: TechnoLibLocal - // implementation project(':Vision') // FLIP: TechnoLibLocal - annotationProcessor files('lib/OpModeAnnotationProcessor.jar') -} diff --git a/Osprey/lib/OpModeAnnotationProcessor.jar b/Osprey/lib/OpModeAnnotationProcessor.jar deleted file mode 100644 index 4825cc36d9606badc3c0eac0f61079f9c5a4d196..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6032 zcmbVQ2UL?;(+*83p-BxzlwKk&NReKJgwU&k8X!OblRy9)0@8(msC0-@1f+)|MG>X< zBH$`gY&1cTZUOaAR##Ye@n645a?X2BX6D{^X6}9F88a9aH9LTomKK1zJZ=WqA?yHZ z09@Zn`;d`|fec*R#K=J3(n=a`@TCg?;OLfP3O)d&86bqIVkx3Zi~EWfV>yTPjt};K z&T063*y0U7^^AWM3hrs|vD|!Hc7VwW=< z^USZe+R{J+$(@&@(xgzGcK*h3phqME+wC~xVm~4Yd|4%D)xvuo6tduY12Xnl0ytqU zxrXyh{-f-AUiDB9>U^0ioy2BWCPcWSz0G8Z$C!z>+dmA!72djV%N`src3M8*kvwj! z8ZK4mT;=98(Ukdt0hs=Z5LtydWbYGpqsJ$|mE-6&OO0y{C9-?2gDPDnhuVllRMSv7>=Eo06tDbqH+jLqAri*`wQp}j|{^x&)vPV!+G;`Q5W4eh9^reuRZI? zPm!>hCzRFWt*`r4jlJYQ_9z@4X+nWKPT&ZQm`(sg^;%}qIu};K8HZsuaTwN$kk#&M zhK9Dl_0(%GqbT2TR3E4{lfE0M#I->yGYdI<4!waD{uTGKZYxf? z^wqg~t=cd14-1tqadBi=E$@qitc{^KyB$ut+Nn#$5fTcyS=65Aa?}dfMO&WLZ!J0t zZ=FA@+nR0>0PCA*Qt{X-a{e3XtWqyNR4 zVH_8d6IkafIz&Ll-AND69vB~XqT=Dd6d=f#gko;(xny_7doRspP(m1gH!gA}N~D}( zC|X4~!@yDL%Dzx1m3NdLOzpF7i3`lWX?X{H-wR}q(`#6JbKdNv4D2HY#>FDs?}D-{ zKUX`WRPuWo{G*fjnQ9b{O7dc69JSvs9XJyZGNK`LHn0X8G0U|4+!kFZ?Rks`UCZf_ zCo!AGa@}z5)zmZ_?bPAJw?-TTTD;4A&)d^5+Sa-?H56A5rm>*83~n4;x;+Cs>?&W$be#pGtaO!;J`zjZ6RkOi$qkig* zr*a<)AfZS`?T%<2^O+M{W_^&PPX(1G?|SV==RCTiAa{!N6+73DU_8bdEVHHx;vap8 zEx{f4jBy@YfLwI70zxD7Uh_jXR0NsFSZdT))YWGKdjjDwt<|zJzKho%jmWr%c{y*j z-CZOcEK4mEARsWhGD*%f7el*m0#fRYXF*~TMnXAqRi7xIzF+jAQkQa^a<+q;K88Hh zb-#6Wg2^zR<22J&h@|+fBj;EzH89+s^(bbH6Jmd;ED1^rT^E+XpD+sbv1!H9J?_o= zG@#rBbdJhs-H0*U6!(SWkQb2`XZKCVzLu_2{Hn%j*sJVuaTzTE{raA|gE`hbV)eFX zVHsa@)A&p&0afg{rn&dzpH8eccHcAgtf1S06lIbYE!=vN&IqQuUF7q2gkr5lrGs)KePp*LABX z!o7|4f&=QNdmrSqQIEHQ>~#8FJ&dIe7sk4#6G0MduZ*x&L+Us4JU1okLzn_$7HWg} zYw^-saps+%4ocX?TF2l(6b3&M1bWyc6!7+Vd=D!*I|8sAnptE^(vkcVl9bw zI-Enpk-%zpl&62ryJF>!XYLDh$=!3$?QWmC9x}|q_lep|=?NYcPiQqg6uvg@z2g_k)T} zef9tVlT6=$_us+I=6?q_VO=t~X`@}Vf553B#>)li^9}V%ERUPAX|sRX1LaDRC}|ap z=CT^D7LT#ji`O#5i%w;Wi#DWW@jq)^K#vpBg5{Pm9|Qula#B9LU|u>3>i4oez*w)a z=;}Z1Fs(YHI`j6Ore+XDZD{00qW6e_W}DX$$8v9uCwS@SF**X#+)7P;yh^eN#9$LV z;;rkD-%Z8vBE`gF@Leaj$b{TO%O=XDOND)DObNCG#H>UOo~mgz;84}deWZCZ(8$s9 zOuKXpmxl@MWj3aedN4Y%0!i`&W0#W z|GKtI^5nEgg>?~HaDh8h+4q+^_d1(1ln;!OWu zWlwyCZ0|Av!s!aK`Kn|OT@3E+YD)rpMl9BY;*3?{h0y`9W>Z4%6l~#1b_IV!CBKKg znCt_i_`0MAD+KG1L27Y*!kpsSu(U=G?#}gRl&y~yJK=tIwD_j26|E4?qx93DsP|PP z5YQ0cwYw;!9POM6VR}Gc<`h@^gHVp3%y|YQ$BsuzMjF8}2R||0smId02^y>FKsH!2 zV#}wU!5R&bi-INU!WuPMZ*<)Q1Y%$F5%7dIQG*L`4Fkxrt%w_5*N7SiH#s;j-@h;m z87-AtFv##>m1+)5ADFNVV@?=p7w5seYrTG3jM9R6D>NSRN5>TPbYQabJ|*JG0W`g%7P)0QC!+; zEi{75FACU3%aM7iYrfQ)FlPGwTL4XHF^EcX(n(;>ysYrVgA*DW&nGC}BARtsA<`Zx zhY)?uYMgxFBX&;W`wc}F4(gVck03e(-taccKE4LcV0=|7n_SP!YKeNY|H_fE4h4a0 z=Prn`MT{pYBCt|$r(Ec#!6@N@&nXGD7SDX7?#%Iyvl(T>v0b?ygI>lRvZJ-=@zuTi z^&G;rM|ldGT&Kg**+sR;JNYNHRG<5Uyvf6S^7I%HZI zwQ>8ffg|;t_`vI2Pas%bKtH)6IG&ZW`b6W6ii)Gu+9W zeUC4PF4BAypt!;^qLF)2P`WEc^wyPlKbr?6-msUpj$(%DGz9?QKuYTFEP;-c&>!q( zcQrDZ_@RbF`uL+rEMNx((EF)Ql>SxU1L2Q2E4@u7q`$KX6xvPt2LujGHRRuUa)CSz6cuPZ+ z$#k*~q4e+X!{mFE-ZEEA5=ut#o}_m_VGlD=fmg@Kbn%J-sI6Y(10*vQ1#jw~En$QW z>nc1oRjtDZH)qwDh<&*2cHRqH0Aa(fbVENYxFieS%=A-6%;Y$UTG`{{HXql?-*7aI zsk_nWAn9OJ@r12S|N$^^D?{81gdDjudb^4MM?69gpX7(jzCr`mE$jq)k1?A zih}5SN~eT}GyBS0ar1mi+#>;dE)i)M-KRlKif;q{EYq>H$v6$tq2&2~D8Ka>Kj%mJ zH}a$H=ZisOPGkIV+c|31Vu^6uL{tSUPg8E2c6+EJ^9J=WKH6MsX6>ziDJRqe zsiRVka%qyHSZI)QNsR3PE?KLbrU?!{pK=XGtspu%2q+Wk-PhYt=^`$gOqj8tWNPbs zD&Cea9+StzP-6UWSw?kzD4vSm0pIotKhE3@I+=U$*y0l|sH(q)#7qPaP9iAvK=a|R z=M>nnR()}gaY|g3KbTS z>owz4-8UfVwLcFBJX*ztW^|ir}0)qo|CQ zuUMy~%%VHwKJ#aqsK~d;sEZGqHYrVlf2N6ok{z%;8`?RaAoXNAF zZILI%@OM1_V-iFT+h4&aFSWBvA$Q{2!b}>H{f|riKYjVmJ&^mMoxn=&hqi^0;Wzj2 zqdWRld5|G`TPA=zkoZ?-f3Fc560rTK%^d)~9bPho??h2Dl5Wf4f3^Qlfc~pZ{{8s$ zkQVs{&%2i{8KAdi@w>$ixZd5A4A|QOBGEkZVn0lG!uRe@$N;`ACc8V?3Fo`JBLn)j z#Qqm|Kj3{kcVqzB2`yxR-*csNk*u diff --git a/Osprey/src/main/AndroidManifest.xml b/Osprey/src/main/AndroidManifest.xml deleted file mode 100644 index 3705b315..00000000 --- a/Osprey/src/main/AndroidManifest.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java deleted file mode 100644 index 930868d6..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl; - -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.motor.Motor; -import com.technototes.library.hardware.sensor.AdafruitIMU; -import com.technototes.library.hardware.sensor.ColorDistanceSensor; -import com.technototes.library.hardware.sensor.IGyro; -import com.technototes.library.hardware.sensor.IMU; -import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.logger.Loggable; -import com.technototes.vision.hardware.Webcam; -import java.util.List; -import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; - -public class Hardware implements Loggable { - - public List hubs; - - public EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor, arm; - public Motor placeholder1; - public DcMotorEx liftMotor; - - public Servo placeholder2; - public Servo servo, clawServo; - - public IGyro imu; - public Webcam camera; - public Rev2MDistanceSensor distanceSensor; - public ColorDistanceSensor colorSensor; - - public Hardware(HardwareMap hwmap) { - hubs = hwmap.getAll(LynxModule.class); - if (Setup.Connected.EXTERNAL_IMU) { - imu = new AdafruitIMU(Setup.HardwareNames.EXTERNAL_IMU, AdafruitIMU.Orientation.Pitch); - Setup.HardwareNames.MOTOR = "External IMU is in use"; - } else { - imu = new IMU( - Setup.HardwareNames.IMU, - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD - ); - Setup.HardwareNames.MOTOR = "Internal IMU is being used"; - } - if (Setup.Connected.DRIVEBASE) { - this.frMotor = new EncodedMotor<>(Setup.HardwareNames.FRMOTOR); - this.flMotor = new EncodedMotor<>(Setup.HardwareNames.FLMOTOR); - this.rrMotor = new EncodedMotor<>(Setup.HardwareNames.RRMOTOR); - this.rlMotor = new EncodedMotor<>(Setup.HardwareNames.RLMOTOR); - if (Setup.Connected.DISTANCE_SENSOR) { - this.distanceSensor = new Rev2MDistanceSensor(Setup.HardwareNames.DISTANCE); - } - } - if (Setup.Connected.MOTOR) { - this.theMotor = new EncodedMotor<>(Setup.HardwareNames.MOTOR); - } - if (Setup.Connected.FLYWHEEL) { - this.placeholder1 = new Motor<>(Setup.HardwareNames.FLYWHEELMOTOR); - } - if (Setup.Connected.WEBCAM) { - camera = new Webcam(Setup.HardwareNames.CAMERA); - } - if (Setup.Connected.TESTSUBSYSTEM) { - if (Setup.Connected.SERVO) { - this.servo = new Servo(Setup.HardwareNames.SERVO); - } - // if (Setup.Connected.COLOR_SENSOR) { - // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); - // } - } - if (Setup.Connected.CLAWSUBSYSTEM) { - this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO); - this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM); - } - } - - // We can read the voltage from the different hubs for fun... - public double voltage() { - double volt = 0; - double count = 0; - for (LynxModule lm : hubs) { - count += 1; - volt += lm.getInputVoltage(VoltageUnit.VOLTS); - } - return volt / count; - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java deleted file mode 100644 index 0a34a722..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl; - -import com.technototes.library.logger.Loggable; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -public class Robot implements Loggable { - - public StartingPosition position; - public Alliance alliance; - public double initialVoltage; - public DrivebaseSubsystem drivebaseSubsystem; - public ClawSubsystem clawSubsystem; - - public Robot(Hardware hw) { - this.initialVoltage = hw.voltage(); - if (Setup.Connected.DRIVEBASE) { - this.drivebaseSubsystem = new DrivebaseSubsystem( - hw.flMotor, - hw.frMotor, - hw.rlMotor, - hw.rrMotor, - hw.imu - ); - } - if (Setup.Connected.CLAWSUBSYSTEM) { - this.clawSubsystem = new ClawSubsystem(hw); - } - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java deleted file mode 100644 index 39a497ac..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl; - -import com.acmerobotics.dashboard.config.Config; - -public class Setup { - - @Config - public static class Connected { - - public static boolean DRIVEBASE = true; - public static boolean TESTSUBSYSTEM = false; - public static boolean CLAWSUBSYSTEM = true; - public static boolean MOTOR = false; - public static boolean SERVO = false; - public static boolean ARM = true; - public static boolean CLAWSERVO = true; - public static boolean DISTANCE_SENSOR = false; - public static boolean COLOR_SENSOR = false; - public static boolean FLYWHEEL = false; - public static boolean WEBCAM = false; - public static boolean EXTERNAL_IMU = false; - } - - @Config - public static class HardwareNames { - - public static String MOTOR = "motor"; - public static String FLMOTOR = "fl"; - public static String FRMOTOR = "fr"; - public static String RLMOTOR = "rl"; - public static String RRMOTOR = "rr"; - public static String FLYWHEELMOTOR = "fly"; - public static String SERVO = "s"; - public static String IMU = "imu"; - public static String EXTERNAL_IMU = "adafruit-imu"; - public static String DISTANCE = "d"; - public static String COLOR = "c"; - public static String CAMERA = "camera"; - public static String ARM = "arm"; - public static String CLAWSERVO = "clawServo"; - } - - @Config - public static class GlobalSettings { - - public static double STICK_DEAD_ZONE = 0.1; - public static double STRAIGHTEN_SCALE_FACTOR = 0.25; - public static double STRAIGHTEN_RANGE = .15; // Fraction of 45 degrees... - public static double TRIGGER_THRESHOLD = 0.7; - public static double STRAIGHTEN_DEAD_ZONE = 0.08; - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java deleted file mode 100644 index 2e11e466..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/DrivingCommands.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands; - -import com.technototes.library.command.Command; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -public class DrivingCommands { - - public static Command NormalDriving(DrivebaseSubsystem ds) { - return Command.create(ds::setNormalMode); - } - - public static Command SnailDriving(DrivebaseSubsystem ds) { - return Command.create(ds::setSnailMode); - } - - public static Command TurboDriving(DrivebaseSubsystem ds) { - return Command.create(ds::setTurboMode); - } - - public static Command NormalSpeed(DrivebaseSubsystem ds) { - return Command.create(ds::auto); - } - - public static Command SlowSpeed(DrivebaseSubsystem ds) { - return Command.create(ds::slow); - } - - public static Command FastSpeed(DrivebaseSubsystem ds) { - return Command.create(ds::fast); - } - - public static Command ResetGyro(DrivebaseSubsystem ds) { - return Command.create(ds::setExternalHeading, 0.0); - } - - public static Command RecordHeading(DrivebaseSubsystem drive) { - return Command.create(drive::saveHeading); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java deleted file mode 100644 index 934521a4..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/EZCmd.java +++ /dev/null @@ -1,27 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands; - -import com.technototes.library.command.Command; -import com.technototes.library.command.MethodCommand; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -public class EZCmd { - - public static class Drive { - - public static Command TurboMode(DrivebaseSubsystem db) { - return new MethodCommand(db::setTurboMode); - } - - public static Command NormalMode(DrivebaseSubsystem db) { - return new MethodCommand(db::setNormalMode); - } - - public static Command SnailMode(DrivebaseSubsystem db) { - return new MethodCommand(db::setSnailMode); - } - - public static Command ResetGyro(DrivebaseSubsystem db) { - return new MethodCommand(db::setExternalHeading, 0.0); - } - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java deleted file mode 100644 index 74ef7baf..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java +++ /dev/null @@ -1,139 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands; - -import com.acmerobotics.roadrunner.drive.DriveSignal; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.technototes.library.command.Command; -import com.technototes.library.control.Stick; -import com.technototes.library.logger.Loggable; -import com.technototes.library.util.MathUtils; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; - -public class JoystickDriveCommand implements Command, Loggable { - - public DrivebaseSubsystem subsystem; - public DoubleSupplier x, y, r; - public BooleanSupplier watchTrigger; - public double targetHeadingRads; - public DoubleSupplier driveStraighten; - public DoubleSupplier drive45; - public boolean driverDriving; - public boolean operatorDriving; - - public JoystickDriveCommand( - DrivebaseSubsystem sub, - Stick xyStick, - Stick rotStick, - DoubleSupplier strtDrive, - DoubleSupplier angleDrive - ) { - addRequirements(sub); - subsystem = sub; - x = xyStick.getXSupplier(); - y = xyStick.getYSupplier(); - r = rotStick.getXSupplier(); - targetHeadingRads = -sub.getExternalHeading(); - driveStraighten = strtDrive; - drive45 = angleDrive; - driverDriving = true; - operatorDriving = false; - } - - // Use this constructor if you don't want auto-straightening - public JoystickDriveCommand(DrivebaseSubsystem sub, Stick xyStick, Stick rotStick) { - this(sub, xyStick, rotStick, null, null); - } - - // This will make the bot snap to an angle, if the 'straighten' button is pressed - // Otherwise, it just reads the rotation value from the rotation stick - private double getRotation(double headingInRads) { - // Check to see if we're trying to straighten the robot - double normalized = 0.0; - boolean straightTrigger; - boolean fortyfiveTrigger; - straightTrigger = isTriggered(driveStraighten); - fortyfiveTrigger = isTriggered(drive45); - if (!straightTrigger && !fortyfiveTrigger) { - // No straighten override: return the stick value - // (with some adjustment...) - return -Math.pow(r.getAsDouble(), 3) * subsystem.speed; - } - if (straightTrigger) { - // headingInRads is [0-2pi] - double heading = -Math.toDegrees(headingInRads); - // Snap to the closest 90 or 270 degree angle (for going through the depot) - double close = MathUtils.closestTo(heading, 0, 90, 180, 270, 360); - double offBy = close - heading; - // Normalize the error to -1 to 1 - normalized = Math.max(Math.min(offBy / 45, 1.), -1.); - // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem) - if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) { - return 0.0; - } - } else { - // headingInRads is [0-2pi] - double heading45 = -Math.toDegrees(headingInRads); - // Snap to the closest 90 or 270 degree angle (for going through the depot) - double close45 = MathUtils.closestTo(heading45, 45, 135, 225, 315); - double offBy45 = close45 - heading45; - // Normalize the error to -1 to 1 - normalized = Math.max(Math.min(offBy45 / 45, 1.), -1.); - // Dead zone of 5 degreesLiftHighJunctionCommand(liftSubsystem) - if (Math.abs(normalized) < Setup.GlobalSettings.STRAIGHTEN_DEAD_ZONE) { - return 0.0; - } - } - // Scale it by the cube root, the scale that down by 30% - // .9 (about 40 degrees off) provides .96 power => .288 - // .1 (about 5 degrees off) provides .46 power => .14 - return Math.cbrt(normalized) * 0.3; - } - - public static boolean isTriggered(DoubleSupplier ds) { - if (ds == null || ds.getAsDouble() < Setup.GlobalSettings.TRIGGER_THRESHOLD) { - return false; - } - return true; - } - - @Override - public void execute() { - // If subsystem is busy it is running a trajectory. - if (!subsystem.isBusy()) { - double curHeading = -subsystem.getExternalHeading(); - - // The math & signs looks wonky, because this makes things field-relative - // (Remember that "3 O'Clock" is zero degrees) - double yvalue = y.getAsDouble(); - double xvalue = x.getAsDouble(); - if (driveStraighten != null) { - if (driveStraighten.getAsDouble() > 0.7) { - if (Math.abs(yvalue) > Math.abs(xvalue)) xvalue = 0; - else yvalue = 0; - } - } - Vector2d input = new Vector2d( - yvalue * subsystem.speed, - xvalue * subsystem.speed - ).rotated(curHeading); - - subsystem.setWeightedDrivePower( - new Pose2d(input.getX(), input.getY(), getRotation(curHeading)) - ); - } - subsystem.update(); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean cancel) { - if (cancel) subsystem.setDriveSignal(new DriveSignal()); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java deleted file mode 100644 index 6d90a265..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.commands; - -import com.qualcomm.robotcore.util.ElapsedTime; -import com.technototes.library.command.Command; -import com.technototes.library.control.Stick; -import com.technototes.library.logger.Loggable; -import java.util.function.DoubleSupplier; -import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; - -public class JoystickIncDecCmd implements Command, Loggable { - - public ClawSubsystem subsystem; - public DoubleSupplier x; - public static ElapsedTime timer; - - public static double time; - - public double interval = 1.5; - - public JoystickIncDecCmd(ClawSubsystem sub, Stick xStick) { - addRequirements(sub); - subsystem = sub; - x = xStick.getXSupplier(); - } - - // This will make the bot snap to an angle, if the 'straighten' button is pressed - // Otherwise, it just reads the rotation value from the rotation stick - - @Override - public void execute() { - double xvalue = -x.getAsDouble(); - // time = timer.time(); - // if (interval < time) - subsystem.increment(xvalue); - // timer.reset(); - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java deleted file mode 100644 index 2727dddc..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.controllers; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandAxis; -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; - -public class DriverController { - - public Robot robot; - public CommandGamepad gamepad; - - public Stick driveLeftStick, driveRightStick; - public CommandButton resetGyroButton, turboButton, snailButton; - public CommandButton override; - - public DriverController(CommandGamepad g, Robot r) { - this.robot = r; - gamepad = g; - override = g.leftTrigger.getAsButton(0.5); - - AssignNamedControllerButton(); - if (Setup.Connected.DRIVEBASE) { - bindDriveControls(); - } - } - - private void AssignNamedControllerButton() { - resetGyroButton = gamepad.ps_options; - driveLeftStick = gamepad.leftStick; - driveRightStick = gamepad.rightStick; - turboButton = gamepad.leftBumper; - snailButton = gamepad.rightBumper; - } - - public void bindDriveControls() { - CommandScheduler.scheduleJoystick( - new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) - ); - - turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); - turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); - snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); - snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); - resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java deleted file mode 100644 index 20985224..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.controllers; - -import android.support.v4.app.INotificationSideChannel; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandAxis; -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; - -public class OneController { - - public Robot robot; - public CommandGamepad gamepad; - public CommandButton openClaw; - public CommandButton closeClaw; - public CommandButton ArmHorizontal; - public CommandButton ArmVertical; - public Stick driveLeftStick, driveRightStick; - public CommandButton resetGyroButton, turboButton, snailButton; - public CommandButton increment; - public CommandButton decrement; - - public OneController(CommandGamepad g, Robot r) { - robot = r; - gamepad = g; - AssignNamedControllerButton(); - BindControls(); - } - - private void AssignNamedControllerButton() { - openClaw = gamepad.leftBumper; - closeClaw = gamepad.rightBumper; - ArmHorizontal = gamepad.ps_square; - ArmVertical = gamepad.ps_triangle; - increment = gamepad.ps_circle; - decrement = gamepad.ps_cross; - resetGyroButton = gamepad.ps_options; - driveLeftStick = gamepad.leftStick; - driveRightStick = gamepad.rightStick; - turboButton = gamepad.leftBumper; - snailButton = gamepad.rightBumper; - } - - public void BindControls() { - if (Setup.Connected.CLAWSUBSYSTEM) { - bindClawSubsystemControls(); - } - if (Setup.Connected.DRIVEBASE) { - bindDriveControls(); - } - } - - public void bindClawSubsystemControls() { - openClaw.whenPressed(robot.clawSubsystem::openClaw); - closeClaw.whenPressed(robot.clawSubsystem::closeClaw); - ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); - ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); - increment.whenPressed(robot.clawSubsystem::incrementn); - decrement.whenPressed(robot.clawSubsystem::decrement); - } - - public void bindDriveControls() { - CommandScheduler.scheduleJoystick( - new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) - ); - - turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); - turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); - snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); - snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); - resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java deleted file mode 100644 index 0cd94954..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.controllers; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; - -public class OperatorController { - - public Robot robot; - public CommandGamepad gamepad; - public CommandButton openClaw; - public CommandButton closeClaw; - public Stick armStick; - public CommandButton ArmHorizontal; - public CommandButton ArmVertical; - public CommandButton intake; - - public OperatorController(CommandGamepad g, Robot r) { - robot = r; - gamepad = g; - AssignNamedControllerButton(); - BindControls(); - } - - private void AssignNamedControllerButton() { - openClaw = gamepad.leftBumper; - closeClaw = gamepad.rightBumper; - armStick = gamepad.rightStick; - ArmHorizontal = gamepad.ps_circle; - ArmVertical = gamepad.ps_triangle; - intake = gamepad.dpadRight; - } - - public void BindControls() { - if (Setup.Connected.CLAWSUBSYSTEM) { - bindClawSubsystemControls(); - } - } - - public void bindClawSubsystemControls() { - openClaw.whenPressed(robot.clawSubsystem::openClaw); - closeClaw.whenPressed(robot.clawSubsystem::closeClaw); - ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); - ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); - intake.whenPressed(robot.clawSubsystem::setArmIntake); - CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java deleted file mode 100644 index e9ddeaf9..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.controllers; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; - -public class TestController { - - public Robot robot; - public CommandGamepad gamepad; - public CommandButton testPower2; - public CommandButton testPower; - public CommandButton armHorizontal; - public Stick armStick; - - public TestController(CommandGamepad g, Robot r) { - robot = r; - gamepad = g; - AssignNamedControllerButton(); - BindControls(); - } - - private void AssignNamedControllerButton() { - testPower = gamepad.leftBumper; - testPower2 = gamepad.rightBumper; - armStick = gamepad.rightStick; - armHorizontal = gamepad.ps_circle; - } - - public void BindControls() { - bindTestControls(); - } - - public void bindTestControls() { - testPower.whenPressed(robot.clawSubsystem::powIncrement); - testPower2.whenPressed(robot.clawSubsystem::powDecrement); - CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java deleted file mode 100644 index af82fa17..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/HeadingHelper.java +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.helpers; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity; - -@Config -public class HeadingHelper { - - public static double DEFAULT_START_HEADING = 180; - public static double DEFAULT_START_X = 53; - public static double DEFAULT_START_Y = 63; - public static int EXPIRATION_TIME = 20; - - public double headingUpdateTime; - public double lastHeading; - public double lastXPosition; - public double lastYPosition; - - public HeadingHelper(double lastX, double lastY, double heading) { - headingUpdateTime = System.currentTimeMillis() / 1000.0; - lastXPosition = lastX; - lastYPosition = lastY; - lastHeading = heading; - } - - public static void saveHeading(double x, double y, double h) { - FtcRobotControllerActivity.SaveBetweenRuns = new HeadingHelper(x, y, h); - } - - public static void savePose(Pose2d p) { - saveHeading(p.getX(), p.getY(), p.getHeading()); - } - - public static void clearSavedInfo() { - FtcRobotControllerActivity.SaveBetweenRuns = null; - } - - public static boolean validHeading() { - HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; - if (hh == null) { - return false; - } - double now = System.currentTimeMillis() / 1000.0; - return now < hh.headingUpdateTime + EXPIRATION_TIME; - } - - public static Pose2d getSavedPose() { - return new Pose2d(getSavedX(), getSavedY(), getSavedHeading()); - } - - public static double getSavedHeading() { - HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; - if (hh != null) { - return hh.lastHeading; - } - return DEFAULT_START_HEADING; - } - - public static double getSavedX() { - HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; - if (hh != null) { - return hh.lastXPosition; - } - return DEFAULT_START_X; - } - - public static double getSavedY() { - HeadingHelper hh = (HeadingHelper) FtcRobotControllerActivity.SaveBetweenRuns; - if (hh != null) { - return hh.lastYPosition; - } - return DEFAULT_START_Y; - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java deleted file mode 100644 index 48f4bea7..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/helpers/StartingPosition.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.helpers; - -public enum StartingPosition { - Backstage, - Wing, - Unspecified, -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java deleted file mode 100644 index a77ee77b..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.path.command.TrajectorySequenceCommand; -import com.technototes.path.geometry.ConfigurablePoseD; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; - -/* - * Some Emojis for opmode names: - * Copy them and paste them in the 'name' section of the @Autonomous tag - * Red alliance: 🟥 - * Blue alliance: 🟦 - * Wing position: 🪶 - * Backstage pos: 🎦 - */ -//@Config -@Autonomous(name = "Basic Auto", preselectTeleOp = "PlainDrive") -@SuppressWarnings("unused") -public class Basic extends CommandOpMode implements Loggable { - - public static int AUTO_TIME = 1; - public Hardware hardware; - public Robot robot; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - CommandScheduler.scheduleForState( - new SequentialCommandGroup( - new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> - b - .apply(new ConfigurablePoseD(0, 0, 0).toPose()) - .lineTo(new Vector2d(5, 5)) - .build() - ), - // new TurboCommand(robot.drivebaseSubsystem), - // new StartSpinningCmd(robot.spinner), - new WaitCommand(AUTO_TIME), - new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> - b - .apply(new ConfigurablePoseD(5, 5, 0).toPose()) - .lineTo(new Vector2d(0, 0)) - .build() - ), - // new StopSpinningCmd(robot.spinner), - CommandScheduler::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java deleted file mode 100644 index 6302c59d..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.library.util.Alliance; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; -import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; -import org.firstinspires.ftc.ptechnodactyl.controllers.OperatorController; -import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; - -@TeleOp(name = "PT Driving w/Turbo!") -@SuppressWarnings("unused") -public class JustDrivingTeleOp extends CommandOpMode implements Loggable { - - public Robot robot; - public DriverController controlsDriver; - public OperatorController controlsOperator; - public Hardware hardware; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - controlsOperator = new OperatorController(codriverGamepad, robot); - if (Setup.Connected.DRIVEBASE) { - controlsDriver = new DriverController(driverGamepad, robot); - CommandScheduler.scheduleForState( - EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), - OpModeState.INIT - ); - } - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java deleted file mode 100644 index d65a0b52..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; -import org.firstinspires.ftc.ptechnodactyl.controllers.TestController; - -@TeleOp(name = "MotorTest") -@SuppressWarnings("unused") -public class MotorTest extends CommandOpMode implements Loggable { - - public Hardware hardware; - public Robot robot; - - public TestController test; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - test = new TestController(driverGamepad, robot); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java deleted file mode 100644 index a6501b33..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; - -@TeleOp(name = "PlainDrive") -@SuppressWarnings("unused") -public class PlainDrive extends CommandOpMode implements Loggable { - - public Hardware hardware; - public Robot robot; - - public DriverController driver; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - driver = new DriverController(driverGamepad, robot); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java deleted file mode 100644 index 69cd1dc5..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; -import org.firstinspires.ftc.ptechnodactyl.controllers.OneController; - -@TeleOp(name = "OneController") -@SuppressWarnings("unused") -public class oneController extends CommandOpMode implements Loggable { - - public Hardware hardware; - public Robot robot; - - public OneController driver; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - driver = new OneController(driverGamepad, robot); - CommandScheduler.register(robot.clawSubsystem); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md deleted file mode 100644 index c2a5bed5..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/readme.md +++ /dev/null @@ -1,140 +0,0 @@ -## TeamCode Module - -Welcome! - -This module, TeamCode, is the place where you will write/paste the code for your -team's robot controller App. This module is currently empty (a clean slate) but -the process for adding OpModes is straightforward. - -## Creating your own OpModes - -The easiest way to create your own OpMode is to copy a Sample OpMode and make it -your own. - -Sample opmodes exist in the FtcRobotController module. To locate these samples, -find the FtcRobotController module in the "Project/Android" tab. - -Expand the following tree elements: -FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples - -### Naming of Samples - -To gain a better understanding of how the samples are organized, and how to -interpret the naming system, it will help to understand the conventions that -were used during their creation. - -These conventions are described (in detail) in the sample_conventions.md file in -this folder. - -To summarize: A range of different samples classes will reside in the -java/external/samples. The class names will follow a naming convention which -indicates the purpose of each class. The prefix of the name will be one of the -following: - -Basic: This is a minimally functional OpMode used to illustrate the -skeleton/structure of a particular style of OpMode. These are bare bones -examples. - -Sensor: This is a Sample OpMode that shows how to use a specific sensor. It is -not intended to drive a functioning robot, it is simply showing the minimal code -required to read and display the sensor values. - -Robot: This is a Sample OpMode that assumes a simple two-motor (differential) -drive base. It may be used to provide a common baseline driving OpMode, or to -demonstrate how a particular sensor or concept can be used to navigate. - -Concept: This is a sample OpMode that illustrates performing a specific function -or concept. These may be complex, but their operation should be explained -clearly in the comments, or the comments should reference an external doc, guide -or tutorial. Each OpMode should try to only demonstrate a single concept so they -are easy to locate based on their name. These OpModes may not produce a drivable -robot. - -After the prefix, other conventions will apply: - -- Sensor class names are constructed as: Sensor - Company - Type -- Robot class names are constructed as: Robot - Mode - Action - OpModetype -- Concept class names are constructed as: Concept - Topic - OpModetype - -Once you are familiar with the range of samples available, you can choose one to -be the basis for your own robot. In all cases, the desired sample(s) needs to be -copied into your TeamCode module to be used. - -This is done inside Android Studio directly, using the following steps: - -1. Locate the desired sample class in the Project/Android tree. - -2. Right click on the sample class and select "Copy" - -3. Expand the TeamCode/java folder - -4. Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" - -5. You will be prompted for a class name for the copy. Choose something - meaningful based on the purpose of this class. Start with a capital letter, - and remember that there may be more similar classes later. - -Once your copy has been created, you should prepare it for use on your robot. -This is done by adjusting the OpMode's name, and enabling it to be displayed on -the Driver Station's OpMode list. - -Each OpMode sample class begins with several lines of code like the ones shown -below: - -``` - @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") - @Disabled -``` - -The name that will appear on the driver station's "opmode list" is defined by -the code: `name="Template: Linear OpMode"` You can change what appears between -the quotes to better describe your opmode. The "group=" portion of the code can -be used to help organize your list of OpModes. - -As shown, the current OpMode will NOT appear on the driver station's OpMode list -because of the `@Disabled` annotation which has been included. This line can -simply be deleted , or commented out, to make the OpMode visible. - -## ADVANCED Multi-Team App management: Cloning the TeamCode Module - -In some situations, you have multiple teams in your club and you want them to -all share a common code organization, with each being able to _see_ the others -code but each having their own team module with their own code that they -maintain themselves. - -In this situation, you might wish to clone the TeamCode module, once for each of -these teams. Each of the clones would then appear along side each other in the -Android Studio module list, together with the FtcRobotController module (and the -original TeamCode module). - -Selective Team phones can then be programmed by selecting the desired Module -from the pulldown list prior to clicking to the green Run arrow. - -Warning: This is not for the inexperienced Software developer. You will need to -be comfortable with File manipulations and managing Android Studio Modules. -These changes are performed OUTSIDE of Android Studios, so close Android Studios -before you do this. - -Also.. Make a full project backup before you start this :) - -To clone TeamCode, do the following: - -Note: Some names start with "Team" and others start with "team". This is -intentional. - -1. Using your operating system file management tools, copy the whole "TeamCode" - folder to a sibling folder with a corresponding new name, eg: "Team0417". - -2. In the new Team0417 folder, delete the TeamCode.iml file. - -3. the new Team0417 folder, rename the - "src/main/java/org/firstinspires/ftc/teamcode" folder to a matching name - with a lowercase 'team' eg: "team0417". - -4. In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, - change the line that contains package="org.firstinspires.ftc.teamcode" to be - package="org.firstinspires.ftc.team0417" - -5. Add: include ':Team0417' to the "/settings.gradle" file. -6. Open up Android Studios and clean out any old files by using the menu to - "Build/Clean Project"" diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java deleted file mode 100644 index 8ae36651..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java +++ /dev/null @@ -1,201 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.control.PIDCoefficients; -import com.acmerobotics.roadrunner.control.PIDFController; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.logger.Log; -import com.technototes.library.logger.Loggable; -import com.technototes.library.subsystem.Subsystem; -import org.firstinspires.ftc.ptechnodactyl.Hardware; - -@Config -public class ClawSubsystem implements Subsystem, Loggable { - - private Servo clawServo; - private EncodedMotor arm; - private boolean isHardware; - - @Log(name = "clawPosition") - public double clawPosition = 0; - - public static double CLAW_OPEN_POSITION = 0.3; - public static double CLAW_CLOSE_POSITION = 0.9; - public static double MIN_ARM_MOTOR_SPEED = -1; - public static double MAX_ARM_MOTOR_SPEED = 1; - public static int INCREMENT_DECREMENT = 5; - public static int INCREMENT_DECREMENT_BUTTON = 50; - public static double FEEDFORWARD_COEFFICIENT = 0.25; - public static int ARM_VERTICAL = 165; - public static int ARM_HORIZONTAL = 53; - public static int INITIAL_POSITION = 20; - public static int ARM_MAX = 327; - public static int ARM_MIN = 8; - - @Log(name = "armTarget") - public int armTargetPos; - - @Log(name = "armPos") - public int armPos; - - @Log(name = "armPow") - public double armPow; - - @Log(name = "armFdFwdVal") - public double armFeedFwdValue; - - private PIDFController armPidController; - - private void setArmPos(int e) { - armPidController.setTargetPosition(e); - armTargetPos = e; - } - - public static PIDCoefficients armPID = new PIDCoefficients(0.02, 0.0, 0); - - private void setClawPosition(double d) { - if (isHardware) { - clawServo.setPosition(d); - clawPosition = d; - } - } - - private int getArmUnmodifiedPosition() { - if (isHardware) { - return (int) arm.getSensorValue(); - } else { - return 0; - } - } - - public ClawSubsystem(Hardware hw) { - isHardware = true; - clawServo = hw.clawServo; - arm = hw.arm; - armPidController = new PIDFController( - armPID, - 0, - 0, - 0, - /* - - The function arguments for the Feed Forward function are Position (ticks) and - Velocity (units?). So, for the arm, we want to check to see if which side of - center we're on, and if the velocity is pushing us down, FF should probably be - low (negative?) while if velocity is pushing us *up*, FF should be high (right?) - Someone who's done physics and/or calculus recently should write some real equations - - Braelyn got the math right - - For angle T through this range where we start at zero: - / - / T - 180 _____/_____ 0 - The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2) - - If we're moving from 0 to 180 degrees, then: - While T is less than 90, the "downward torque" is working *against* the motor - When T is greater than 90, the "downward torque" is working *with* the motor - - */ - - (ticks, velocity) -> { - armFeedFwdValue = FEEDFORWARD_COEFFICIENT * Math.cos(getArmAngle(ticks)); - - // if (velocity > MIN_ANGULAR_VELOCITY) { - // //increase armFeedFwdValue to avoid slamming or increase D in PID - // armFeedFwdValue += ARM_SLAM_PREVENTION; - // } - - return armFeedFwdValue; - } - ); - setArmPos(ARM_HORIZONTAL); - } - - public ClawSubsystem() { - isHardware = false; - clawServo = null; - arm = null; - } - - public void increment(double value) { - int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT); - if (newArmPos > ARM_MAX) { - newArmPos = ARM_MAX; - } else if (newArmPos < ARM_MIN) { - newArmPos = ARM_MIN; - } - setArmPos(newArmPos); - } - - public void increment_button(double value) { - int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT_BUTTON); - if (newArmPos > ARM_MAX) { - newArmPos = ARM_MAX; - } else if (newArmPos < ARM_MIN) { - newArmPos = ARM_MIN; - } - setArmPos(newArmPos); - } - - public void incrementn() { - increment_button(1); - } - - public void decrement() { - increment_button(-1); - } - - public void powIncrement() { - setArmPos(armTargetPos + 1); - } - - public void powDecrement() { - setArmPos(armTargetPos - 1); - } - - private static double getArmAngle(double ticks) { - // our horizontal value starts at ARM_HORIZONTAL, so we need to - // subtract it - return ((Math.PI / 2.0) * (ticks - ARM_HORIZONTAL)) / (ARM_VERTICAL - ARM_HORIZONTAL); - } - - public void openClaw() { - setClawPosition(CLAW_OPEN_POSITION); - } - - public void closeClaw() { - setClawPosition(CLAW_CLOSE_POSITION); - } - - public void setArmVertical() { - setArmPos(ARM_VERTICAL); - } - - public void setArmHorizontal() { - setArmPos(ARM_HORIZONTAL); - } - - public void setArmIntake() { - setArmPos(ARM_MAX); - } - - private void setArmMotorPower(double speed) { - if (isHardware) { - double clippedSpeed = Range.clip(speed, MIN_ARM_MOTOR_SPEED, MAX_ARM_MOTOR_SPEED); - arm.setPower(clippedSpeed); - } - armPow = speed; - } - - @Override - public void periodic() { - armPos = getArmUnmodifiedPosition(); - armPow = armPidController.update(armPos); - setArmMotorPower(armPow); - } -} diff --git a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java b/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java deleted file mode 100644 index 5ec27188..00000000 --- a/Osprey/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java +++ /dev/null @@ -1,234 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.subsystems; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.control.PIDCoefficients; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.sensor.IGyro; -import com.technototes.library.logger.Log; -import com.technototes.library.logger.Loggable; -import com.technototes.path.subsystem.MecanumConstants; -import com.technototes.path.subsystem.PathingMecanumDrivebaseSubsystem; -import java.util.function.Supplier; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.helpers.HeadingHelper; - -public class DrivebaseSubsystem - extends PathingMecanumDrivebaseSubsystem - implements Supplier, Loggable { - - @Override - public Pose2d get() { - return getPoseEstimate(); - } - - @Config - public abstract static class DriveConstants implements MecanumConstants { - - public static double SLOW_MOTOR_SPEED = 0.6; - public static double FAST_MOTOR_SPEED = 1.0; - public static double AUTO_MOTOR_SPEED = 0.9; - - @TicksPerRev - public static final double TICKS_PER_REV = 537.7; // From GoBilda's website - - @MaxRPM - public static final double MAX_RPM = 312; - - public static double MAX_TICKS_PER_SEC = (TICKS_PER_REV * MAX_RPM) / 60.0; - - @UseDriveEncoder - public static final boolean RUN_USING_ENCODER = true; - - @MotorVeloPID - public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( - 20, - 0, - 3, - MecanumConstants.getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) - ); - - @WheelRadius - public static double WHEEL_RADIUS = 1.88976; // inches "roughly" lol - - @GearRatio - public static double GEAR_RATIO = 1.0; // output (wheel) speed / input (motor) speed - - @TrackWidth - public static double TRACK_WIDTH = 9.25; // inches - - @WheelBase - public static double WHEEL_BASE = 9.25; // inches - - @KV - public static double kV = - 1.0 / MecanumConstants.rpmToVelocity(MAX_RPM, WHEEL_RADIUS, GEAR_RATIO); - - @KA - public static double kA = 0; - - @KStatic - public static double kStatic = 0; - - // This was 60, which was too fast. Things slid around a lot. - @MaxVelo - public static double MAX_VEL = 50; - - // This was 35, which also felt a bit too fast. The bot controls more smoothly now - @MaxAccel - public static double MAX_ACCEL = 30; - - // This was 180 degrees - @MaxAngleVelo - public static double MAX_ANG_VEL = Math.toRadians(180); - - // This was 90 degrees - @MaxAngleAccel - public static double MAX_ANG_ACCEL = Math.toRadians(90); - - @TransPID - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); - - @HeadPID - public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); - - @LateralMult - public static double LATERAL_MULTIPLIER = 1.0; // For Mecanum, this was by 1.14 (14% off) - - @VXWeight - public static double VX_WEIGHT = 1; - - @VYWeight - public static double VY_WEIGHT = 1; - - @OmegaWeight - public static double OMEGA_WEIGHT = 1; - - @PoseLimit - public static int POSE_HISTORY_LIMIT = 100; - - // Helps deal with tired motors - public static double AFR_SCALE = 0.9; - public static double AFL_SCALE = 0.9; - public static double ARR_SCALE = 0.9; - public static double ARL_SCALE = 0.9; - } - - private static final boolean ENABLE_POSE_DIAGNOSTICS = true; - - @Log(name = "Pose2d: ") - public String poseDisplay = ENABLE_POSE_DIAGNOSTICS ? "" : null; - - // @Log.Number(name = "FL") - public EncodedMotor fl2; - - // @Log.Number(name = "FR") - public EncodedMotor fr2; - - // @Log.Number(name = "RL") - public EncodedMotor rl2; - - // @Log.Number(name = "RR") - public EncodedMotor rr2; - - // @Log(name = "Turbo") - public boolean Turbo = false; - - // @Log(name = "Snail") - public boolean Snail = false; - - @Log.Number(name = "cur heading") - public double curHeading; - - public DrivebaseSubsystem( - EncodedMotor flMotor, - EncodedMotor frMotor, - EncodedMotor rlMotor, - EncodedMotor rrMotor, - IGyro imu - ) { - super(flMotor, frMotor, rlMotor, rrMotor, imu, () -> DriveConstants.class); - fl2 = flMotor; - fr2 = frMotor; - rl2 = rlMotor; - rr2 = rrMotor; - curHeading = imu.getHeading(); - Setup.HardwareNames.COLOR = imu.getClass().toString(); - } - - @Override - public void periodic() { - if (ENABLE_POSE_DIAGNOSTICS) { - updatePoseEstimate(); - Pose2d pose = getPoseEstimate(); - Pose2d poseVelocity = getPoseVelocity(); - poseDisplay = pose.toString() + - " : " + - (poseVelocity != null ? poseVelocity.toString() : "nullv"); - curHeading = this.gyro.getHeading(); - Setup.HardwareNames.FLYWHEELMOTOR = this.gyro.getClass().toString(); - } - } - - public void fast() { - speed = DriveConstants.FAST_MOTOR_SPEED; - } - - public void slow() { - speed = DriveConstants.SLOW_MOTOR_SPEED; - } - - public void auto() { - speed = DriveConstants.AUTO_MOTOR_SPEED; - } - - public void setSnailMode() { - Snail = true; - Turbo = false; - } - - public void saveHeading() { - HeadingHelper.saveHeading(get().getX(), get().getY(), gyro.getHeading()); - } - - public void setTurboMode() { - Turbo = true; - Snail = false; - } - - public void setNormalMode() { - Snail = false; - Turbo = false; - } - - @Override - public void setMotorPowers(double lfv, double lrv, double rrv, double rfv) { - // TODO: Use the stick position to determine how to scale these values - // in Turbo mode (If the robot is driving in a straight line, the values are - // going to max out at sqrt(2)/2, rather than: We can go faster, but we don't - // *always* want to scale faster, only when we're it turbo mode, and when one (or more) - // of the control sticks are at their limit - double maxlv = Math.max(Math.abs(lfv), Math.abs(lrv)); - double maxrv = Math.max(Math.abs(rfv), Math.abs(rrv)); - double maxall = Math.max(maxlv, maxrv); - if (Snail == true) { - maxall = 1.0 / DriveConstants.SLOW_MOTOR_SPEED; - } else if (Turbo == false) { - maxall = 1.0 / DriveConstants.AUTO_MOTOR_SPEED; - } - leftFront.setVelocity( - (lfv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.AFL_SCALE) / maxall - ); - leftRear.setVelocity( - (lrv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.ARL_SCALE) / maxall - ); - rightRear.setVelocity( - (rrv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.ARR_SCALE) / maxall - ); - rightFront.setVelocity( - (rfv * DriveConstants.MAX_TICKS_PER_SEC * DriveConstants.AFR_SCALE) / maxall - ); - } -} diff --git a/Osprey/src/main/res/raw/readme.md b/Osprey/src/main/res/raw/readme.md deleted file mode 100644 index cf732dc9..00000000 --- a/Osprey/src/main/res/raw/readme.md +++ /dev/null @@ -1 +0,0 @@ -Place your sound files in this folder to use them as project resources. diff --git a/Osprey/src/main/res/values/strings.xml b/Osprey/src/main/res/values/strings.xml deleted file mode 100644 index d781ec5f..00000000 --- a/Osprey/src/main/res/values/strings.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/Osprey/src/main/res/xml/teamwebcamcalibrations.xml b/Osprey/src/main/res/xml/teamwebcamcalibrations.xml deleted file mode 100644 index 22ae7a86..00000000 --- a/Osprey/src/main/res/xml/teamwebcamcalibrations.xml +++ /dev/null @@ -1,161 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java new file mode 100644 index 00000000..95e91267 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Controls.java @@ -0,0 +1,291 @@ +package org.firstinspires.ftc.ptechnodactyl; + +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.ARM_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.BRAKE_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.CAP_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.CAROUSEL_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.DRIVE_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.EXTENSION_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.INTAKE_ENABLED; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.LIFT_ENABLED; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.library.control.CommandAxis; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.CommandInput; +import com.technototes.library.control.Stick; +import com.technototes.library.util.Alliance; +import com.technototes.path.command.ResetGyroCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmInCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmSharedCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpVariableCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapArmTranslateCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapCloseCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapDownCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapOpenCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapOutCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapStoreCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.cap.CapTurretTranslateCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.carousel.CarouselLeftCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.carousel.CarouselRightCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionSideCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.TurretTranslateCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeSafeCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel1Command; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevelCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftSharedCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftTranslateCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class Controls { + + public CommandGamepad driverGamepad, codriverGamepad; + + public Robot robot; + + public CommandAxis dumpAxis; + public CommandInput toIntakeButton; + public CommandButton sharedHubButton, allianceHubButton; + + public CommandButton liftAdjustUpButton, liftAdjustDownButton, turretAdjustRightButton, turretAdjustLeftButton; + + public CommandButton intakeInButton, intakeOutButton; + + public CommandButton carouselButton, carouselBackButton; + + public CommandButton capUpButton, capDownButton; + + public Stick driveLeftStick, driveRightStick; + public CommandButton resetGyroButton, brakeButton; + + public CommandButton strategy1Button, strategy2Button; + + public Controls(Robot r, CommandGamepad driver, CommandGamepad codriver) { + driverGamepad = driver; + codriverGamepad = codriver; + robot = r; + + dumpAxis = driverGamepad.leftTrigger.setTriggerThreshold(0.2); + sharedHubButton = driverGamepad.leftBumper; + allianceHubButton = driverGamepad.rightBumper; + toIntakeButton = driverGamepad.rightTrigger.setTriggerThreshold(0.3); + + liftAdjustUpButton = driverGamepad.dpadUp; + liftAdjustDownButton = driverGamepad.dpadDown; + turretAdjustRightButton = driverGamepad.dpadRight; + turretAdjustLeftButton = driverGamepad.dpadLeft; + + intakeInButton = driverGamepad.ps_cross; + intakeOutButton = driverGamepad.ps_circle; + + carouselButton = driverGamepad.ps_square; + carouselBackButton = driverGamepad.ps_triangle; + + resetGyroButton = driverGamepad.rightStickButton; + brakeButton = driverGamepad.leftStickButton; + + driveLeftStick = driverGamepad.leftStick; + driveRightStick = driverGamepad.rightStick; + + capUpButton = driverGamepad.ps_options; + + strategy1Button = driverGamepad.ps_options; + strategy2Button = driverGamepad.ps_share; + + RobotState.setStrategy( + RobotState.AllianceHubStrategy.HIGH, + RobotState.SharedHubStrategy.OWN + ); + + bindControls(); + } + + public void bindControls() { + if (LIFT_ENABLED) bindLiftControls(); + if (ARM_ENABLED) bindArmControls(); + if (DRIVE_ENABLED) bindDriveControls(); + if (INTAKE_ENABLED) bindIntakeControls(); + if (CAROUSEL_ENABLED) bindCarouselControls(); + if (CAP_ENABLED) bindCapControls(); + if (EXTENSION_ENABLED) bindExtensionControls(); + if (BRAKE_ENABLED) bindBrakeControls(); + + strategy1Button.whenPressed(() -> RobotState.strategy1()); + codriverGamepad.ps_options.whenPressed(() -> RobotState.strategy1()); + strategy2Button.whenPressed(() -> RobotState.strategy2()); + codriverGamepad.ps_share.whenPressed(() -> RobotState.strategy2()); + } + + public void bindBrakeControls() { + brakeButton.whenPressed(robot.brakeSubsystem::lower); + CommandScheduler.scheduleJoystick( + robot.brakeSubsystem::raise, + () -> driveLeftStick.getDistanceFromCenter() > 0.1 && robot.brakeSubsystem.get() + ); + } + + public void bindArmControls() { + dumpAxis.whilePressedOnce( + new BucketDumpVariableCommand(robot.armSubsystem, dumpAxis).asConditional( + EXTENSION_ENABLED ? robot.extensionSubsystem::isSlideOut : () -> true + ) + ); + toIntakeButton.whenPressed(new ArmInCommand(robot.armSubsystem)); + allianceHubButton.whileReleasedOnce(new ArmAllianceCommand(robot.armSubsystem)); + sharedHubButton.whileReleasedOnce(new ArmSharedCommand(robot.armSubsystem)); + intakeInButton.whenPressed(new ArmInCommand(robot.armSubsystem)); + } + + public void bindLiftControls() { + sharedHubButton.whileReleasedOnce( + new LiftSharedCommand(robot.liftSubsystem).withTimeout(0.5) + ); + allianceHubButton.whileReleasedOnce( + new LiftLevelCommand(robot.liftSubsystem).withTimeout(0.5) + ); + toIntakeButton.whenPressed( + new LiftLevel1Command(robot.liftSubsystem) + .withTimeout(0.5) + .alongWith(new WaitCommand(0.5)) + .andThen(new LiftCollectCommand(robot.liftSubsystem).withTimeout(0.4)) + ); + liftAdjustUpButton.whilePressed(new LiftTranslateCommand(robot.liftSubsystem, 50)); + liftAdjustDownButton.whilePressed(new LiftTranslateCommand(robot.liftSubsystem, -50)); + } + + public void bindDriveControls() { + if (EXTENSION_ENABLED && ARM_ENABLED && LIFT_ENABLED && INTAKE_ENABLED) { + // allianceHubButton.whilePressed( new TeleopDepositAllianceCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem).andThen(new TeleopIntakeAllianceWarehouseCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem))); + // sharedHubButton.whilePressed(new TeleopDepositSharedCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem).andThen(new TeleopIntakeSharedWarehouseCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem))); + } + robot.drivebaseSubsystem.setDefaultCommand( + new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) + ); + codriverGamepad.leftTrigger.whilePressedOnce( + new JoystickDriveCommand( + robot.drivebaseSubsystem, + codriverGamepad.leftStick, + codriverGamepad.rightStick + ) + ); // allianceHubButton.whenPressed(new DriveSpeedCommand(robot.drivebaseSubsystem).cancelUpon(toIntakeButton)); + resetGyroButton.whenPressed(new ResetGyroCommand(robot.drivebaseSubsystem)); + } + + public void bindIntakeControls() { + if (ARM_ENABLED) { + toIntakeButton.whenPressed( + new SequentialCommandGroup( + new WaitCommand(0.8), + new IntakeSafeCommand(robot.intakeSubsystem), + driverGamepad::rumbleBlip, + robot.armSubsystem::slightCarry, + new WaitCommand(0.2), + new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.5) + ).ignoreCancel() + ); + intakeInButton.whilePressedContinuous( + new SequentialCommandGroup( + new IntakeSafeCommand(robot.intakeSubsystem), + driverGamepad::rumbleBlip, + robot.armSubsystem::slightCarry, + new WaitCommand(0.2), + new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.5) + ).ignoreCancel() + ); + } else { + toIntakeButton.whenPressed( + new WaitCommand(0.8).andThen(new IntakeSafeCommand(robot.intakeSubsystem)) + ); + intakeInButton.whilePressedContinuous(new IntakeInCommand(robot.intakeSubsystem)); + } + intakeOutButton.whilePressedOnce(new IntakeOutCommand(robot.intakeSubsystem)); + allianceHubButton.whenReleased( + new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2) + ); + sharedHubButton.whenReleased(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2)); + } + + public void bindCarouselControls() { + carouselButton.whilePressedOnce( + RobotConstants.getAlliance() + .selectOf( + new CarouselLeftCommand(robot.carouselSubsystem), + new CarouselRightCommand(robot.carouselSubsystem) + ) + ); + carouselBackButton.whilePressedOnce( + RobotConstants.getAlliance() + .selectOf( + new CarouselRightCommand(robot.carouselSubsystem), + new CarouselLeftCommand(robot.carouselSubsystem) + ) + ); + } + + public void bindCapControls() { + codriverGamepad.ps_circle.whenPressed(new CapOpenCommand(robot.capSubsystem)); + codriverGamepad.ps_cross.whenPressed(new CapCloseCommand(robot.capSubsystem)); + codriverGamepad.rightBumper.whenPressed(new CapOutCommand(robot.capSubsystem)); + codriverGamepad.leftBumper.whenPressed(new CapStoreCommand(robot.capSubsystem)); + codriverGamepad.rightTrigger.whenPressed(new CapDownCommand(robot.capSubsystem)); + codriverGamepad.dpadLeft.whilePressed( + new CapTurretTranslateCommand(robot.capSubsystem, -0.02) + ); + codriverGamepad.dpadRight.whilePressed( + new CapTurretTranslateCommand(robot.capSubsystem, 0.02) + ); + codriverGamepad.dpadUp.whilePressed(new CapArmTranslateCommand(robot.capSubsystem, 0.05)); + codriverGamepad.dpadDown.whilePressed( + new CapArmTranslateCommand(robot.capSubsystem, -0.05) + ); + } + + public void bindExtensionControls() { + allianceHubButton.whileReleasedOnce( + new ExtensionCommand( + robot.extensionSubsystem, + ExtensionSubsystem.ExtensionConstants.TELEOP_ALLIANCE, + ExtensionSubsystem.ExtensionConstants.CENTER + ) + ); + sharedHubButton.whileReleasedOnce(new ExtensionSideCommand(robot.extensionSubsystem)); + toIntakeButton.whenPressed(new ExtensionCollectCommand(robot.extensionSubsystem)); + turretAdjustLeftButton.whilePressed( + new TurretTranslateCommand( + robot.extensionSubsystem, + 0.05, + () -> + DRIVE_ENABLED && + ((RobotConstants.getAlliance() == Alliance.RED) ^ + (robot.drivebaseSubsystem.getExternalHeading() > + ExtensionSubsystem.ExtensionConstants.SNAP_1 && + robot.drivebaseSubsystem.getExternalHeading() < + ExtensionSubsystem.ExtensionConstants.SNAP_2)) + ) + ); + turretAdjustRightButton.whilePressed( + new TurretTranslateCommand( + robot.extensionSubsystem, + -0.05, + () -> + DRIVE_ENABLED && + ((RobotConstants.getAlliance() == Alliance.RED) ^ + (robot.drivebaseSubsystem.getExternalHeading() > + ExtensionSubsystem.ExtensionConstants.SNAP_1 && + robot.drivebaseSubsystem.getExternalHeading() < + ExtensionSubsystem.ExtensionConstants.SNAP_2)) + ) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java index 930868d6..027e0a89 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java @@ -1,91 +1,143 @@ package org.firstinspires.ftc.ptechnodactyl; -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import static org.firstinspires.ftc.ptechnodactyl.Hardware.HardwareConstants.*; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.*; + import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.hardware.motor.Motor; -import com.technototes.library.hardware.sensor.AdafruitIMU; import com.technototes.library.hardware.sensor.ColorDistanceSensor; -import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; +import com.technototes.library.hardware.sensor.IMU.AxesSigns; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Loggable; import com.technototes.vision.hardware.Webcam; -import java.util.List; -import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.BrakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class Hardware implements Loggable { - public List hubs; + @com.acmerobotics.dashboard.config.Config + public static class HardwareConstants { + + public static String LIFT = "lift"; + + public static String DUMP = "dump"; + public static String ARM = "arm"; + + public static String SLIDE = "slide"; + public static String TURRET = "turret"; + + public static String FL_MOTOR = "flmotor"; + public static String FR_MOTOR = "frmotor"; + public static String RL_MOTOR = "rlmotor"; + public static String RR_MOTOR = "rrmotor"; + public static String IMU = "imu"; + public static String L_RANGE = "ldistance"; + public static String R_RANGE = "rdistance"; + public static String F_RANGE = "fdistance"; + + public static String CAROUSEL = "carousel"; + + public static String CAMERA = "webcam"; + + public static String INTAKE = "intake"; + public static String INTAKE_COLOR = "irange"; + + public static String CAP = "cap"; + + public static String BRAKE = "brake"; + } + + public EncodedMotor liftMotor; + + public ServoGroup dumpServo; + public ServoGroup armServo; - public EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor, arm; - public Motor placeholder1; - public DcMotorEx liftMotor; + public ServoGroup turretServo; + public ServoGroup slideServo; - public Servo placeholder2; - public Servo servo, clawServo; + public EncodedMotor flDriveMotor; + public EncodedMotor frDriveMotor; + public EncodedMotor rlDriveMotor; + public EncodedMotor rrDriveMotor; + public static IMU imu; + public Rev2MDistanceSensor leftRangeSensor; + public Rev2MDistanceSensor rightRangeSensor; + public Rev2MDistanceSensor frontRangeSensor; + + public Motor intakeMotor; + public ColorDistanceSensor intakeSensor; + + public Motor carouselMotor; + + public ServoGroup capServo; - public IGyro imu; public Webcam camera; - public Rev2MDistanceSensor distanceSensor; - public ColorDistanceSensor colorSensor; - - public Hardware(HardwareMap hwmap) { - hubs = hwmap.getAll(LynxModule.class); - if (Setup.Connected.EXTERNAL_IMU) { - imu = new AdafruitIMU(Setup.HardwareNames.EXTERNAL_IMU, AdafruitIMU.Orientation.Pitch); - Setup.HardwareNames.MOTOR = "External IMU is in use"; - } else { - imu = new IMU( - Setup.HardwareNames.IMU, - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD - ); - Setup.HardwareNames.MOTOR = "Internal IMU is being used"; + + public ServoGroup brake; + + public ServoGroup capLeftArmServo; + public ServoGroup capRightArmServo; + public ServoGroup capArmServos; + + public ServoGroup capClawServo; + public ServoGroup capTurretServo; + + public Hardware() { + if (BRAKE_ENABLED) { + brake = new ServoGroup(BRAKE).startAt(BrakeSubsystem.BrakeConstants.UP); } - if (Setup.Connected.DRIVEBASE) { - this.frMotor = new EncodedMotor<>(Setup.HardwareNames.FRMOTOR); - this.flMotor = new EncodedMotor<>(Setup.HardwareNames.FLMOTOR); - this.rrMotor = new EncodedMotor<>(Setup.HardwareNames.RRMOTOR); - this.rlMotor = new EncodedMotor<>(Setup.HardwareNames.RLMOTOR); - if (Setup.Connected.DISTANCE_SENSOR) { - this.distanceSensor = new Rev2MDistanceSensor(Setup.HardwareNames.DISTANCE); - } + if (LIFT_ENABLED) { + liftMotor = new EncodedMotor(LIFT).brake().tare(); } - if (Setup.Connected.MOTOR) { - this.theMotor = new EncodedMotor<>(Setup.HardwareNames.MOTOR); + if (ARM_ENABLED) { + dumpServo = new ServoGroup(DUMP).invert().startAt(ArmSubsystem.ArmConstants.CARRY); + armServo = new ServoGroup(ARM).startAt(ArmSubsystem.ArmConstants.UP); } - if (Setup.Connected.FLYWHEEL) { - this.placeholder1 = new Motor<>(Setup.HardwareNames.FLYWHEELMOTOR); + if (EXTENSION_ENABLED) { + slideServo = new ServoGroup(SLIDE).startAt(ExtensionSubsystem.ExtensionConstants.IN); + turretServo = new ServoGroup(TURRET) + .startAt(ExtensionSubsystem.ExtensionConstants.CENTER) + .expandedRange(); } - if (Setup.Connected.WEBCAM) { - camera = new Webcam(Setup.HardwareNames.CAMERA); + if (DRIVE_ENABLED) { + flDriveMotor = new EncodedMotor<>(FL_MOTOR); + frDriveMotor = new EncodedMotor<>(FR_MOTOR); + rlDriveMotor = new EncodedMotor<>(RL_MOTOR); + rrDriveMotor = new EncodedMotor<>(RR_MOTOR); + imu = new IMU(HardwareConstants.IMU).remapAxes(AxesOrder.YXZ, AxesSigns.NPP); + leftRangeSensor = new Rev2MDistanceSensor(L_RANGE).onUnit(DistanceUnit.INCH); + rightRangeSensor = new Rev2MDistanceSensor(R_RANGE).onUnit(DistanceUnit.INCH); + frontRangeSensor = new Rev2MDistanceSensor(F_RANGE).onUnit(DistanceUnit.INCH); } - if (Setup.Connected.TESTSUBSYSTEM) { - if (Setup.Connected.SERVO) { - this.servo = new Servo(Setup.HardwareNames.SERVO); - } - // if (Setup.Connected.COLOR_SENSOR) { - // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); - // } + if (CAROUSEL_ENABLED) { + carouselMotor = new Motor(CAROUSEL).brake(); } - if (Setup.Connected.CLAWSUBSYSTEM) { - this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO); - this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM); + if (VISION_ENABLED) { + camera = new Webcam(CAMERA); } - } + if (INTAKE_ENABLED) { + intakeMotor = new Motor<>(INTAKE); + intakeSensor = new ColorDistanceSensor(INTAKE_COLOR).onUnit(DistanceUnit.INCH); + } + if (CAP_ENABLED) { + capLeftArmServo = new ServoGroup("caparml").onRange(0.25, 0.65).invert(); + capRightArmServo = new ServoGroup("caparmr").onRange(0.35, 0.75); + capArmServos = new ServoGroup(capLeftArmServo, capRightArmServo).startAt( + CapSubsystem.CapConstants.ARM_INIT + ); - // We can read the voltage from the different hubs for fun... - public double voltage() { - double volt = 0; - double count = 0; - for (LynxModule lm : hubs) { - count += 1; - volt += lm.getInputVoltage(VoltageUnit.VOLTS); + capClawServo = new ServoGroup("capclaw").startAt(CapSubsystem.CapConstants.CLAW_CLOSE); + capTurretServo = new ServoGroup("capturr").startAt( + CapSubsystem.CapConstants.TURRET_INIT + ); } - return volt / count; } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java index 20189789..fb1fc8e0 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java @@ -1,30 +1,103 @@ package org.firstinspires.ftc.ptechnodactyl; -import com.technototes.library.logger.Loggable; -import com.technototes.library.util.Alliance; +import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.*; -import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; +import com.acmerobotics.dashboard.config.Config; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.LogConfig; +import com.technototes.library.logger.Loggable; +import com.technototes.library.util.Color; +import org.firstinspires.ftc.ptechnodactyl.opmodes.tele.TeleOpBase; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.BrakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; public class Robot implements Loggable { - public StartingPosition position; - public Alliance alliance; - public double initialVoltage; + @Config + public static class SubsystemConstants { + + public static boolean LIFT_ENABLED = true; + public static boolean ARM_ENABLED = true; + public static boolean EXTENSION_ENABLED = true; + public static boolean DRIVE_ENABLED = true; + public static boolean CAROUSEL_ENABLED = true; + public static boolean INTAKE_ENABLED = true; + public static boolean VISION_ENABLED = true; + public static boolean CAP_ENABLED = true; + public static boolean SPEAKER_ENABLED = true; + public static boolean BRAKE_ENABLED = true; + } + + @Log.Number(name = "Lift") + public LiftSubsystem liftSubsystem; + + @Log(name = "Deposit") + public ArmSubsystem armSubsystem; + + @Log(name = "Extension") + public ExtensionSubsystem extensionSubsystem; + + @LogConfig.DenyList(TeleOpBase.class) + @Log(name = "Drivebase") public DrivebaseSubsystem drivebaseSubsystem; + @Log.Number(name = "Carousel") + public CarouselSubsystem carouselSubsystem; + + @Log(name = "Intake") + public IntakeSubsystem intakeSubsystem; + + @Log(name = "Cap") + public CapSubsystem capSubsystem; + + public VisionSubsystem visionSubsystem; + + @Log.Boolean(name = "Brake", trueValue = "ACTIVE", falseValue = "INACTIVE", index = 0) + public BrakeSubsystem brakeSubsystem; + + public Robot(Hardware hardware) { + if (BRAKE_ENABLED) brakeSubsystem = new BrakeSubsystem(hardware.brake); + + if (LIFT_ENABLED) liftSubsystem = new LiftSubsystem(hardware.liftMotor); + + if (ARM_ENABLED) armSubsystem = new ArmSubsystem(hardware.dumpServo, hardware.armServo); + + if (EXTENSION_ENABLED) extensionSubsystem = new ExtensionSubsystem( + hardware.slideServo, + hardware.turretServo + ); + + if (DRIVE_ENABLED) drivebaseSubsystem = new DrivebaseSubsystem( + hardware.flDriveMotor, + hardware.frDriveMotor, + hardware.rlDriveMotor, + hardware.rrDriveMotor, + hardware.imu, + hardware.leftRangeSensor, + hardware.rightRangeSensor, + hardware.frontRangeSensor + ); + + if (CAROUSEL_ENABLED) carouselSubsystem = new CarouselSubsystem(hardware.carouselMotor); + + if (INTAKE_ENABLED) intakeSubsystem = new IntakeSubsystem( + hardware.intakeMotor, + hardware.intakeSensor + ); - public Robot(Hardware hw) { - this.initialVoltage = hw.voltage(); - if (Setup.Connected.DRIVEBASE) { - this.drivebaseSubsystem = new DrivebaseSubsystem( - hw.flMotor, - hw.frMotor, - hw.rlMotor, - hw.rrMotor, - hw.imu - ); - } + if (VISION_ENABLED) visionSubsystem = new VisionSubsystem(hardware.camera); + if (CAP_ENABLED) capSubsystem = new CapSubsystem( + hardware.capArmServos, + hardware.capClawServo, + hardware.capTurretServo + ); } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java new file mode 100644 index 00000000..7acfe50c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotConstants.java @@ -0,0 +1,271 @@ +package org.firstinspires.ftc.ptechnodactyl; + +import static java.lang.Math.toRadians; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.technototes.library.util.Alliance; +import com.technototes.path.geometry.ConfigurablePose; +import com.technototes.path.trajectorysequence.TrajectorySequence; +import com.technototes.path.trajectorysequence.TrajectorySequenceBuilder; +import java.util.function.BiFunction; +import java.util.function.Function; +import java.util.function.Supplier; + +public class RobotConstants { + + @Config + public static class AutoRedConstants { + + public static ConfigurablePose CYCLE_START = new ConfigurablePose(12, -63, toRadians(90)); + public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(7, -52, toRadians(125)); + public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(24, -64, toRadians(180)); + public static ConfigurablePose CYCLE_INTERMEDIATE = new ConfigurablePose( + 42, + -64, + toRadians(180) + ); + public static ConfigurablePose[] AUTO_WAREHOUSE = new ConfigurablePose[] { + new ConfigurablePose(44, -64, toRadians(190)), + new ConfigurablePose(46.5, -64, toRadians(190)), + new ConfigurablePose(49, -64, toRadians(190)), + new ConfigurablePose(51.5, -64, toRadians(190)), + new ConfigurablePose(54, -64, toRadians(190)), + }; + + public static ConfigurablePose DUCK_START = new ConfigurablePose(-36, -63, toRadians(90)); + public static ConfigurablePose DUCK_HUB = new ConfigurablePose(-32, -52, toRadians(55)); + public static ConfigurablePose CAROUSEL = new ConfigurablePose(-61, -59, toRadians(0)); + public static ConfigurablePose DUCK_INTAKE_START = new ConfigurablePose( + -30, + -58, + toRadians(145) + ); + public static ConfigurablePose DUCK_INTAKE_END = new ConfigurablePose( + -60, + -61, + toRadians(35) + ); + public static ConfigurablePose SQUARE = new ConfigurablePose(-67, -36, toRadians(0)); + public static ConfigurablePose BARRIER_PARK = new ConfigurablePose(60, -30, toRadians(0)); + + public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(64, -23, toRadians(90)); + public static ConfigurablePose SHARED_HUB = new ConfigurablePose(64, -17, toRadians(100)); + public static ConfigurablePose SHARED_INTAKE = new ConfigurablePose(64, -50, toRadians(85)); + } + + @Config + public static class AutoBlueConstants { + + public static ConfigurablePose CYCLE_START = new ConfigurablePose(12, 63, toRadians(-90)); + public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(7, 52, toRadians(-125)); + public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(24, 64, toRadians(-180)); + public static ConfigurablePose CYCLE_INTERMEDIATE = new ConfigurablePose( + 42, + 64, + toRadians(-180) + ); + public static ConfigurablePose[] AUTO_WAREHOUSE = new ConfigurablePose[] { + new ConfigurablePose(44, 64, toRadians(-190)), + new ConfigurablePose(46.5, 64, toRadians(-190)), + new ConfigurablePose(49, 64, toRadians(-190)), + new ConfigurablePose(51.5, 64, toRadians(-190)), + new ConfigurablePose(54, 64, toRadians(-190)), + }; + + public static ConfigurablePose DUCK_START = new ConfigurablePose(-36, 63, toRadians(-90)); + public static ConfigurablePose DUCK_HUB = new ConfigurablePose(-32, 52, toRadians(-55)); + public static ConfigurablePose CAROUSEL = new ConfigurablePose(-61, 59, toRadians(-90)); + public static ConfigurablePose DUCK_INTAKE_START = new ConfigurablePose( + -30, + 58, + toRadians(-145) + ); + public static ConfigurablePose DUCK_INTAKE_END = new ConfigurablePose( + -60, + 61, + toRadians(-35) + ); + public static ConfigurablePose SQUARE = new ConfigurablePose(-67, 36, toRadians(0)); + public static ConfigurablePose BARRIER_PARK = new ConfigurablePose(60, 30, toRadians(0)); + + public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(64, 23, toRadians(-90)); + public static ConfigurablePose SHARED_HUB = new ConfigurablePose(64, 17, toRadians(-90)); + public static ConfigurablePose SHARED_INTAKE = new ConfigurablePose(64, 50, toRadians(-90)); + } + + private static Alliance alliance = Alliance.BLUE; + + public static void updateAlliance(Alliance i) { + alliance = i; + } + + public static Alliance getAlliance() { + return alliance; + } + + public static final Supplier CYCLE_START_SELECT = () -> + alliance + .selectOf(AutoRedConstants.CYCLE_START, AutoBlueConstants.CYCLE_START) + .toPose(), ALLIANCE_HUB_SELECT = () -> + alliance + .selectOf(AutoRedConstants.ALLIANCE_HUB, AutoBlueConstants.ALLIANCE_HUB) + .toPose(), SHARED_HUB_SELECT = () -> + alliance + .selectOf(AutoRedConstants.SHARED_HUB, AutoBlueConstants.SHARED_HUB) + .toPose(), SHARED_INTAKE_SELECT = () -> + alliance + .selectOf(AutoRedConstants.SHARED_INTAKE, AutoBlueConstants.SHARED_INTAKE) + .toPose(), ALLIANCE_TRENCH_SELECT = () -> + alliance + .selectOf(AutoRedConstants.CYCLE_TRENCH, AutoBlueConstants.CYCLE_TRENCH) + .toPose(), CYCLE_INTERMEDIATE_SELECT = () -> + alliance + .selectOf(AutoRedConstants.CYCLE_INTERMEDIATE, AutoBlueConstants.CYCLE_INTERMEDIATE) + .toPose(), SHARED_TRENCH_SELECT = () -> + alliance + .selectOf(AutoRedConstants.SHARED_TRENCH, AutoBlueConstants.SHARED_TRENCH) + .toPose(), DUCK_START_SELECT = () -> + alliance + .selectOf(AutoRedConstants.DUCK_START, AutoBlueConstants.DUCK_START) + .toPose(), DUCK_ALLIANCE_HUB_SELECT = () -> + alliance + .selectOf(AutoRedConstants.DUCK_HUB, AutoBlueConstants.DUCK_HUB) + .toPose(), CAROUSEL_SELECT = () -> + alliance + .selectOf(AutoRedConstants.CAROUSEL, AutoBlueConstants.CAROUSEL) + .toPose(), DUCK_INTAKE_START_SELECT = () -> + alliance + .selectOf(AutoRedConstants.DUCK_INTAKE_START, AutoBlueConstants.DUCK_INTAKE_START) + .toPose(), DUCK_INTAKE_END_SELECT = () -> + alliance + .selectOf(AutoRedConstants.DUCK_INTAKE_END, AutoBlueConstants.DUCK_INTAKE_END) + .toPose(), SQUARE_SELECT = () -> + alliance + .selectOf(AutoRedConstants.SQUARE, AutoBlueConstants.SQUARE) + .toPose(), BARRIER_SELECT = () -> + alliance.selectOf(AutoRedConstants.BARRIER_PARK, AutoBlueConstants.BARRIER_PARK).toPose(); + + public static final Function CYCLE_INTAKE_SELECT = i -> + alliance + .selectOf(AutoRedConstants.AUTO_WAREHOUSE[i], AutoBlueConstants.AUTO_WAREHOUSE[i]) + .toPose(); + + public static final Function< + Function, + TrajectorySequence + > CYCLE_DEPOSIT_PRELOAD = b -> + b + .apply(CYCLE_START_SELECT.get()) + .setAccelConstraint((a, e, c, d) -> 30) + .lineToLinearHeading(ALLIANCE_HUB_SELECT.get()) + .build(), DUCK_DEPOSIT_PRELOAD = b -> + b + .apply(DUCK_START_SELECT.get()) + .setAccelConstraint((a, e, c, d) -> 30) + .lineToLinearHeading(DUCK_ALLIANCE_HUB_SELECT.get()) + .build(), HUB_TO_CAROUSEL = b -> + b + .apply(DUCK_ALLIANCE_HUB_SELECT.get()) + .setAccelConstraint((a, e, c, d) -> 30) + .lineToLinearHeading(CAROUSEL_SELECT.get()) + .build(), HUB_TO_SQUARE = b -> + b + .apply(DUCK_ALLIANCE_HUB_SELECT.get()) + .setAccelConstraint((a, e, c, d) -> 30) + .lineToLinearHeading(SQUARE_SELECT.get()) + .build(), HUB_BARRIER_PARK = b -> + b + .apply(DUCK_ALLIANCE_HUB_SELECT.get()) + .turn(BARRIER_SELECT.get().getHeading() - DUCK_ALLIANCE_HUB_SELECT.get().getHeading()) + .setVelConstraint((a, e, c, d) -> 100) + .lineTo(BARRIER_SELECT.get().vec()) + .build(), HUB_TO_PARK = b -> + b + .apply(ALLIANCE_HUB_SELECT.get()) + .setReversed(true) + .setAccelConstraint((a, e, c, d) -> 30) + .splineTo(ALLIANCE_TRENCH_SELECT.get().vec(), 0) + // .setAccelConstraint((a, e, c, d) -> 100) + // .setVelConstraint((a, e, c, d)->70) + .lineToSplineHeading(CYCLE_INTERMEDIATE_SELECT.get()) + .build(), DUCK_INTAKE_TO_HUB = b -> + b + .apply(DUCK_INTAKE_END_SELECT.get()) + .setAccelConstraint((a, e, c, d) -> 30) + .lineToLinearHeading(DUCK_ALLIANCE_HUB_SELECT.get()) + .build(), SHARED_HUB_TO_WAREHOUSE = b -> + b + .apply(SHARED_HUB_SELECT.get()) + .setReversed(true) + .setAccelConstraint((a, e, c, d) -> 30) + .splineTo( + SHARED_TRENCH_SELECT.get().vec(), + toRadians(RobotConstants.getAlliance().selectOf(-90, 90)) + ) + // .setAccelConstraint((a, e, c, d) -> 100) + // .setVelConstraint((a, e, c, d)->70) + .lineToSplineHeading(SHARED_INTAKE_SELECT.get()) + .build(), CAROUSEL_TO_DUCK_INTAKE = b -> + b + .apply(CAROUSEL_SELECT.get()) + .setAccelConstraint((a, e, c, d) -> 20) + .setVelConstraint((a, e, c, d) -> 30) + .turn(DUCK_INTAKE_START_SELECT.get().getHeading() - CAROUSEL_SELECT.get().getHeading()) + .lineToLinearHeading(DUCK_INTAKE_START_SELECT.get()) + .turn( + DUCK_INTAKE_END_SELECT.get().getHeading() - + DUCK_INTAKE_START_SELECT.get().getHeading() + ) + .lineToLinearHeading(DUCK_INTAKE_END_SELECT.get()) + .build(); + + public static final BiFunction< + Function, + Integer, + TrajectorySequence + > HUB_TO_WAREHOUSE = (b, i) -> + b + .apply(ALLIANCE_HUB_SELECT.get()) + .setReversed(true) + .setAccelConstraint((a, e, c, d) -> 30) + .splineTo(ALLIANCE_TRENCH_SELECT.get().vec(), 0) + .setAccelConstraint((a, e, c, d) -> 60) + .lineToSplineHeading(CYCLE_INTERMEDIATE_SELECT.get()) + .setVelConstraint((a, e, c, d) -> 20) + .lineToSplineHeading(CYCLE_INTAKE_SELECT.apply(i)) + .build(); + + public static final BiFunction< + Function, + Supplier, + TrajectorySequence + > WAREHOUSE_TO_HUB = (b, p) -> + b + .apply( + new Pose2d( + Math.max(p.get().getX(), ALLIANCE_TRENCH_SELECT.get().getX() + 1), + ALLIANCE_TRENCH_SELECT.get().getY(), + // p.get().getY(), + p.get().getHeading() + ) + ) + // .setTurnConstraint(toRadians(30), toRadians(30)) + .lineToSplineHeading(ALLIANCE_TRENCH_SELECT.get()) + // .splineToSplineHeading(ALLIANCE_TRENCH_SELECT.get(), toRadians(180)) + .setAccelConstraint((a, e, c, d) -> 30) + .splineTo(ALLIANCE_HUB_SELECT.get().vec(), ALLIANCE_HUB_SELECT.get().getHeading()) + .build(), WAREHOUSE_TO_SHARED_HUB = (b, p) -> + b + .apply( + new Pose2d( + p.get().getX(), + // p.get().getY(), + SHARED_TRENCH_SELECT.get().getY(), + p.get().getHeading() + ) + ) + .lineToSplineHeading(SHARED_TRENCH_SELECT.get()) + .splineToSplineHeading(SHARED_HUB_SELECT.get(), SHARED_HUB_SELECT.get().getHeading()) + .build(); +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java new file mode 100644 index 00000000..97e1120f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/RobotState.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.ptechnodactyl; + +import com.acmerobotics.dashboard.config.Config; + +public class RobotState { + + private static AllianceHubStrategy allianceHubStrategy; + private static SharedHubStrategy sharedHubStrategy; + + public static AllianceHubStrategy getAllianceStrategy() { + return allianceHubStrategy; + } + + public static SharedHubStrategy getSharedStrategy() { + return sharedHubStrategy; + } + + public static void strategy1() { + setStrategy(AllianceHubStrategy.HIGH, SharedHubStrategy.OWN); + } + + public static void strategy2() { + setStrategy(AllianceHubStrategy.MID, SharedHubStrategy.STEAL); + } + + public static void setStrategy( + AllianceHubStrategy allianceHubStrategy, + SharedHubStrategy sharedHubStrategy + ) { + RobotState.allianceHubStrategy = allianceHubStrategy; + RobotState.sharedHubStrategy = sharedHubStrategy; + } + + public enum AllianceHubStrategy { + HIGH, + MID, + } + + public enum SharedHubStrategy { + OWN, + STEAL, + } + + private static boolean depositTarget = true; + + public static void startDeposit() { + depositTarget = true; + } + + public static void stopDeposit() { + depositTarget = false; + } + + public static boolean isDepositing() { + return depositTarget; + } + + public enum Freight { + CUBE, + BALL, + DUCK, + NONE, + } + + private static Freight freight = Freight.NONE; + + public static void setFreight(Freight f) { + freight = f; + } + + public static Freight getFreight() { + return freight; + } + + public static boolean hasFreight() { + return freight != Freight.NONE; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java new file mode 100644 index 00000000..ac50008e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmAllianceCommand.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; + +public class ArmAllianceCommand extends ArmCommand { + + public ArmAllianceCommand(ArmSubsystem s) { + super(s); + } + + @Override + public void execute() { + subsystem.out(); + subsystem.carry(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java new file mode 100644 index 00000000..96dcf7ae --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmBarcodeSelectCommand.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class ArmBarcodeSelectCommand extends ArmCommand { + + public VisionSubsystem visionSubsystem; + + public ArmBarcodeSelectCommand(ArmSubsystem s, VisionSubsystem v) { + super(s); + visionSubsystem = v; + } + + @Override + public void execute() { + subsystem.carry(); + if (visionSubsystem.barcodePipeline.zero()) subsystem.down(); + else subsystem.out(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java new file mode 100644 index 00000000..9741f8f1 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public abstract class ArmCommand implements Command { + + public ArmSubsystem subsystem; + + public ArmCommand(ArmSubsystem arm) { + subsystem = arm; + addRequirements(arm); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.5; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java new file mode 100644 index 00000000..31e630d0 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmInCommand.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class ArmInCommand extends ArmCommand { + + public ArmInCommand(ArmSubsystem s) { + super(s); + } + + @Override + public void execute() { + subsystem.in(); + subsystem.collect(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.4; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java new file mode 100644 index 00000000..195c8949 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseCommand.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class ArmRaiseCommand extends ArmCommand { + + public ArmRaiseCommand(ArmSubsystem s) { + super(s); + } + + @Override + public void execute() { + subsystem.carry(); + subsystem.up(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java new file mode 100644 index 00000000..4df1dace --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmRaiseInCommand.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class ArmRaiseInCommand extends ArmCommand { + + public ArmRaiseInCommand(ArmSubsystem s) { + super(s); + } + + @Override + public void execute() { + if (getRuntime().seconds() > 0.2) subsystem.fakeCarry(); + subsystem.up(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.3; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java new file mode 100644 index 00000000..c6f19213 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmSharedCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class ArmSharedCommand extends ArmCommand { + + public ArmSharedCommand(ArmSubsystem s) { + super(s); + } + + @Override + public void execute() { + subsystem.down(); + subsystem.fakeCarry(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.5; + } + + @Override + public void end(boolean cancel) { + if (!cancel) subsystem.carry(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java new file mode 100644 index 00000000..6b435d2f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/ArmTranslateCommand.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class ArmTranslateCommand implements Command { + + public ArmSubsystem subsystem; + public double amount; + + public ArmTranslateCommand(ArmSubsystem s, double amt) { + subsystem = s; + amount = amt; + addRequirements(s); + } + + @Override + public void initialize() { + subsystem.translateArm(amount); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.2; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java new file mode 100644 index 00000000..c00d95f5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCarryCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class BucketCarryCommand implements Command { + + public ArmSubsystem depositSubsystem; + + public BucketCarryCommand(ArmSubsystem s) { + depositSubsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + depositSubsystem.setDump(ArmSubsystem.ArmConstants.CARRY); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java new file mode 100644 index 00000000..8c5b1c55 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketCollectCommand.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class BucketCollectCommand implements Command { + + public ArmSubsystem depositSubsystem; + + public BucketCollectCommand(ArmSubsystem s) { + depositSubsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + depositSubsystem.setDump(ArmSubsystem.ArmConstants.COLLECT); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.5; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java new file mode 100644 index 00000000..69e7714a --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpCommand.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class BucketDumpCommand implements Command { + + public ArmSubsystem depositSubsystem; + + public BucketDumpCommand(ArmSubsystem s) { + depositSubsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + depositSubsystem.dump(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.3; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java new file mode 100644 index 00000000..2a52a9e8 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/arm/BucketDumpVariableCommand.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.arm; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.*; + +import com.technototes.library.command.Command; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.ptechnodactyl.subsystems.*; + +public class BucketDumpVariableCommand implements Command { + + public ArmSubsystem subsystem; + public DoubleSupplier supplier; + + public BucketDumpVariableCommand(ArmSubsystem s, DoubleSupplier pos) { + subsystem = s; + supplier = pos; + addRequirements(s); + } + + public BucketDumpVariableCommand(ArmSubsystem s, double pos) { + this(s, () -> pos); + } + + @Override + public void execute() { + subsystem.setDump(((supplier.getAsDouble() - 0.2) * (DUMP - AUTO_CARRY)) + AUTO_CARRY); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + subsystem.setDump(AUTO_CARRY); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java new file mode 100644 index 00000000..eab077ac --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCarouselCommand.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.carousel.CarouselSpinCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class AutoCarouselCommand extends SequentialCommandGroup { + + public AutoCarouselCommand( + DrivebaseSubsystem drive, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension, + CarouselSubsystem carousel + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_CAROUSEL).alongWith( + new DepositCollectCommand(deposit, extension, lift) + ), + new CarouselSpinCommand(carousel).withTimeout(4) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java new file mode 100644 index 00000000..faa599ed --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCycleCommand.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.acmerobotics.dashboard.config.Config; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.IterativeCommand; +import com.technototes.library.command.SequentialCommandGroup; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +@Config +public class AutoCycleCommand extends SequentialCommandGroup { + + public static int MAX_AUTO_CYCLES = 5; + public static int TIME_TO_STOP = 30; + + public AutoCycleCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension, + VisionSubsystem vision + ) { + super( + () -> drive.setPoseEstimate(RobotConstants.CYCLE_START_SELECT.get()), + () -> + drive.distanceSensorLocalizer.setGyroOffset( + -RobotConstants.CYCLE_START_SELECT.get().getHeading() + ), + drive::relocalize, + new AutoCyclePreloadCommand(drive, deposit, extension, lift, vision), + new IterativeCommand( + i -> + new AutoIntakeWarehouseCommand(drive, intake, lift, deposit, extension, i) + .andThen( + new AutoDepositAllianceCommand(drive, intake, lift, deposit, extension) + ) + .onlyIf(() -> CommandScheduler.getOpModeRuntime() < TIME_TO_STOP), + MAX_AUTO_CYCLES + ), + new AutoParkWarehouseCommand(drive, lift, deposit, extension), + CommandScheduler::terminateOpMode + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java new file mode 100644 index 00000000..a2d35cb6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoCyclePreloadCommand.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositPreloadCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class AutoCyclePreloadCommand extends SequentialCommandGroup { + + public AutoCyclePreloadCommand( + DrivebaseSubsystem drive, + ArmSubsystem depot, + ExtensionSubsystem extension, + LiftSubsystem lift, + VisionSubsystem vision + ) { + super( + new TrajectorySequenceCommand( + drive, + RobotConstants.CYCLE_DEPOSIT_PRELOAD + )//.alongWith(new LiftBarcodeSelectCommand(lift, vision) + .alongWith(new DepositPreloadCommand(depot, extension, lift, vision)), + new BucketDumpCommand(depot) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java new file mode 100644 index 00000000..edbb925c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositAllianceCommand.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.RegenerativeTrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCycleAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +@com.acmerobotics.dashboard.config.Config +public class AutoDepositAllianceCommand extends SequentialCommandGroup { + + public static double TIME = 1; + + public AutoDepositAllianceCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + drive::relocalize, + new RegenerativeTrajectorySequenceCommand( + drive, + RobotConstants.WAREHOUSE_TO_HUB, + drive + ).alongWith( + deposit::slightCarry, + new WaitCommand(0.3) + .andThen(new IntakeOutCommand(intake)) + .withTimeout(TIME) + .andThen(new DepositCycleAllianceCommand(deposit, extension, lift)) + ), + new BucketDumpCommand(deposit) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java new file mode 100644 index 00000000..90c706d5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDepositDuckCommand.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class AutoDepositDuckCommand extends SequentialCommandGroup { + + public AutoDepositDuckCommand( + DrivebaseSubsystem drive, + ArmSubsystem depot, + ExtensionSubsystem extension, + LiftSubsystem lift, + IntakeSubsystem intake + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.DUCK_INTAKE_TO_HUB).alongWith( + new WaitCommand(0.3).andThen(new IntakeOutCommand(intake)).withTimeout(0.6), + depot::slightCarry, + new WaitCommand(0.3).andThen(new DepositAllianceCommand(depot, extension, lift)) + ), + new BucketDumpCommand(depot).sleep(0.3) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java new file mode 100644 index 00000000..7ae11190 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckCommand.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class AutoDuckCommand extends SequentialCommandGroup { + + public AutoDuckCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension, + VisionSubsystem vision, + CarouselSubsystem carousel + ) { + super( + () -> drive.setPoseEstimate(RobotConstants.DUCK_START_SELECT.get()), + () -> + drive.distanceSensorLocalizer.setGyroOffset( + -RobotConstants.DUCK_START_SELECT.get().getHeading() + ), + //drive::relocalize, + new AutoDuckPreloadCommand(drive, deposit, extension, lift, vision), + new AutoCarouselCommand(drive, lift, deposit, extension, carousel), + new AutoIntakeDuckCommand(drive, intake), + new AutoDepositDuckCommand(drive, deposit, extension, lift, intake), + new AutoParkBarrierCommand(drive, lift, deposit, extension), + CommandScheduler::terminateOpMode + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java new file mode 100644 index 00000000..bcf61202 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoDuckPreloadCommand.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositPreloadCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class AutoDuckPreloadCommand extends SequentialCommandGroup { + + public AutoDuckPreloadCommand( + DrivebaseSubsystem drive, + ArmSubsystem depot, + ExtensionSubsystem extension, + LiftSubsystem lift, + VisionSubsystem vision + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.DUCK_DEPOSIT_PRELOAD).alongWith( + new DepositPreloadCommand(depot, extension, lift, vision) + ), + new BucketDumpCommand(depot) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java new file mode 100644 index 00000000..3c3ae489 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeDuckCommand.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; + +public class AutoIntakeDuckCommand extends SequentialCommandGroup { + + public AutoIntakeDuckCommand(DrivebaseSubsystem drive, IntakeSubsystem intake) { + super( + //drive::relocalize, + new TrajectorySequenceCommand(drive, RobotConstants.CAROUSEL_TO_DUCK_INTAKE).alongWith( + new IntakeInCommand(intake) + ), + new WaitCommand(1) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java new file mode 100644 index 00000000..23cd5757 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoIntakeWarehouseCommand.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeSafeCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class AutoIntakeWarehouseCommand extends SequentialCommandGroup { + + public AutoIntakeWarehouseCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension, + int cycle + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_WAREHOUSE, cycle) + .alongWith(new WaitCommand(1.4).andThen(new IntakeInCommand(intake))) + .alongWith(new DepositCollectCommand(deposit, extension, lift)) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java new file mode 100644 index 00000000..ed05c062 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkBarrierCommand.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.ConditionalCommand; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class AutoParkBarrierCommand extends SequentialCommandGroup { + + public AutoParkBarrierCommand( + DrivebaseSubsystem drive, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new DepositCollectCommand(deposit, extension, lift), + new ConditionalCommand(() -> CommandScheduler.getOpModeRuntime() > 26), + new TrajectorySequenceCommand(drive, RobotConstants.HUB_BARRIER_PARK) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java new file mode 100644 index 00000000..e84e511e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkSquareCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class AutoParkSquareCommand extends SequentialCommandGroup { + + public AutoParkSquareCommand( + DrivebaseSubsystem drive, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_SQUARE).alongWith( + new DepositCollectCommand(deposit, extension, lift) + ) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java new file mode 100644 index 00000000..956cccad --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/AutoParkWarehouseCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class AutoParkWarehouseCommand extends SequentialCommandGroup { + + public AutoParkWarehouseCommand( + DrivebaseSubsystem drive, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_PARK).alongWith( + new DepositCollectCommand(deposit, extension, lift) + ) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java new file mode 100644 index 00000000..92204213 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositAllianceCommand.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.RegenerativeTrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.RobotState; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class TeleopDepositAllianceCommand extends SequentialCommandGroup { + + public DrivebaseSubsystem drivebaseSubsystem; + + public TeleopDepositAllianceCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new WaitCommand(0.3), + drive::relocalizeUnsafe, + new RegenerativeTrajectorySequenceCommand( + drive, + RobotConstants.WAREHOUSE_TO_HUB, + drive + ).alongWith( + new IntakeOutCommand(intake) + .withTimeout(0.3) + .andThen(new DepositAllianceCommand(deposit, extension, lift)) + ), + new BucketDumpCommand(deposit) + ); + drivebaseSubsystem = drive; + } + + @Override + public void initialize() { + super.initialize(); + RobotState.startDeposit(); + } + + @Override + public void end(boolean cancel) { + RobotState.stopDeposit(); + super.end(cancel); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java new file mode 100644 index 00000000..d2f2b462 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopDepositSharedCommand.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.RegenerativeTrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.RobotState; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.BucketDumpCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositSharedCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeOutCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class TeleopDepositSharedCommand extends SequentialCommandGroup { + + public DrivebaseSubsystem drivebaseSubsystem; + + public TeleopDepositSharedCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new WaitCommand(0.3), + drive::relocalizeUnsafe, + new RegenerativeTrajectorySequenceCommand( + drive, + RobotConstants.WAREHOUSE_TO_SHARED_HUB, + drive + ).alongWith( + new IntakeOutCommand(intake).withTimeout(0.5), + //new WaitCommand(0.3).andThen(new DriveRelocalizeSharedCommand(drive)), + new WaitCommand(0.1).andThen(new DepositSharedCommand(deposit, extension, lift)) + ), + new WaitCommand(0.1), + new BucketDumpCommand(deposit) + ); + drivebaseSubsystem = drive; + } + + @Override + public void initialize() { + super.initialize(); + RobotState.startDeposit(); + } + + @Override + public void end(boolean cancel) { + RobotState.stopDeposit(); + super.end(cancel); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java new file mode 100644 index 00000000..ebe0d33a --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeAllianceWarehouseCommand.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class TeleopIntakeAllianceWarehouseCommand extends SequentialCommandGroup { + + public DrivebaseSubsystem drivebaseSubsystem; + + public TeleopIntakeAllianceWarehouseCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_WAREHOUSE, 4).alongWith( + new DepositCollectCommand(deposit, extension, lift).andThen( + new IntakeInCommand(intake) + ) + ), + new WaitCommand(0.3) + ); + drivebaseSubsystem = drive; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java new file mode 100644 index 00000000..3963e2bb --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/autonomous/TeleopIntakeSharedWarehouseCommand.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.autonomous; + +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.path.command.TrajectorySequenceCommand; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.deposit.DepositCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.intake.IntakeInCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class TeleopIntakeSharedWarehouseCommand extends SequentialCommandGroup { + + public DrivebaseSubsystem drivebaseSubsystem; + + public TeleopIntakeSharedWarehouseCommand( + DrivebaseSubsystem drive, + IntakeSubsystem intake, + LiftSubsystem lift, + ArmSubsystem deposit, + ExtensionSubsystem extension + ) { + super( + new TrajectorySequenceCommand(drive, RobotConstants.SHARED_HUB_TO_WAREHOUSE).alongWith( + new DepositCollectCommand(deposit, extension, lift) + .sleep(0.3) + .andThen(new IntakeInCommand(intake)) + ), + new WaitCommand(0.3) + ); + drivebaseSubsystem = drive; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java new file mode 100644 index 00000000..612c0374 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapArmTranslateCommand.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapArmTranslateCommand implements Command { + + public CapSubsystem subsystem; + public double amount; + + public CapArmTranslateCommand(CapSubsystem cap, double amt) { + subsystem = cap; + addRequirements(cap); + amount = amt; + } + + @Override + public void initialize() { + subsystem.translateArm(amount); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.05; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java new file mode 100644 index 00000000..14a582fd --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapCloseCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapCloseCommand implements Command { + + public CapSubsystem subsystem; + + public CapCloseCommand(CapSubsystem cap) { + subsystem = cap; + addRequirements(cap); + } + + @Override + public void execute() { + subsystem.close(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java new file mode 100644 index 00000000..72591b4c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapDownCommand.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapDownCommand implements Command { + + public CapSubsystem subsystem; + + public CapDownCommand(CapSubsystem cap) { + subsystem = cap; + addRequirements(cap); + } + + @Override + public void execute() { + subsystem.down(); + subsystem.close(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.6; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java new file mode 100644 index 00000000..cc7c27b9 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOpenCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapOpenCommand implements Command { + + public CapSubsystem subsystem; + + public CapOpenCommand(CapSubsystem cap) { + subsystem = cap; + addRequirements(cap); + } + + @Override + public void execute() { + subsystem.open(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java new file mode 100644 index 00000000..dc05e5f5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapOutCommand.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapOutCommand implements Command { + + public CapSubsystem subsystem; + + public CapOutCommand(CapSubsystem cap) { + subsystem = cap; + addRequirements(cap); + } + + @Override + public void execute() { + subsystem.raise(); + if (getRuntime().seconds() > 0.5) subsystem.raise2(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 1; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java new file mode 100644 index 00000000..fad63c65 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapStoreCommand.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapStoreCommand implements Command { + + public CapSubsystem subsystem; + + public CapStoreCommand(CapSubsystem cap) { + subsystem = cap; + addRequirements(cap); + } + + @Override + public void execute() { + subsystem.up(); + subsystem.close(); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.5; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java new file mode 100644 index 00000000..ec318fdf --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/cap/CapTurretTranslateCommand.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.cap; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CapSubsystem; + +public class CapTurretTranslateCommand implements Command { + + public CapSubsystem subsystem; + public double amount; + + public CapTurretTranslateCommand(CapSubsystem cap, double amt) { + subsystem = cap; + addRequirements(cap); + amount = amt; + } + + @Override + public void initialize() { + subsystem.translateTurret(amount); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.1; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java new file mode 100644 index 00000000..28f322e8 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselLeftCommand.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.carousel; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.SPIN_OFFSET; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; + +public class CarouselLeftCommand implements Command { + + CarouselSubsystem subsystem; + + public CarouselLeftCommand(CarouselSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + subsystem.left(getRuntime().seconds() / SPIN_OFFSET); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + subsystem.stop(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java new file mode 100644 index 00000000..1c0bdf1d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselRightCommand.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.carousel; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem.CarouselConstants.SPIN_OFFSET; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; + +public class CarouselRightCommand implements Command { + + CarouselSubsystem subsystem; + + public CarouselRightCommand(CarouselSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + subsystem.right(getRuntime().seconds() / SPIN_OFFSET); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + subsystem.stop(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java new file mode 100644 index 00000000..8b71c4a6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselSpinCommand.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.carousel; + +import com.technototes.library.command.Command; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; + +public class CarouselSpinCommand implements Command { + + CarouselSubsystem subsystem; + + public CarouselSpinCommand(CarouselSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + if (RobotConstants.getAlliance() == Alliance.RED) subsystem.left(); + else subsystem.right(); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + subsystem.stop(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java new file mode 100644 index 00000000..1415825b --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/carousel/CarouselStopCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.carousel; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.CarouselSubsystem; + +public class CarouselStopCommand implements Command { + + public CarouselSubsystem subsystem; + + public CarouselStopCommand(CarouselSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + subsystem.stop(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java new file mode 100644 index 00000000..db828cc5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositAllianceCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.deposit; + +import com.technototes.library.command.ParallelCommandGroup; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionOutCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel3Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class DepositAllianceCommand extends ParallelCommandGroup { + + public DepositAllianceCommand( + ArmSubsystem arm, + ExtensionSubsystem extension, + LiftSubsystem lift + ) { + super( + new LiftLevel3Command(lift).withTimeout(1), + new WaitCommand(0).andThen(new ExtensionOutCommand(extension)), + new ArmAllianceCommand(arm) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java new file mode 100644 index 00000000..6104c928 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCollectCommand.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.deposit; + +import com.technototes.library.command.ParallelCommandGroup; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmInCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftCollectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel1Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class DepositCollectCommand extends ParallelCommandGroup { + + public DepositCollectCommand( + ArmSubsystem arm, + ExtensionSubsystem extension, + LiftSubsystem lift + ) { + super( + new WaitCommand(0.2).andThen(new ArmInCommand(arm)), + new ExtensionCollectCommand(extension), + new WaitCommand(0.2).andThen( + new LiftLevel1Command(lift) + .withTimeout(0.5) + .alongWith(new WaitCommand(0.5)) + .andThen(new LiftCollectCommand(lift).withTimeout(0.3)) + ) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java new file mode 100644 index 00000000..e579e161 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositCycleAllianceCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.deposit; + +import com.technototes.library.command.ParallelCommandGroup; +import com.technototes.library.command.WaitCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionOutCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftLevel3Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class DepositCycleAllianceCommand extends ParallelCommandGroup { + + public DepositCycleAllianceCommand( + ArmSubsystem arm, + ExtensionSubsystem extension, + LiftSubsystem lift + ) { + super( + new LiftLevel3Command(lift).withTimeout(1), + //new WaitCommand(0).andThen(new ExtensionOutCommand(extension, RobotConstants.getAlliance().selectOf(ExtensionSubsystem.ExtensionConstants.AUTO_LEFT, ExtensionSubsystem.ExtensionConstants.AUTO_RIGHT))), + new ExtensionOutCommand(extension), + new ArmAllianceCommand(arm) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java new file mode 100644 index 00000000..11fed43e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositOppositeSharedCommand.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.deposit; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.OUT; + +import com.technototes.library.command.ParallelCommandGroup; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmAllianceCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionLeftSideCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionRightSideCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftSharedCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class DepositOppositeSharedCommand extends ParallelCommandGroup { + + public DepositOppositeSharedCommand( + ArmSubsystem arm, + ExtensionSubsystem extension, + LiftSubsystem lift + ) { + super( + new LiftSharedCommand(lift), + Alliance.Selector.selectOf( + RobotConstants.getAlliance(), + new ExtensionLeftSideCommand(extension, OUT), + new ExtensionRightSideCommand(extension, OUT) + ), + new ArmAllianceCommand(arm) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java new file mode 100644 index 00000000..e7676788 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositPreloadCommand.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.deposit; + +import com.technototes.library.command.ParallelCommandGroup; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmBarcodeSelectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionBarcodeSelectCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftBarcodeSelectCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class DepositPreloadCommand extends ParallelCommandGroup { + + public DepositPreloadCommand( + ArmSubsystem arm, + ExtensionSubsystem extension, + LiftSubsystem lift, + VisionSubsystem vision + ) { + super( + new LiftBarcodeSelectCommand(lift, vision).withTimeout(1), + new ArmBarcodeSelectCommand(arm, vision), + new ExtensionBarcodeSelectCommand(extension, vision) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java new file mode 100644 index 00000000..2c692e90 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/deposit/DepositSharedCommand.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.deposit; + +import com.technototes.library.command.ParallelCommandGroup; +import org.firstinspires.ftc.ptechnodactyl.commands.arm.ArmSharedCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.extension.ExtensionSideCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.lift.LiftSharedCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class DepositSharedCommand extends ParallelCommandGroup { + + public DepositSharedCommand( + ArmSubsystem arm, + ExtensionSubsystem extension, + LiftSubsystem lift + ) { + super( + new LiftSharedCommand(lift), + new ExtensionSideCommand(extension), + new ArmSharedCommand(arm) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java new file mode 100644 index 00000000..13471553 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionBarcodeSelectCommand.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class ExtensionBarcodeSelectCommand extends ExtensionOutCommand { + + public VisionSubsystem visionSubsystem; + + public ExtensionBarcodeSelectCommand(ExtensionSubsystem s, VisionSubsystem v) { + super(s); + visionSubsystem = v; + } + + @Override + public void execute() { + extensionSubsystem.center(); + extensionSubsystem.setSlide( + visionSubsystem.barcodePipeline.zero() + ? ExtensionSubsystem.ExtensionConstants.LOW_GOAL_AUTO + : ExtensionSubsystem.ExtensionConstants.OUT + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java new file mode 100644 index 00000000..8601576f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectCommand.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionCollectCommand extends ExtensionCommand { + + public ExtensionCollectCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionConstants.IN, ExtensionConstants.CENTER); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.7; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java new file mode 100644 index 00000000..882c3f48 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCollectSafeCommand.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionCollectSafeCommand extends ExtensionCommand { + + public ExtensionCollectSafeCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionConstants.IN, ExtensionConstants.CENTER); + } + + @Override + public void execute() { + extensionSubsystem.setTurret(turretTarget); + if (getRuntime().seconds() > 0.3) extensionSubsystem.setSlide(slideTarget); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.5; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java new file mode 100644 index 00000000..fdbabd9e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionCommand.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionCommand implements Command { + + public ExtensionSubsystem extensionSubsystem; + public double slideTarget, turretTarget; + + public ExtensionCommand(ExtensionSubsystem subsystem, double slide, double turret) { + extensionSubsystem = subsystem; + addRequirements(subsystem); + slideTarget = slide; + turretTarget = turret; + } + + @Override + public void execute() { + extensionSubsystem.setTurret(turretTarget); + extensionSubsystem.setSlide(slideTarget); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java new file mode 100644 index 00000000..74b9e321 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftOutCommand.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionLeftOutCommand extends ExtensionLeftSideCommand { + + public ExtensionLeftOutCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionSubsystem.ExtensionConstants.OUT); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java new file mode 100644 index 00000000..a7bd0acc --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionLeftSideCommand.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionLeftSideCommand extends ExtensionOutCommand { + + public ExtensionLeftSideCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionConstants.SHARED, ExtensionConstants.LEFT); + } + + public ExtensionLeftSideCommand(ExtensionSubsystem subsystem, double extension) { + super(subsystem, extension, ExtensionConstants.LEFT); + } + + @Override + public void execute() { + if (getRuntime().seconds() < 0.7) extensionSubsystem.fullyOut(); + else { + extensionSubsystem.setSlide(slideTarget); + extensionSubsystem.setTurret(turretTarget); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java new file mode 100644 index 00000000..9667bea3 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionOutCommand.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionOutCommand extends ExtensionCommand { + + public ExtensionOutCommand(ExtensionSubsystem subsystem, double turret) { + super(subsystem, ExtensionConstants.OUT, turret); + } + + public ExtensionOutCommand(ExtensionSubsystem subsystem, double slide, double turret) { + super(subsystem, slide, turret); + } + + public ExtensionOutCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionConstants.OUT, ExtensionConstants.CENTER); + } + + @Override + public void execute() { + extensionSubsystem.setSlide(slideTarget); + if (getRuntime().seconds() > 0.3) extensionSubsystem.setTurret(turretTarget); + } + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.6; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java new file mode 100644 index 00000000..c20bea4d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightOutCommand.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionRightOutCommand extends ExtensionRightSideCommand { + + public ExtensionRightOutCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionSubsystem.ExtensionConstants.OUT); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java new file mode 100644 index 00000000..0c422a1c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionRightSideCommand.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionRightSideCommand extends ExtensionOutCommand { + + public ExtensionRightSideCommand(ExtensionSubsystem subsystem) { + super(subsystem, ExtensionConstants.SHARED, ExtensionConstants.RIGHT); + } + + public ExtensionRightSideCommand(ExtensionSubsystem subsystem, double extension) { + super(subsystem, extension, ExtensionConstants.RIGHT); + } + + @Override + public void execute() { + if (getRuntime().seconds() < 0.7) extensionSubsystem.fullyOut(); + else { + extensionSubsystem.setSlide(slideTarget); + extensionSubsystem.setTurret(turretTarget); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java new file mode 100644 index 00000000..af4f2db5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionSideCommand.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import static org.firstinspires.ftc.ptechnodactyl.RobotConstants.getAlliance; +import static org.firstinspires.ftc.ptechnodactyl.RobotState.SharedHubStrategy.OWN; +import static org.firstinspires.ftc.ptechnodactyl.RobotState.SharedHubStrategy.STEAL; +import static org.firstinspires.ftc.ptechnodactyl.RobotState.getSharedStrategy; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.LEFT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.RIGHT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.SHARED; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem.ExtensionConstants.STEAL_SHARED; + +import android.util.Pair; +import com.technototes.library.command.ChoiceCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionSideCommand extends ChoiceCommand { + + public ExtensionSideCommand(ExtensionSubsystem subsystem) { + super( + new Pair<>( + () -> getSharedStrategy() == OWN, + new ExtensionOutCommand(subsystem, SHARED, getAlliance().selectOf(RIGHT, LEFT)) + ), + new Pair<>( + () -> getSharedStrategy() == STEAL, + new ExtensionOutCommand( + subsystem, + STEAL_SHARED, + getAlliance().selectOf(LEFT, RIGHT) + ) + ) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java new file mode 100644 index 00000000..11605473 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/ExtensionTranslateCommand.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class ExtensionTranslateCommand extends ExtensionCommand { + + public ExtensionTranslateCommand(ExtensionSubsystem subsystem, double slide) { + super(subsystem, slide, 0); + } + + @Override + public void initialize() { + extensionSubsystem.translateSlide(slideTarget); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.05; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java new file mode 100644 index 00000000..c7ced9e3 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/extension/TurretTranslateCommand.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.extension; + +import java.util.function.BooleanSupplier; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ExtensionSubsystem; + +public class TurretTranslateCommand extends ExtensionCommand { + + public BooleanSupplier flipTranslate; + + public TurretTranslateCommand( + ExtensionSubsystem subsystem, + double turret, + BooleanSupplier flip + ) { + super(subsystem, 0, turret); + flipTranslate = flip; + } + + @Override + public void initialize() { + extensionSubsystem.translateTurret( + flipTranslate.getAsBoolean() ? -turretTarget : turretTarget + ); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return getRuntime().seconds() > 0.01; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java new file mode 100644 index 00000000..1fe462c7 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeInCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.intake; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; + +public class IntakeInCommand implements Command { + + IntakeSubsystem subsystem; + + public IntakeInCommand(IntakeSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + subsystem.in(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java new file mode 100644 index 00000000..2f05c979 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeOutCommand.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.intake; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; + +public class IntakeOutCommand implements Command { + + IntakeSubsystem subsystem; + + public IntakeOutCommand(IntakeSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + if (getRuntime().seconds() % 0.2 < 0.1) subsystem.out(); + else subsystem.stop(); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + subsystem.stop(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java new file mode 100644 index 00000000..91562182 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeSafeCommand.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.intake; + +import org.firstinspires.ftc.ptechnodactyl.RobotState; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; + +public class IntakeSafeCommand extends IntakeInCommand { + + public IntakeSafeCommand(IntakeSubsystem s) { + super(s); + } + + @Override + public boolean isFinished() { + return RobotState.hasFreight() && getRuntime().seconds() > 0.2; + } + + @Override + public void end(boolean cancel) { + if (!cancel) subsystem.idle(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java new file mode 100644 index 00000000..d1c4d1ef --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/intake/IntakeStopCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.intake; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem; + +public class IntakeStopCommand implements Command { + + IntakeSubsystem subsystem; + + public IntakeStopCommand(IntakeSubsystem s) { + subsystem = s; + addRequirements(s); + } + + @Override + public void execute() { + subsystem.stop(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java new file mode 100644 index 00000000..f4842384 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftBarcodeSelectCommand.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import android.util.Pair; +import com.technototes.library.command.ChoiceCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class LiftBarcodeSelectCommand extends ChoiceCommand { + + public LiftBarcodeSelectCommand(LiftSubsystem lift, VisionSubsystem vision) { + super( + new Pair<>(vision.barcodePipeline::zero, new LiftLevel1Command(lift)), + new Pair<>(vision.barcodePipeline::one, new LiftLevel2Command(lift)), + new Pair<>(vision.barcodePipeline::twoOrDefault, new LiftLevel3Command(lift)) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java new file mode 100644 index 00000000..fda5a0ae --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCollectCommand.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftCollectCommand extends LiftCommand { + + public LiftCollectCommand(LiftSubsystem l) { + super(l, LiftSubsystem.LiftConstants.COLLECT); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java new file mode 100644 index 00000000..a0ca79e7 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftCommand.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import com.technototes.library.command.Command; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftCommand implements Command { + + public LiftSubsystem liftSys; + public DoubleSupplier doubleSupplier; + + public LiftCommand(LiftSubsystem ls, DoubleSupplier ds) { + liftSys = ls; + doubleSupplier = ds; + addRequirements(ls); + } + + public LiftCommand(LiftSubsystem ls, double ds) { + this(ls, () -> ds); + } + + @Override + public void initialize() { + liftSys.setLiftPosition(doubleSupplier.getAsDouble()); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return liftSys.isAtTarget(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java new file mode 100644 index 00000000..8820e0df --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel1Command.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftLevel1Command extends LiftCommand { + + public LiftLevel1Command(LiftSubsystem l) { + super(l, LiftSubsystem.LiftConstants.LEVEL_1); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java new file mode 100644 index 00000000..3df1fb24 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2Command.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftLevel2Command extends LiftCommand { + + public LiftLevel2Command(LiftSubsystem l) { + super(l, LiftSubsystem.LiftConstants.LEVEL_2); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java new file mode 100644 index 00000000..42afc7e6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel2TeleCommand.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftLevel2TeleCommand extends LiftCommand { + + public LiftLevel2TeleCommand(LiftSubsystem l) { + super(l, LiftSubsystem.LiftConstants.TELEOP_LEVEL_2); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java new file mode 100644 index 00000000..48c5524b --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevel3Command.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftLevel3Command extends LiftCommand { + + public LiftLevel3Command(LiftSubsystem l) { + super(l, LiftSubsystem.LiftConstants.LEVEL_3); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java new file mode 100644 index 00000000..6bd57dab --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftLevelCommand.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import static org.firstinspires.ftc.ptechnodactyl.RobotState.AllianceHubStrategy.HIGH; +import static org.firstinspires.ftc.ptechnodactyl.RobotState.AllianceHubStrategy.MID; +import static org.firstinspires.ftc.ptechnodactyl.RobotState.getAllianceStrategy; + +import android.util.Pair; +import com.technototes.library.command.ChoiceCommand; +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftLevelCommand extends ChoiceCommand { + + public LiftLevelCommand(LiftSubsystem ls) { + super( + new Pair<>(() -> getAllianceStrategy() == HIGH, new LiftLevel3Command(ls)), + new Pair<>(() -> getAllianceStrategy() == MID, new LiftLevel2TeleCommand(ls)) + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java new file mode 100644 index 00000000..d4c9505e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftSharedCommand.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftSharedCommand extends LiftCommand { + + public LiftSharedCommand(LiftSubsystem l) { + super(l, LiftSubsystem.LiftConstants.NEUTRAL); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java new file mode 100644 index 00000000..dfda6a2c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/lift/LiftTranslateCommand.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.lift; + +import org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem; + +public class LiftTranslateCommand extends LiftCommand { + + public LiftTranslateCommand(LiftSubsystem ls, double amt) { + super(ls, amt); + } + + @Override + public void initialize() { + liftSys.setLiftPosition(liftSys.get() + doubleSupplier.getAsDouble()); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return super.isFinished() && getRuntime().seconds() > 0.1; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java new file mode 100644 index 00000000..0734241f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/vision/VisionBarcodeCommand.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.ptechnodactyl.commands.vision; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem; + +public class VisionBarcodeCommand implements Command { + + public VisionSubsystem subsystem; + + public VisionBarcodeCommand(VisionSubsystem s) { + subsystem = s; + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.startBarcodePipeline(); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean cancel) { + subsystem.stopBarcodePipeline(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java deleted file mode 100644 index 2727dddc..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.controllers; - -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.control.CommandAxis; -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.DrivingCommands; -import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; - -public class DriverController { - - public Robot robot; - public CommandGamepad gamepad; - - public Stick driveLeftStick, driveRightStick; - public CommandButton resetGyroButton, turboButton, snailButton; - public CommandButton override; - - public DriverController(CommandGamepad g, Robot r) { - this.robot = r; - gamepad = g; - override = g.leftTrigger.getAsButton(0.5); - - AssignNamedControllerButton(); - if (Setup.Connected.DRIVEBASE) { - bindDriveControls(); - } - } - - private void AssignNamedControllerButton() { - resetGyroButton = gamepad.ps_options; - driveLeftStick = gamepad.leftStick; - driveRightStick = gamepad.rightStick; - turboButton = gamepad.leftBumper; - snailButton = gamepad.rightBumper; - } - - public void bindDriveControls() { - CommandScheduler.scheduleJoystick( - new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) - ); - - turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); - turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); - snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); - snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); - resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java deleted file mode 100644 index 22ab7690..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.controllers; - -import com.technototes.library.control.CommandButton; -import com.technototes.library.control.CommandGamepad; -import com.technototes.library.control.Stick; -import org.firstinspires.ftc.ptechnodactyl.Robot; - -public class OperatorController { - - public Robot robot; - public CommandGamepad gamepad; - public CommandButton openClaw; - public CommandButton closeClaw; - public Stick armStick; - public CommandButton ArmHorizontal; - public CommandButton ArmVertical; - public CommandButton intake; - - public OperatorController(CommandGamepad g, Robot r) { - robot = r; - gamepad = g; - AssignNamedControllerButton(); - BindControls(); - } - - private void AssignNamedControllerButton() { - openClaw = gamepad.leftBumper; - closeClaw = gamepad.rightBumper; - armStick = gamepad.rightStick; - ArmHorizontal = gamepad.ps_circle; - ArmVertical = gamepad.ps_triangle; - intake = gamepad.dpadRight; - } - - public void BindControls() { - - } - - -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java new file mode 100644 index 00000000..f3c8bd1e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoCycleBase.java @@ -0,0 +1,56 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.technototes.library.command.Command; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.autonomous.AutoCycleCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.vision.VisionBarcodeCommand; + +public abstract class AutoCycleBase extends CommandOpMode implements Loggable { + + public Robot robot; + public Hardware hardware; + + @Override + public void uponInit() { + RobotConstants.updateAlliance(Alliance.get(getClass())); + hardware = new Hardware(); + robot = new Robot(hardware); + CommandScheduler.scheduleInit(new VisionBarcodeCommand(robot.visionSubsystem)); + CommandScheduler.scheduleOnceForState( + new AutoCycleCommand( + robot.drivebaseSubsystem, + robot.intakeSubsystem, + robot.liftSubsystem, + robot.armSubsystem, + robot.extensionSubsystem, + robot.visionSubsystem + ), + OpModeState.RUN + ); + } + + @Override + public void uponStart() { + if (Robot.SubsystemConstants.CAP_ENABLED) robot.capSubsystem.up(); + } + + @Override + public void end() {} + + @Autonomous(name = "\uD83D\uDD35 ♻️ Blue Cycle") + @Alliance.Blue + public static class CycleBlueAuto extends AutoCycleBase {} + + @Autonomous(name = "\uD83D\uDFE5 ♻️ Red Cycle") + @Alliance.Red + public static class CycleRedAuto extends AutoCycleBase {} +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java new file mode 100644 index 00000000..a383a111 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/AutoDuckBase.java @@ -0,0 +1,56 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; +import org.firstinspires.ftc.ptechnodactyl.commands.autonomous.AutoDuckCommand; +import org.firstinspires.ftc.ptechnodactyl.commands.vision.VisionBarcodeCommand; + +public abstract class AutoDuckBase extends CommandOpMode implements Loggable { + + public Robot robot; + public Hardware hardware; + + @Override + public void uponInit() { + RobotConstants.updateAlliance(Alliance.get(getClass())); + hardware = new Hardware(); + robot = new Robot(hardware); + CommandScheduler.scheduleInit(new VisionBarcodeCommand(robot.visionSubsystem)); + CommandScheduler.scheduleOnceForState( + new AutoDuckCommand( + robot.drivebaseSubsystem, + robot.intakeSubsystem, + robot.liftSubsystem, + robot.armSubsystem, + robot.extensionSubsystem, + robot.visionSubsystem, + robot.carouselSubsystem + ), + OpModeState.RUN + ); + } + + @Override + public void uponStart() { + if (Robot.SubsystemConstants.CAP_ENABLED) robot.capSubsystem.up(); + } + + @Override + public void end() {} + + @Autonomous(name = "\uD83D\uDD35 \uD83E\uDD86 Blue Duck") + @Alliance.Blue + public static class DuckBlueAuto extends AutoDuckBase {} + + @Autonomous(name = "\uD83D\uDFE5 \uD83E\uDD86 Red Duck") + @Alliance.Red + public static class DuckRedAuto extends AutoDuckBase {} +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java deleted file mode 100644 index a77ee77b..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/Basic.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.command.SequentialCommandGroup; -import com.technototes.library.command.WaitCommand; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.path.command.TrajectorySequenceCommand; -import com.technototes.path.geometry.ConfigurablePoseD; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; - -/* - * Some Emojis for opmode names: - * Copy them and paste them in the 'name' section of the @Autonomous tag - * Red alliance: 🟥 - * Blue alliance: 🟦 - * Wing position: 🪶 - * Backstage pos: 🎦 - */ -//@Config -@Autonomous(name = "Basic Auto", preselectTeleOp = "PlainDrive") -@SuppressWarnings("unused") -public class Basic extends CommandOpMode implements Loggable { - - public static int AUTO_TIME = 1; - public Hardware hardware; - public Robot robot; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - CommandScheduler.scheduleForState( - new SequentialCommandGroup( - new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> - b - .apply(new ConfigurablePoseD(0, 0, 0).toPose()) - .lineTo(new Vector2d(5, 5)) - .build() - ), - // new TurboCommand(robot.drivebaseSubsystem), - // new StartSpinningCmd(robot.spinner), - new WaitCommand(AUTO_TIME), - new TrajectorySequenceCommand(robot.drivebaseSubsystem, b -> - b - .apply(new ConfigurablePoseD(5, 5, 0).toPose()) - .lineTo(new Vector2d(0, 0)) - .build() - ), - // new StopSpinningCmd(robot.spinner), - CommandScheduler::terminateOpMode - ), - CommandOpMode.OpModeState.RUN - ); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java new file mode 100644 index 00000000..3e988799 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/LoopTimeTest.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.hardware.sensor.ColorDistanceSensor; +import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; +import com.technototes.library.logger.Log; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +@TeleOp(name = "looptimetest") +public class LoopTimeTest extends CommandOpMode { + + public ColorDistanceSensor sensor; + + @Log.Number(index = 0, name = "sensor calls per loop") + public double sensorLoops = 0; + + @Log.Number(index = 1, name = "looptime in hz") + public double hz; + + @Log.Number(index = 2, name = "avg sensor call time") + public double timePerSensorCall; + + @Log.Number(index = 3, name = "sensor distance inches") + public double distance; + + @Log.Number(index = 4, name = "sensor raw light") + public double light; + + private long pastTime; + + @Override + public void uponInit() { + sensor = new ColorDistanceSensor("irange").onUnit(DistanceUnit.INCH); + + driverGamepad.dpadUp.whenPressed(() -> sensorLoops++); + driverGamepad.dpadDown.whenPressed(() -> sensorLoops--); + } + + @Override + public void runLoop() { + long l = System.currentTimeMillis(); + for (int i = 0; i < sensorLoops; i++) { + distance = sensor.getDistance(); + light = sensor.getLight(); + } + if (sensorLoops != 0) timePerSensorCall = (System.currentTimeMillis() - l) / sensorLoops; + hz = 1000.0 / (System.currentTimeMillis() - pastTime); + pastTime = System.currentTimeMillis(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java new file mode 100644 index 00000000..4d47a8fa --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/BackAndForth.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.technototes.library.hardware.HardwareDevice; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin moving forward and + * backward. You should observe the target position (green) and your pose estimate (blue) and adjust + * your follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + * + * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It + * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class BackAndForth extends LinearOpMode { + + public static double DISTANCE = 40; + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); + // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectoryForward = drive + .trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + Trajectory trajectoryBackward = drive + .trajectoryBuilder(trajectoryForward.end()) + .back(DISTANCE) + .build(); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + drive.followTrajectory(trajectoryForward); + drive.followTrajectory(trajectoryBackward); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java new file mode 100644 index 00000000..352cdf17 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/DriveVelocityPIDTuner.java @@ -0,0 +1,174 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.*; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.RobotLog; +import com.technototes.library.hardware.HardwareDevice; +import java.util.List; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/* + * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- + * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as + * important as the positional parameters. Like the other manual tuning routines, this op mode + * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's + * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC + * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully + * connected, start the program, and your robot will begin moving forward and backward according to + * a motion profile. Your job is to graph the velocity errors over time and adjust the PID + * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). + * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the + * MOTOR_VELO_PID field. + * + * Recommended tuning process: + * + * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to + * mitigate oscillations. + * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. + * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class DriveVelocityPIDTuner extends LinearOpMode { + + public static double DISTANCE = 70; // in + + enum Mode { + DRIVER_MODE, + TUNING_MODE, + } + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + HardwareDevice.hardwareMap = hardwareMap; + + if (!RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg( + "%s does not need to be run if the built-in motor velocity" + "PID is not in use", + getClass().getSimpleName() + ); + } + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); + + Mode mode = Mode.TUNING_MODE; + + double lastKp = MOTOR_VELO_PID.p; + double lastKi = MOTOR_VELO_PID.i; + double lastKd = MOTOR_VELO_PID.d; + double lastKf = MOTOR_VELO_PID.f; + + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = kV * motionState.getV(); + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + + List velocities = drive.getWheelVelocities(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData("measuredVelocity" + i, velocities.get(i)); + telemetry.addData("error" + i, motionState.getV() - velocities.get(i)); + } + break; + case DRIVER_MODE: + if (gamepad1.b) { + drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + if ( + lastKp != MOTOR_VELO_PID.p || + lastKd != MOTOR_VELO_PID.d || + lastKi != MOTOR_VELO_PID.i || + lastKf != MOTOR_VELO_PID.f + ) { + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + lastKp = MOTOR_VELO_PID.p; + lastKi = MOTOR_VELO_PID.i; + lastKd = MOTOR_VELO_PID.d; + lastKf = MOTOR_VELO_PID.f; + } + + telemetry.update(); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java new file mode 100644 index 00000000..920ed20d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/FollowerPIDTuner.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.technototes.library.hardware.HardwareDevice; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin driving in a square. + * You should observe the target position (green) and your pose estimate (blue) and adjust your + * follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class FollowerPIDTuner extends LinearOpMode { + + public static double DISTANCE = 48; // in + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + //DrivebaseSubsystem drive = new DrivebaseSubsystem(); + + Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); + + drive.setPoseEstimate(startPose); + + waitForStart(); + + if (isStopRequested()) return; + + while (!isStopRequested()) { + Trajectory traj = drive.trajectoryBuilder(startPose).forward(DISTANCE).build(); + drive.followTrajectory(traj); + drive.turn(Math.toRadians(90)); + + startPose = traj.end().plus(new Pose2d(0, 0, Math.toRadians(90))); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java new file mode 100644 index 00000000..bab57fac --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/LocalizationTest.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.technototes.library.hardware.HardwareDevice; + +/** + * This is a simple teleop routine for testing localization. Drive the robot around like a normal + * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight + * errors are not out of the ordinary, especially with sudden drive motions). The goal of this + * exercise is to ascertain whether the localizer has been configured properly (note: the pure + * encoder localizer heading may be significantly off if the track width has not been tuned). + */ +@Disabled +@TeleOp(group = "drive") +public class LocalizationTest extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + //DrivebaseSubsystem drive = new DrivebaseSubsystem(); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + waitForStart(); + + while (!isStopRequested()) { + drive.setWeightedDrivePower( + new Pose2d(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x) + ); + System.out.println( + drive.leftFront.getCurrentPosition() + + " " + + drive.leftRear.getCurrentPosition() + + " " + + drive.rightFront.getCurrentPosition() + + " " + + drive.rightRear.getCurrentPosition() + ); + + drive.update(); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java new file mode 100644 index 00000000..294e253f --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/ManualFeedforwardTuner.java @@ -0,0 +1,156 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.*; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.kinematics.Kinematics; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; +import com.technototes.library.hardware.HardwareDevice; +import java.util.Objects; + +/* + * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, + * tuning these coefficients is just as important as the positional parameters. Like the other + * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if + * you are using the Control Hub. Once you've successfully connected, start the program, and your + * robot will begin moving forward and backward according to a motion profile. Your job is to graph + * the velocity errors over time and adjust the feedforward coefficients. Once you've found a + * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class ManualFeedforwardTuner extends LinearOpMode { + + public static double DISTANCE = 72; // in + + private FtcDashboard dashboard = FtcDashboard.getInstance(); + + // private DrivebaseSubsystem drive; + SampleMecanumDrive drive; + + enum Mode { + DRIVER_MODE, + TUNING_MODE, + } + + private Mode mode; + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + HardwareDevice.hardwareMap = hardwareMap; + + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg( + "Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID." + ); + } + + telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); + + //drive = new DrivebaseSubsystem(); + drive = new SampleMecanumDrive(hardwareMap); + + mode = Mode.TUNING_MODE; + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = Kinematics.calculateMotorFeedforward( + motionState.getV(), + motionState.getA(), + kV, + kA, + kStatic + ); + + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull( + drive.getPoseVelocity(), + "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." + ); + double currentVelo = poseVelo.getX(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + telemetry.addData("measuredVelocity", currentVelo); + telemetry.addData("error", motionState.getV() - currentVelo); + break; + case DRIVER_MODE: + if (gamepad1.b) { + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + telemetry.update(); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java new file mode 100644 index 00000000..28a7b351 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxAngularVeloTuner.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. + *

+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds. + *

+ * Further fine tuning of MAX_ANG_VEL may be desired. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class MaxAngularVeloTuner extends LinearOpMode { + + public static double RUNTIME = 4.0; + + private ElapsedTime timer; + private double maxAngVelocity = 0.0; + + @Override + public void runOpMode() throws InterruptedException { + //DrivebaseSubsystem drive = new DrivebaseSubsystem(); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(0, 0, 1)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull( + drive.getPoseVelocity(), + "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." + ); + + maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity); + } + + drive.setDrivePower(new Pose2d()); + + telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity); + telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)); + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java new file mode 100644 index 00000000..8f7d6fbf --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MaxVelocityTuner.java @@ -0,0 +1,101 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.TICKS_PER_REV; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.WHEEL_RADIUS; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.technototes.library.hardware.HardwareDevice; +import com.technototes.path.subsystem.MecanumConstants; +import java.util.Objects; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/** + * This routine is designed to calculate the maximum velocity your bot can achieve under load. It + * will also calculate the effective kF value for your velocity PID. + *

+ * Upon pressing start, your bot will run at max power for RUNTIME seconds. + *

+ * Further fine tuning of kF may be desired. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class MaxVelocityTuner extends LinearOpMode { + + public static double RUNTIME = 2.0; + + private ElapsedTime timer; + private double maxVelocity = 0.0; + + private VoltageSensor batteryVoltageSensor; + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + // DrivebaseSubsystem drive = new DrivebaseSubsystem(); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull( + drive.getPoseVelocity(), + "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." + ); + + maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); + } + + drive.setDrivePower(new Pose2d()); + + double effectiveKf = MecanumConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); + + telemetry.addData("Max Velocity", maxVelocity); + telemetry.addData( + "Voltage Compensated kF", + (effectiveKf * batteryVoltageSensor.getVoltage()) / 12 + ); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) idle(); + } + + private double veloInchesToTicks(double inchesPerSec) { + return ( + (inchesPerSec / + (2 * Math.PI * WHEEL_RADIUS) / + DrivebaseSubsystem.DriveConstants.GEAR_RATIO) * + TICKS_PER_REV + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java new file mode 100644 index 00000000..8d0dbdb8 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/MotorDirectionDebugger.java @@ -0,0 +1,105 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.hardware.HardwareDevice; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/** + * This is a simple teleop routine for debugging your motor configuration. + * Pressing each of the buttons will power its respective motor. + * + * Button Mappings: + * + * Xbox/PS4 Button - Motor + * X / ▢ - Front Left + * Y / Δ - Front Right + * B / O - Rear Right + * A / X - Rear Left + * The buttons are mapped to match the wheels spatially if you + * were to rotate the gamepad 45deg°. x/square is the front left + * ________ and each button corresponds to the wheel as you go clockwise + * / ______ \ + * ------------.-' _ '-..+ Front of Bot + * / _ ( Y ) _ \ ^ + * | ( X ) _ ( B ) | Front Left \ Front Right + * ___ '. ( A ) /| Wheel \ Wheel + * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) + * | | | \ + * '.___.' '. | Rear Left \ Rear Right + * '. / Wheel \ Wheel + * \. .' (A/X) \ (B/O) + * \________/ + * + * Uncomment the @Disabled tag below to use this opmode. + */ +@Disabled +@Config +@TeleOp(group = "drive") +public class MotorDirectionDebugger extends LinearOpMode { + + public static double MOTOR_POWER = 0.7; + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + //DrivebaseSubsystem drive = new DrivebaseSubsystem(); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + telemetry.addLine("Press play to begin the debugging opmode"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + while (!isStopRequested()) { + telemetry.addLine("Press each button to turn on its respective motor"); + telemetry.addLine(); + telemetry.addLine("Xbox/PS4 Button - Motor"); + telemetry.addLine( + "  X / ▢         - Front Left" + ); + telemetry.addLine( + "  Y / Δ         - Front Right" + ); + telemetry.addLine( + "  B / O         - Rear  Right" + ); + telemetry.addLine( + "  A / X         - Rear  Left" + ); + telemetry.addLine(); + + if (gamepad1.x) { + drive.setMotorPowers(MOTOR_POWER, 0, 0, 0); + telemetry.addLine("Running Motor: Front Left"); + } else if (gamepad1.y) { + drive.setMotorPowers(0, 0, 0, MOTOR_POWER); + telemetry.addLine("Running Motor: Front Right"); + } else if (gamepad1.b) { + drive.setMotorPowers(0, 0, MOTOR_POWER, 0); + telemetry.addLine("Running Motor: Rear Right"); + } else if (gamepad1.a) { + drive.setMotorPowers(0, MOTOR_POWER, 0, 0); + telemetry.addLine("Running Motor: Rear Left"); + } else { + drive.setMotorPowers(0, 0, 0, 0); + telemetry.addLine("Running Motor: None"); + } + + telemetry.update(); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java new file mode 100644 index 00000000..15335649 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/OpenCVTest.java @@ -0,0 +1,373 @@ +/* + * Copyright (c) 2020 OpenFTC Team + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.ptechnodactyl.subsystems.BarcodePipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvInternalCamera; +import org.openftc.easyopencv.OpenCvPipeline; +import org.openftc.easyopencv.OpenCvWebcam; + +/* + * This sample demonstrates a basic (but battle-tested and essentially + * 100% accurate) method of detecting the skystone when lined up with + * the sample regions over the first 3 stones. + */ +@Disabled +@TeleOp +public class OpenCVTest extends LinearOpMode { + + OpenCvWebcam webcam; + BarcodePipeline pipeline; + + @Override + public void runOpMode() { + /** + * NOTE: Many comments have been omitted from this sample for the + * sake of conciseness. If you're just starting out with EasyOpenCv, + * you should take a look at {@link InternalCamera1Example} or its + * webcam counterpart, {@link WebcamExample} first. + */ + + int cameraMonitorViewId = hardwareMap.appContext + .getResources() + .getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance() + .createWebcam(hardwareMap.get(WebcamName.class, "webcam"), cameraMonitorViewId); + pipeline = new BarcodePipeline(); + webcam.setPipeline(pipeline); + + webcam.openCameraDeviceAsync( + new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + /* + * This will be called if the camera could not be opened + */ + } + } + ); + + waitForStart(); + + while (opModeIsActive()) { + telemetry.addData("Analysis", pipeline.get()); + telemetry.update(); + + // Don't burn CPU cycles busy-looping in this sample + sleep(50); + } + } + + public static class SkystoneDeterminationPipeline extends OpenCvPipeline { + + /* + * An enum to define the skystone position + */ + public enum SkystonePosition { + LEFT, + CENTER, + RIGHT, + } + + /* + * Some color constants + */ + static final Scalar BLUE = new Scalar(0, 0, 255); + static final Scalar GREEN = new Scalar(0, 255, 0); + + /* + * The core values which define the location and size of the sample regions + */ + static final Point REGION1_TOPLEFT_ANCHOR_POINT = new Point(109, 98); + static final Point REGION2_TOPLEFT_ANCHOR_POINT = new Point(181, 98); + static final Point REGION3_TOPLEFT_ANCHOR_POINT = new Point(253, 98); + static final int REGION_WIDTH = 20; + static final int REGION_HEIGHT = 20; + + /* + * Points which actually define the sample region rectangles, derived from above values + * + * Example of how points A and B work to define a rectangle + * + * ------------------------------------ + * | (0,0) Point A | + * | | + * | | + * | | + * | | + * | | + * | | + * | Point B (70,50) | + * ------------------------------------ + * + */ + Point region1_pointA = new Point( + REGION1_TOPLEFT_ANCHOR_POINT.x, + REGION1_TOPLEFT_ANCHOR_POINT.y + ); + Point region1_pointB = new Point( + REGION1_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, + REGION1_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT + ); + Point region2_pointA = new Point( + REGION2_TOPLEFT_ANCHOR_POINT.x, + REGION2_TOPLEFT_ANCHOR_POINT.y + ); + Point region2_pointB = new Point( + REGION2_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, + REGION2_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT + ); + Point region3_pointA = new Point( + REGION3_TOPLEFT_ANCHOR_POINT.x, + REGION3_TOPLEFT_ANCHOR_POINT.y + ); + Point region3_pointB = new Point( + REGION3_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, + REGION3_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT + ); + + /* + * Working variables + */ + Mat region1_Cb, region2_Cb, region3_Cb; + Mat YCrCb = new Mat(); + Mat Cb = new Mat(); + int avg1, avg2, avg3; + + // Volatile since accessed by OpMode thread w/o synchronization + private volatile SkystonePosition position = SkystonePosition.LEFT; + + /* + * This function takes the RGB frame, converts to YCrCb, + * and extracts the Cb channel to the 'Cb' variable + */ + void inputToCb(Mat input) { + Imgproc.cvtColor(input, YCrCb, Imgproc.COLOR_RGB2YCrCb); + Core.extractChannel(YCrCb, Cb, 2); + } + + @Override + public void init(Mat firstFrame) { + /* + * We need to call this in order to make sure the 'Cb' + * object is initialized, so that the submats we make + * will still be linked to it on subsequent frames. (If + * the object were to only be initialized in processFrame, + * then the submats would become delinked because the backing + * buffer would be re-allocated the first time a real frame + * was crunched) + */ + inputToCb(firstFrame); + + /* + * Submats are a persistent reference to a region of the parent + * buffer. Any changes to the child affect the parent, and the + * reverse also holds true. + */ + region1_Cb = Cb.submat(new Rect(region1_pointA, region1_pointB)); + region2_Cb = Cb.submat(new Rect(region2_pointA, region2_pointB)); + region3_Cb = Cb.submat(new Rect(region3_pointA, region3_pointB)); + } + + @Override + public Mat processFrame(Mat input) { + /* + * Overview of what we're doing: + * + * We first convert to YCrCb color space, from RGB color space. + * Why do we do this? Well, in the RGB color space, chroma and + * luma are intertwined. In YCrCb, chroma and luma are separated. + * YCrCb is a 3-channel color space, just like RGB. YCrCb's 3 channels + * are Y, the luma channel (which essentially just a B&W image), the + * Cr channel, which records the difference from red, and the Cb channel, + * which records the difference from blue. Because chroma and luma are + * not related in YCrCb, vision code written to look for certain values + * in the Cr/Cb channels will not be severely affected by differing + * light intensity, since that difference would most likely just be + * reflected in the Y channel. + * + * After we've converted to YCrCb, we extract just the 2nd channel, the + * Cb channel. We do this because stones are bright yellow and contrast + * STRONGLY on the Cb channel against everything else, including SkyStones + * (because SkyStones have a black label). + * + * We then take the average pixel value of 3 different regions on that Cb + * channel, one positioned over each stone. The brightest of the 3 regions + * is where we assume the SkyStone to be, since the normal stones show up + * extremely darkly. + * + * We also draw rectangles on the screen showing where the sample regions + * are, as well as drawing a solid rectangle over top the sample region + * we believe is on top of the SkyStone. + * + * In order for this whole process to work correctly, each sample region + * should be positioned in the center of each of the first 3 stones, and + * be small enough such that only the stone is sampled, and not any of the + * surroundings. + */ + + /* + * Get the Cb channel of the input frame after conversion to YCrCb + */ + inputToCb(input); + + /* + * Compute the average pixel value of each submat region. We're + * taking the average of a single channel buffer, so the value + * we need is at index 0. We could have also taken the average + * pixel value of the 3-channel image, and referenced the value + * at index 2 here. + */ + avg1 = (int) Core.mean(region1_Cb).val[0]; + avg2 = (int) Core.mean(region2_Cb).val[0]; + avg3 = (int) Core.mean(region3_Cb).val[0]; + + /* + * Draw a rectangle showing sample region 1 on the screen. + * Simply a visual aid. Serves no functional purpose. + */ + Imgproc.rectangle( + input, // Buffer to draw on + region1_pointA, // First point which defines the rectangle + region1_pointB, // Second point which defines the rectangle + BLUE, // The color the rectangle is drawn in + 2 + ); // Thickness of the rectangle lines + + /* + * Draw a rectangle showing sample region 2 on the screen. + * Simply a visual aid. Serves no functional purpose. + */ + Imgproc.rectangle( + input, // Buffer to draw on + region2_pointA, // First point which defines the rectangle + region2_pointB, // Second point which defines the rectangle + BLUE, // The color the rectangle is drawn in + 2 + ); // Thickness of the rectangle lines + + /* + * Draw a rectangle showing sample region 3 on the screen. + * Simply a visual aid. Serves no functional purpose. + */ + Imgproc.rectangle( + input, // Buffer to draw on + region3_pointA, // First point which defines the rectangle + region3_pointB, // Second point which defines the rectangle + BLUE, // The color the rectangle is drawn in + 2 + ); // Thickness of the rectangle lines + + /* + * Find the max of the 3 averages + */ + int maxOneTwo = Math.max(avg1, avg2); + int max = Math.max(maxOneTwo, avg3); + + /* + * Now that we found the max, we actually need to go and + * figure out which sample region that value was from + */ + if ( + max == avg1 + ) { // Was it from region 1? + position = SkystonePosition.LEFT; // Record our analysis + + /* + * Draw a solid rectangle on top of the chosen region. + * Simply a visual aid. Serves no functional purpose. + */ + Imgproc.rectangle( + input, // Buffer to draw on + region1_pointA, // First point which defines the rectangle + region1_pointB, // Second point which defines the rectangle + GREEN, // The color the rectangle is drawn in + -1 + ); // Negative thickness means solid fill + } else if ( + max == avg2 + ) { // Was it from region 2? + position = SkystonePosition.CENTER; // Record our analysis + + /* + * Draw a solid rectangle on top of the chosen region. + * Simply a visual aid. Serves no functional purpose. + */ + Imgproc.rectangle( + input, // Buffer to draw on + region2_pointA, // First point which defines the rectangle + region2_pointB, // Second point which defines the rectangle + GREEN, // The color the rectangle is drawn in + -1 + ); // Negative thickness means solid fill + } else if ( + max == avg3 + ) { // Was it from region 3? + position = SkystonePosition.RIGHT; // Record our analysis + + /* + * Draw a solid rectangle on top of the chosen region. + * Simply a visual aid. Serves no functional purpose. + */ + Imgproc.rectangle( + input, // Buffer to draw on + region3_pointA, // First point which defines the rectangle + region3_pointB, // Second point which defines the rectangle + GREEN, // The color the rectangle is drawn in + -1 + ); // Negative thickness means solid fill + } + + /* + * Render the 'input' buffer to the viewport. But note this is not + * simply rendering the raw camera feed, because we called functions + * to add some annotations to this buffer earlier up. + */ + return input; + } + + /* + * Call this from the OpMode thread to obtain the latest analysis + */ + public SkystonePosition getAnalysis() { + return position; + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java new file mode 100644 index 00000000..e2bf55f1 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SampleMecanumDrive.java @@ -0,0 +1,496 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import androidx.annotation.NonNull; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.MecanumDrive; +import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; +import com.technototes.path.util.AxesSigns; +import com.technototes.path.util.BNO055IMUUtil; +import com.technototes.path.util.DashboardUtil; +import com.technototes.path.util.LynxModuleUtil; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.LinkedList; +import java.util.List; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; + +/* + * Simple mecanum drive hardware implementation for REV hardware. + */ +@SuppressWarnings("unused") +@Disabled +@Config +public class SampleMecanumDrive extends MecanumDrive { + + /* + * These are motor constants that should be listed online for your motors. + */ + public static final double TICKS_PER_REV = 28; + public static final double MAX_RPM = 6000; + + /* + * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. + * Set this flag to false if drive encoders are not present and an alternative localization + * method is in use (e.g., tracking wheels). + * + * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients + * from DriveVelocityPIDTuner. + */ + public static final boolean RUN_USING_ENCODER = true; + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( + 10, + 0, + 0, + getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) + ); + + /* + * These are physical constants that can be determined from your robot (including the track + * width; it will be tune empirically later although a rough estimate is important). Users are + * free to chose whichever linear distance unit they would like so long as it is consistently + * used. The default values were selected with inches in mind. Road runner uses radians for + * angular distances although most angular parameters are wrapped in Math.toRadians() for + * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. + */ + public static double WHEEL_RADIUS = 1.88976; // in + public static double GEAR_RATIO = 1 / 15.6; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 16.4; // in + + /* + * These are the feedforward parameters used to model the drive motor behavior. If you are using + * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive + * motor encoders or have elected not to use them for velocity control, these values should be + * empirically tuned. + */ + public static double kV = 1.0 / rpmToVelocity(MAX_RPM); + public static double kA = 0; + public static double kStatic = 0; + + /* + * These values are used to generate the trajectories for you robot. To ensure proper operation, + * the constraints should never exceed ~80% of the robot's actual capabilities. While Road + * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start + * small and gradually increase them later after everything is working. All distance units are + * inches. + */ + public static double MAX_VEL = 40; + public static double MAX_ACCEL = 30; + public static double MAX_ANG_VEL = Math.toRadians(60); + public static double MAX_ANG_ACCEL = Math.toRadians(60); + + public static double encoderTicksToInches(double ticks) { + return (WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks) / TICKS_PER_REV; + } + + public static double rpmToVelocity(double rpm) { + return (rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS) / 60.0; + } + + public static double getMotorVelocityF(double ticksPerSecond) { + // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx + return 32767 / ticksPerSecond; + } + + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); + + public static double LATERAL_MULTIPLIER = 1; + + public static double VX_WEIGHT = 1; + public static double VY_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1; + + public static int POSE_HISTORY_LIMIT = 100; + + public enum Mode { + IDLE, + TURN, + FOLLOW_TRAJECTORY, + } + + private FtcDashboard dashboard; + private NanoClock clock; + + private Mode mode; + + private PIDFController turnController; + private MotionProfile turnProfile; + private double turnStart; + + private TrajectoryVelocityConstraint velConstraint; + private TrajectoryAccelerationConstraint accelConstraint; + private TrajectoryFollower follower; + + private LinkedList poseHistory; + + public DcMotorEx leftFront, leftRear, rightRear, rightFront; + private List motors; + private BNO055IMU imu; + + private VoltageSensor batteryVoltageSensor; + + private Pose2d lastPoseOnTurn; + + public SampleMecanumDrive(HardwareMap hardwareMap) { + super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER); + dashboard = FtcDashboard.getInstance(); + dashboard.setTelemetryTransmissionInterval(25); + + clock = NanoClock.system(); + + mode = Mode.IDLE; + + turnController = new PIDFController(HEADING_PID); + turnController.setInputBounds(0, 2 * Math.PI); + + velConstraint = new MinVelocityConstraint( + Arrays.asList( + new AngularVelocityConstraint(MAX_ANG_VEL), + new MecanumVelocityConstraint(MAX_VEL, TRACK_WIDTH) + ) + ); + accelConstraint = new ProfileAccelerationConstraint(MAX_ACCEL); + follower = new HolonomicPIDVAFollower( + TRANSLATIONAL_PID, + TRANSLATIONAL_PID, + HEADING_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), + 0.5 + ); + + poseHistory = new LinkedList<>(); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + imu = hardwareMap.get(BNO055IMU.class, "imu"); + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; + imu.initialize(parameters); + + BNO055IMUUtil.remapAxes(imu, AxesOrder.YXZ, AxesSigns.NNN); + + rightFront = hardwareMap.get(DcMotorEx.class, "frMotor"); + leftFront = hardwareMap.get(DcMotorEx.class, "flMotor"); + leftRear = hardwareMap.get(DcMotorEx.class, "rlMotor"); + rightRear = hardwareMap.get(DcMotorEx.class, "rrMotor"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + if (RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + } + + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + //setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap)); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, velConstraint, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, velConstraint, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, velConstraint, accelConstraint); + } + + public void turnAsync(double angle) { + double heading = getPoseEstimate().getHeading(); + + lastPoseOnTurn = getPoseEstimate(); + + turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( + new MotionState(heading, 0, 0, 0), + new MotionState(heading + angle, 0, 0, 0), + MAX_ANG_VEL, + MAX_ANG_ACCEL + ); + + turnStart = clock.seconds(); + mode = Mode.TURN; + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + follower.followTrajectory(trajectory); + mode = Mode.FOLLOW_TRAJECTORY; + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public Pose2d getLastError() { + switch (mode) { + case FOLLOW_TRAJECTORY: + return follower.getLastError(); + case TURN: + return new Pose2d(0, 0, turnController.getLastError()); + case IDLE: + return new Pose2d(); + } + throw new AssertionError(); + } + + public void update() { + updatePoseEstimate(); + + Pose2d currentPose = getPoseEstimate(); + Pose2d lastError = getLastError(); + + poseHistory.add(currentPose); + + if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) { + poseHistory.removeFirst(); + } + + TelemetryPacket packet = new TelemetryPacket(); + Canvas fieldOverlay = packet.fieldOverlay(); + + packet.put("mode", mode); + + packet.put("x", currentPose.getX()); + packet.put("y", currentPose.getY()); + packet.put("heading (deg)", Math.toDegrees(currentPose.getHeading())); + + packet.put("xError", lastError.getX()); + packet.put("yError", lastError.getY()); + packet.put("headingError (deg)", Math.toDegrees(lastError.getHeading())); + + switch (mode) { + case IDLE: + // do nothing + break; + case TURN: { + double t = clock.seconds() - turnStart; + + MotionState targetState = turnProfile.get(t); + + turnController.setTargetPosition(targetState.getX()); + + double correction = turnController.update(currentPose.getHeading()); + + double targetOmega = targetState.getV(); + double targetAlpha = targetState.getA(); + setDriveSignal( + new DriveSignal( + new Pose2d(0, 0, targetOmega + correction), + new Pose2d(0, 0, targetAlpha) + ) + ); + + Pose2d newPose = lastPoseOnTurn.copy( + lastPoseOnTurn.getX(), + lastPoseOnTurn.getY(), + targetState.getX() + ); + + fieldOverlay.setStroke("#4CAF50"); + DashboardUtil.drawRobot(fieldOverlay, newPose); + + if (t >= turnProfile.duration()) { + mode = Mode.IDLE; + setDriveSignal(new DriveSignal()); + } + + break; + } + case FOLLOW_TRAJECTORY: { + setDriveSignal(follower.update(currentPose, getPoseVelocity())); + + Trajectory trajectory = follower.getTrajectory(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke("#4CAF50"); + DashboardUtil.drawSampledPath(fieldOverlay, trajectory.getPath()); + double t = follower.elapsedTime(); + DashboardUtil.drawRobot(fieldOverlay, trajectory.get(t)); + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory); + + if (!follower.isFollowing()) { + mode = Mode.IDLE; + setDriveSignal(new DriveSignal()); + } + + break; + } + } + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawRobot(fieldOverlay, currentPose); + + dashboard.sendTelemetryPacket(packet); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) { + update(); + } + } + + public boolean isBusy() { + return mode != Mode.IDLE; + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, + coefficients.i, + coefficients.d, + (coefficients.f * 12) / batteryVoltageSensor.getVoltage() + ); + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if ( + Math.abs(drivePower.getX()) + + Math.abs(drivePower.getY()) + + Math.abs(drivePower.getHeading()) > + 1 + ) { + // re-normalize the powers according to the weights + double denom = + VX_WEIGHT * Math.abs(drivePower.getX()) + + VY_WEIGHT * Math.abs(drivePower.getY()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + VY_WEIGHT * drivePower.getY(), + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + List wheelPositions = new ArrayList<>(); + for (DcMotorEx motor : motors) { + wheelPositions.add(encoderTicksToInches(motor.getCurrentPosition())); + } + return wheelPositions; + } + + @Override + public List getWheelVelocities() { + List wheelVelocities = new ArrayList<>(); + for (DcMotorEx motor : motors) { + wheelVelocities.add(encoderTicksToInches(motor.getVelocity())); + } + return wheelVelocities; + } + + @Override + public void setMotorPowers(double v, double v1, double v2, double v3) { + leftFront.setPower(v); + leftRear.setPower(v1); + rightRear.setPower(v2); + rightFront.setPower(v3); + } + + @Override + public double getRawExternalHeading() { + return imu.getAngularOrientation().firstAngle; + } + + @Override + public Double getExternalHeadingVelocity() { + // TODO: This must be changed to match your configuration + // | Z axis + // | + // (Motor Port Side) | / X axis + // ____|__/____ + // Y axis / * | / /| (IO Side) + // _________ /______|/ // I2C + // /___________ // Digital + // |____________|/ Analog + // + // (Servo Port Side) + // + // The positive x axis points toward the USB port(s) + // + // Adjust the axis rotation rate as necessary + // Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying + // flat on a surface + + return (double) imu.getAngularVelocity().xRotationRate; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java new file mode 100644 index 00000000..11b28d5d --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/SplineTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.technototes.library.hardware.HardwareDevice; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/* + * This is an example of a more complex path to really test the tuning. + */ +@Disabled +@Autonomous(group = "drive") +public class SplineTest extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); + // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + Trajectory traj = drive + .trajectoryBuilder(new Pose2d()) + .splineTo(new Vector2d(70, 10), 0) + .build(); + + drive.followTrajectory(traj); + sleep(2000); + + drive.followTrajectory( + drive + .trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + ); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java new file mode 100644 index 00000000..06974ced --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StrafeTest.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.technototes.library.hardware.HardwareDevice; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class StrafeTest extends LinearOpMode { + + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); + // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()).strafeRight(DISTANCE).build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java new file mode 100644 index 00000000..bd1cb059 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/StraightTest.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.technototes.library.hardware.HardwareDevice; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class StraightTest extends LinearOpMode { + + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); + // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()).forward(DISTANCE).build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java new file mode 100644 index 00000000..051bc940 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/auto/rr/TurnTest.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.auto.rr; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.technototes.library.hardware.HardwareDevice; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; + +/* + * This is a simple routine to test turning capabilities. + */ +@Disabled +@Config +@Autonomous(group = "drive") +public class TurnTest extends LinearOpMode { + + public static double ANGLE = 90; // deg + + @Override + public void runOpMode() throws InterruptedException { + HardwareDevice.hardwareMap = hardwareMap; + + DrivebaseSubsystem drive = new DrivebaseSubsystem(new Hardware()); + // SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + drive.turn(Math.toRadians(ANGLE)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java deleted file mode 100644 index e1562808..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.command.CommandScheduler; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import com.technototes.library.util.Alliance; - -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.Setup; -import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; -import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; -import org.firstinspires.ftc.ptechnodactyl.controllers.OperatorController; - -@TeleOp(name = "PT Driving w/Turbo!") -@SuppressWarnings("unused") -public abstract class JustDrivingTeleOp extends CommandOpMode implements Loggable { - - public Robot robot; - public DriverController controlsDriver; - public OperatorController controlsOperator; - public Hardware hardware; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - controlsOperator = new OperatorController(codriverGamepad, robot); - if (Setup.Connected.DRIVEBASE) { - controlsDriver = new DriverController(driverGamepad, robot); - CommandScheduler.scheduleForState( - EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), - OpModeState.INIT - ); - } - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java deleted file mode 100644 index a6501b33..00000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/PlainDrive.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.technototes.library.logger.Loggable; -import com.technototes.library.structure.CommandOpMode; -import org.firstinspires.ftc.ptechnodactyl.Hardware; -import org.firstinspires.ftc.ptechnodactyl.Robot; -import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; - -@TeleOp(name = "PlainDrive") -@SuppressWarnings("unused") -public class PlainDrive extends CommandOpMode implements Loggable { - - public Hardware hardware; - public Robot robot; - - public DriverController driver; - - @Override - public void uponInit() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - hardware = new Hardware(hardwareMap); - robot = new Robot(hardware); - driver = new DriverController(driverGamepad, robot); - } -} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java new file mode 100644 index 00000000..122fe08c --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/TeleOpBase.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.command.WaitCommand; +import com.technototes.library.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.util.Alliance; +import org.firstinspires.ftc.ptechnodactyl.Controls; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; + +public abstract class TeleOpBase extends CommandOpMode implements Loggable { + + public Robot robot; + public Hardware hardware; + public Controls controls; + + @Override + public void uponInit() { + RobotConstants.updateAlliance(Alliance.get(getClass())); + hardware = new Hardware(); + robot = new Robot(hardware); + controls = new Controls(robot, driverGamepad, codriverGamepad); + } + + @Override + public void uponStart() { + if (Robot.SubsystemConstants.CAP_ENABLED) robot.capSubsystem.up(); + if (Robot.SubsystemConstants.DRIVE_ENABLED) { + robot.drivebaseSubsystem.resetGyro(); + robot.drivebaseSubsystem.setExternalHeading(Math.toRadians(180)); + } + } + + @TeleOp(name = "\uD83D\uDFE5 \uD83C\uDFAE Red TeleOp") + @Alliance.Red + public static class RedTeleOp extends TeleOpBase {} + + @TeleOp(name = "\uD83D\uDD35 \uD83C\uDFAE Blue Teleop") + @Alliance.Blue + public static class BlueTeleOp extends TeleOpBase {} +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java index 9f0eb4a6..b0a5443e 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java @@ -1,28 +1,27 @@ package org.firstinspires.ftc.ptechnodactyl.subsystems; -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.hardware.servo.ServoProfiler; -import com.technototes.library.subsystem.Subsystem; - -import java.util.function.Supplier; - +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.ARM_CONSTRAINTS; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.CARRY; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.COLLECT; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.ARM_CONSTRAINTS; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DIFFERENTIAL; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DOWN; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DUMP; -import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.DIFFERENTIAL; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.FAKE_CARRY; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.IN; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.OUT; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.SLIGHT_CARRY; import static org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem.ArmConstants.UP; -@SuppressWarnings("unused") +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.Range; +import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.ServoProfiler; +import com.technototes.library.subsystem.Subsystem; +import java.util.function.Supplier; +@SuppressWarnings("unused") public class ArmSubsystem implements Subsystem, Supplier { + /** * Deposit is am arm to hold and drop freight * @@ -31,22 +30,29 @@ public class ArmSubsystem implements Subsystem, Supplier { */ @Config public static class ArmConstants { + //public static double MIN = 0, MAX = 0.5; - public static double DUMP = 0.55, CARRY = 0.25, FAKE_CARRY = 0.15, COLLECT = 0.03, AUTO_CARRY = 0.3, SLIGHT_CARRY = 0.15; + public static double DUMP = 0.55, CARRY = 0.25, FAKE_CARRY = 0.15, COLLECT = + 0.03, AUTO_CARRY = 0.3, SLIGHT_CARRY = 0.15; public static double IN = 0.02, UP = 0.3, OUT = 0.6, DOWN = 0.75; public static double DIFFERENTIAL = 2.8; - public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints(5, 5, 5); + public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints( + 5, + 5, + 5 + ); } - public Servo dumpServo; - public Servo armServo; + public ServoGroup dumpServo; + public ServoGroup armServo; public ServoProfiler armController; - public ArmSubsystem(Servo l, Servo r) { + public ArmSubsystem(ServoGroup l, ServoGroup r) { dumpServo = l; armServo = r; - armController = new ServoProfiler(armServo).setConstraints(ARM_CONSTRAINTS).setTargetPosition(UP); - + armController = new ServoProfiler(armServo) + .setConstraints(ARM_CONSTRAINTS) + .setTargetPosition(UP); } /** diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java index 01dc57c5..7d8ec6bb 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java @@ -1,20 +1,21 @@ package org.firstinspires.ftc.ptechnodactyl.subsystems; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.subsystem.Subsystem; - import java.util.function.Supplier; public class BrakeSubsystem implements Subsystem, Supplier { + @com.acmerobotics.dashboard.config.Config public static class BrakeConstants { + public static double UP = 0.02, DOWN = 0.34; } private boolean isBraking; - public Servo servo; + public ServoGroup servo; - public BrakeSubsystem(Servo s) { + public BrakeSubsystem(ServoGroup s) { servo = s; isBraking = false; } @@ -34,5 +35,3 @@ public Boolean get() { return isBraking; } } - - diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java index b1d6ae8b..9261c949 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java @@ -4,69 +4,86 @@ import com.acmerobotics.dashboard.config.Config; import com.technototes.library.command.CommandScheduler; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.hardware.servo.ServoProfiler; import com.technototes.library.subsystem.Subsystem; - - import java.util.function.Supplier; - public class CapSubsystem implements Subsystem, Supplier { + @Config public static class CapConstants { - public static double TURRET_INIT = 0.8, TURRET_PICKUP = 0.1, TURRET_CARRY = 0.8, TURRET_CAP = 0.5; + + public static double TURRET_INIT = 0.8, TURRET_PICKUP = 0.1, TURRET_CARRY = + 0.8, TURRET_CAP = 0.5; public static double CLAW_OPEN = 0.1, CLAW_CLOSE = 0.53; public static double ARM_UP = 1, ARM_CAP = 0.85, ARM_INIT = 0.4, ARM_DOWN = 0.05; - public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints(5, 5, 5); - public static ServoProfiler.Constraints TURRET_CONSTRAINTS = new ServoProfiler.Constraints(3, 2, 2); - + public static ServoProfiler.Constraints ARM_CONSTRAINTS = new ServoProfiler.Constraints( + 5, + 5, + 5 + ); + public static ServoProfiler.Constraints TURRET_CONSTRAINTS = new ServoProfiler.Constraints( + 3, + 2, + 2 + ); } - public Servo armServo; - public Servo clawServo; - public Servo turretServo; + public ServoGroup armServo; + public ServoGroup clawServo; + + public ServoGroup turretServo; public ServoProfiler armProfiler; public ServoProfiler turretProfiler; - - public CapSubsystem(Servo arm, Servo claw, Servo turret){ + public CapSubsystem(ServoGroup arm, ServoGroup claw, ServoGroup turret) { CommandScheduler.register(this); armServo = arm; clawServo = claw; turretServo = turret; - armProfiler = new ServoProfiler(armServo).setServoRange(0.4).setConstraints(ARM_CONSTRAINTS).setTargetPosition(ARM_INIT); - turretProfiler = new ServoProfiler(turretServo).setConstraints(TURRET_CONSTRAINTS).setTargetPosition(TURRET_INIT); + armProfiler = new ServoProfiler(armServo) + .setServoRange(0.4) + .setConstraints(ARM_CONSTRAINTS) + .setTargetPosition(ARM_INIT); + turretProfiler = new ServoProfiler(turretServo) + .setConstraints(TURRET_CONSTRAINTS) + .setTargetPosition(TURRET_INIT); } - public void open(){ + public void open() { clawServo.setPosition(CLAW_OPEN); } - public void close(){ + public void close() { clawServo.setPosition(CLAW_CLOSE); } - public void up(){ + public void up() { armProfiler.setTargetPosition(ARM_UP); turretProfiler.setTargetPosition(TURRET_CARRY); clawServo.setPosition(CLAW_CLOSE); } - public void raise(){ + + public void raise() { armProfiler.setTargetPosition(ARM_CAP); } - public void raise2(){ + + public void raise2() { turretProfiler.setTargetPosition(TURRET_CAP); } - public void down(){ + + public void down() { armProfiler.setTargetPosition(ARM_DOWN); turretProfiler.setTargetPosition(TURRET_PICKUP); } - public void translateArm(double translation){ + + public void translateArm(double translation) { armProfiler.translateTargetPosition(translation); } - public void translateTurret(double translation){ + + public void translateTurret(double translation) { turretProfiler.translateTargetPosition(translation); } @@ -78,8 +95,13 @@ public void periodic() { @Override public String get() { - return "CLAW: "+clawServo.getPosition()+", ARM: "+armProfiler.getTargetPosition()+", TURRET: "+turretProfiler.getTargetPosition(); + return ( + "CLAW: " + + clawServo.getPosition() + + ", ARM: " + + armProfiler.getTargetPosition() + + ", TURRET: " + + turretProfiler.getTargetPosition() + ); } - } - diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java index 5ec27188..b907af21 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/DrivebaseSubsystem.java @@ -1,19 +1,32 @@ package org.firstinspires.ftc.ptechnodactyl.subsystems; +import static org.firstinspires.ftc.ptechnodactyl.Hardware.imu; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.TIP_AUTHORITY; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem.DriveConstants.TIP_TOLERANCE; + +import android.util.Pair; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.control.PIDCoefficients; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.Range; import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.sensor.IGyro; +import com.technototes.library.hardware.sensor.IMU; +import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; +import com.technototes.library.util.MapUtils; +import com.technototes.path.geometry.ConfigurablePose; +import com.technototes.path.subsystem.DistanceSensorLocalizer; import com.technototes.path.subsystem.MecanumConstants; import com.technototes.path.subsystem.PathingMecanumDrivebaseSubsystem; import java.util.function.Supplier; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.RobotConstants; import org.firstinspires.ftc.ptechnodactyl.Setup; import org.firstinspires.ftc.ptechnodactyl.helpers.HeadingHelper; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; public class DrivebaseSubsystem extends PathingMecanumDrivebaseSubsystem @@ -32,10 +45,10 @@ public abstract static class DriveConstants implements MecanumConstants { public static double AUTO_MOTOR_SPEED = 0.9; @TicksPerRev - public static final double TICKS_PER_REV = 537.7; // From GoBilda's website + public static final double TICKS_PER_REV = 28; // From GoBilda's website @MaxRPM - public static final double MAX_RPM = 312; + public static final double MAX_RPM = 6000; public static double MAX_TICKS_PER_SEC = (TICKS_PER_REV * MAX_RPM) / 60.0; @@ -44,9 +57,9 @@ public abstract static class DriveConstants implements MecanumConstants { @MotorVeloPID public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients( - 20, + 25, 0, - 3, + 4, MecanumConstants.getMotorVelocityF((MAX_RPM / 60) * TICKS_PER_REV) ); @@ -54,13 +67,13 @@ public abstract static class DriveConstants implements MecanumConstants { public static double WHEEL_RADIUS = 1.88976; // inches "roughly" lol @GearRatio - public static double GEAR_RATIO = 1.0; // output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 1 / 13.7; // output (wheel) speed / input (motor) speed @TrackWidth - public static double TRACK_WIDTH = 9.25; // inches + public static double TRACK_WIDTH = 9; // inches @WheelBase - public static double WHEEL_BASE = 9.25; // inches + public static double WHEEL_BASE = 8.5; // inches @KV public static double kV = @@ -74,11 +87,11 @@ public abstract static class DriveConstants implements MecanumConstants { // This was 60, which was too fast. Things slid around a lot. @MaxVelo - public static double MAX_VEL = 50; + public static double MAX_VEL = 60; // This was 35, which also felt a bit too fast. The bot controls more smoothly now @MaxAccel - public static double MAX_ACCEL = 30; + public static double MAX_ACCEL = 50; // This was 180 degrees @MaxAngleVelo @@ -92,7 +105,7 @@ public abstract static class DriveConstants implements MecanumConstants { public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0); @HeadPID - public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(4, 0, 0); @LateralMult public static double LATERAL_MULTIPLIER = 1.0; // For Mecanum, this was by 1.14 (14% off) @@ -109,6 +122,26 @@ public abstract static class DriveConstants implements MecanumConstants { @PoseLimit public static int POSE_HISTORY_LIMIT = 100; + public static double TIP_TOLERANCE = Math.toRadians(10); + + public static double TIP_AUTHORITY = 0.9; + + public static ConfigurablePose LEFT_SENSOR_POSE = new ConfigurablePose( + 0.5, + -5.5, + Math.toRadians(-90) + ); + public static ConfigurablePose RIGHT_SENSOR_POSE = new ConfigurablePose( + 0.5, + 5.5, + Math.toRadians(90) + ); + public static ConfigurablePose FRONT_SENSOR_POSE = new ConfigurablePose( + -5.7, + -2.5, + Math.toRadians(180) + ); + // Helps deal with tired motors public static double AFR_SCALE = 0.9; public static double AFL_SCALE = 0.9; @@ -117,6 +150,12 @@ public abstract static class DriveConstants implements MecanumConstants { } private static final boolean ENABLE_POSE_DIAGNOSTICS = true; + public Rev2MDistanceSensor left, right, front; + // protected FtcDashboard dashboard; + + public float xOffset, yOffset; + + public DistanceSensorLocalizer distanceSensorLocalizer; @Log(name = "Pose2d: ") public String poseDisplay = ENABLE_POSE_DIAGNOSTICS ? "" : null; @@ -147,7 +186,10 @@ public DrivebaseSubsystem( EncodedMotor frMotor, EncodedMotor rlMotor, EncodedMotor rrMotor, - IGyro imu + IMU imu, + Rev2MDistanceSensor l, + Rev2MDistanceSensor r, + Rev2MDistanceSensor f ) { super(flMotor, frMotor, rlMotor, rrMotor, imu, () -> DriveConstants.class); fl2 = flMotor; @@ -156,6 +198,19 @@ public DrivebaseSubsystem( rr2 = rrMotor; curHeading = imu.getHeading(); Setup.HardwareNames.COLOR = imu.getClass().toString(); + distanceSensorLocalizer = new DistanceSensorLocalizer( + imu, + MapUtils.of( + new Pair<>(l, DriveConstants.LEFT_SENSOR_POSE.toPose()), + new Pair<>(r, DriveConstants.RIGHT_SENSOR_POSE.toPose()), + new Pair<>(f, DriveConstants.FRONT_SENSOR_POSE.toPose()) + ) + ); + left = l; + right = r; + front = f; + + resetGyro(); } @Override @@ -164,9 +219,10 @@ public void periodic() { updatePoseEstimate(); Pose2d pose = getPoseEstimate(); Pose2d poseVelocity = getPoseVelocity(); - poseDisplay = pose.toString() + - " : " + - (poseVelocity != null ? poseVelocity.toString() : "nullv"); + poseDisplay = + pose.toString() + + " : " + + (poseVelocity != null ? poseVelocity.toString() : "nullv"); curHeading = this.gyro.getHeading(); Setup.HardwareNames.FLYWHEELMOTOR = this.gyro.getClass().toString(); } @@ -180,6 +236,55 @@ public void slow() { speed = DriveConstants.SLOW_MOTOR_SPEED; } + public DrivebaseSubsystem(Hardware hardware) { + this( + hardware.flDriveMotor, + hardware.frDriveMotor, + hardware.rlDriveMotor, + hardware.rrDriveMotor, + imu, + hardware.leftRangeSensor, + hardware.rightRangeSensor, + hardware.frontRangeSensor + ); + } + + public void relocalize() { + distanceSensorLocalizer.update(getPoseEstimate()); + setPoseEstimate(distanceSensorLocalizer.getPoseEstimate()); + } + + public void relocalizeUnsafe() { + distanceSensorLocalizer.update(); + setPoseEstimate(distanceSensorLocalizer.getPoseEstimate()); + } + + public void resetGyro() { + Orientation i = imu.getAngularOrientation(); + xOffset = i.secondAngle; + yOffset = i.thirdAngle; + distanceSensorLocalizer.setGyroOffset( + i.firstAngle - Math.toRadians(RobotConstants.getAlliance().selectOf(-90, 90)) + ); + } + + public void setSafeDrivePower(Pose2d raw) { + Orientation i = imu.getAngularOrientation(); + float x = 0, y = 0, adjX = xOffset - i.secondAngle, adjY = i.thirdAngle - yOffset; + if (Math.abs(adjY) > TIP_TOLERANCE) y = adjY; + if (Math.abs(adjX) > TIP_TOLERANCE) x = adjX; + // setWeightedDrivePower(raw.plus(new Pose2d(x>0 ? Math.max(x-TIP_TOLERANCE, 0) : Math.min(x+TIP_TOLERANCE, 0), y>0 ? Math.max(y-TIP_TOLERANCE, 0) : Math.min(y+TIP_TOLERANCE, 0), 0))); + setWeightedDrivePower( + raw.plus( + new Pose2d( + Range.clip(x * 2, -TIP_AUTHORITY, TIP_AUTHORITY), + Range.clip(y * 2, -TIP_AUTHORITY, TIP_AUTHORITY), + 0 + ) + ) + ); + } + public void auto() { speed = DriveConstants.AUTO_MOTOR_SPEED; } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java new file mode 100644 index 00000000..c41d2fa0 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java @@ -0,0 +1,82 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.Range; +import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.subsystem.Subsystem; +import java.util.function.Supplier; + +@SuppressWarnings("unused") +public class ExtensionSubsystem implements Subsystem, Supplier { + + @Config + public static class ExtensionConstants { + + public static double SNAP_1 = 0, SNAP_2 = Math.PI; + public static double IN = 0.46, SHARED = 0.3, TELEOP_ALLIANCE = 0.35, STEAL_SHARED = + 0.2, LOW_GOAL_AUTO = 0, OUT = 0; + public static double LEFT = 0.1, CENTER = 0.5, RIGHT = 0.9, AUTO_LEFT = 0.25, AUTO_RIGHT = + 0.75; + } + + public ServoGroup slideServo; + public ServoGroup turretServo; + + public ExtensionSubsystem(ServoGroup s, ServoGroup t) { + slideServo = s; + turretServo = t; + } + + public void left() { + turretServo.setPosition(ExtensionConstants.LEFT); + } + + public void right() { + turretServo.setPosition(ExtensionConstants.RIGHT); + } + + public void center() { + turretServo.setPosition(ExtensionConstants.CENTER); + } + + public void setTurret(double v) { + turretServo.setPosition(v); + } + + public boolean isSlideOut() { + return slideServo.getPosition() < ExtensionConstants.IN - 0.05; + } + + public boolean isSlideIn() { + return !isSlideOut(); + } + + public void fullyIn() { + slideServo.setPosition(ExtensionConstants.IN); + } + + public void fullyOut() { + slideServo.setPosition(ExtensionConstants.OUT); + } + + public void setSlide(double v) { + slideServo.setPosition(Range.clip(v, ExtensionConstants.OUT, ExtensionConstants.IN)); + } + + public void translateSlide(double v) { + setSlide(slideServo.getPosition() + v); + } + + public void translateTurret(double v) { + setTurret(turretServo.getPosition() + v); + } + + /** + *Method to display the extension and dump value on the telemetry to the driver knows + * how much they are extending the arm and positioning the cup + */ + @Override + public String get() { + return "TURRET: " + turretServo.getPosition() + ", SLIDE: " + slideServo.getPosition(); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java new file mode 100644 index 00000000..a3bd9032 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/IntakeSubsystem.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.IntakeSubsystem.IntakeConstants.*; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.technototes.library.hardware.motor.Motor; +import com.technototes.library.hardware.sensor.ColorDistanceSensor; +import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.util.Range; +import java.util.function.Supplier; +import org.firstinspires.ftc.ptechnodactyl.RobotState; + +/** + * Subsystem holding methods used for Intake commands. Intake will be responsible for bringing + * freight in and out of our robot + */ +public class IntakeSubsystem implements Subsystem, Supplier { + + @Config + public static class IntakeConstants { + + public static double INTAKE_IN_SPEED = 1; + public static double INTAKE_OUT_SPEED = -1; + public static double INTAKE_STOP_SPEED = 0; + public static double INTAKE_IDLE_SPEED = 0.5; + + public static Range CUBE_RANGE = new Range(400, 2100); + public static Range DUCK_RANGE = new Range(400, 2100); + public static Range BALL_RANGE = new Range(400, 2100); + + public static double SENSOR_REFRESH_RATE = 50; + } + + public Motor motor; + + public ColorDistanceSensor sensor; + + public IntakeSubsystem(Motor m, ColorDistanceSensor s) { + motor = m; + sensor = s; + } + + /** + * Set the intake motor to run in at a constant speed + */ + public void in() { + motor.setSpeed(INTAKE_IN_SPEED); + } + + /** + * Set the intake motor to run out at a constant speed + */ + public void out() { + motor.setSpeed(INTAKE_OUT_SPEED); + } + + /** + * Set the intake motor to stop running + */ + public void stop() { + motor.setSpeed(INTAKE_STOP_SPEED); + } + + public void idle() { + motor.setSpeed(INTAKE_IDLE_SPEED); + } + + private double light; + + @Override + public String get() { + return RobotState.getFreight().toString(); + } + + ElapsedTime t = new ElapsedTime(); + + @Override + public void periodic() { + if (t.seconds() > 1 / SENSOR_REFRESH_RATE && motor.getSpeed() > 0) { + t.reset(); + light = sensor.getLight(); + RobotState.setFreight(parseFreight()); + } + } + + public RobotState.Freight parseFreight() { + if (CUBE_RANGE.inRange(light)) return RobotState.Freight.CUBE; + if (DUCK_RANGE.inRange(light)) return RobotState.Freight.DUCK; + if (BALL_RANGE.inRange(light)) return RobotState.Freight.BALL; + return RobotState.Freight.NONE; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java new file mode 100644 index 00000000..4979f8dd --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/LiftSubsystem.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.DEADZONE; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.LIFT_LOWER_LIMIT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.LIFT_UPPER_LIMIT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.LiftSubsystem.LiftConstants.PID; + +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.Range; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.subsystem.Subsystem; +import java.util.function.Supplier; + +@SuppressWarnings("unused") +public class LiftSubsystem implements Subsystem, Supplier { + + //cringe but otherwise code borks for some reason i am confused plz help + @com.acmerobotics.dashboard.config.Config + public static class LiftConstants { + + public static double LIFT_UPPER_LIMIT = 600.0; + public static double LIFT_LOWER_LIMIT = 0.0; + //300 for single slide + public static double COLLECT = 0, NEUTRAL = 150, LEVEL_1 = 100, LEVEL_2 = + 200, TELEOP_LEVEL_2 = 150, LEVEL_3 = 550; + + public static double DEADZONE = 10; + + public static PIDCoefficients PID = new PIDCoefficients(0.008, 0, 0.0005); + } + + public EncodedMotor liftMotor; + + public PIDFController pidController; + + public LiftSubsystem(EncodedMotor l) { + liftMotor = l; + pidController = new PIDFController(PID, 0, 0, 0, (x, y) -> 0.1); + } + + public void setLiftPosition(double pos) { + pidController.setTargetPosition(Range.clip(pos, LIFT_LOWER_LIMIT, LIFT_UPPER_LIMIT)); + } + + /** + * set the lift to top, with the float constant LIFT_UPPER_LIMIT + */ + public void liftToTop() { + setLiftPosition(LIFT_UPPER_LIMIT); + } + + /** + * set the lift to bottom, with the float constant LIFT_LOWER_LIMIT, which is 0 + */ + public void liftToBottom() { + setLiftPosition(LIFT_LOWER_LIMIT); + } + + /** + * basically update something every loop + * which is let the motor running until receive signal about stopping + */ + @Override + public void periodic() { + liftMotor.setSpeed(Range.clip(-pidController.update(-liftMotor.get()), -0.9, 0.2)); + } + + /** + * @return true when motor position reached around target position + * using something called dead-zone, so when the motor moved slightly over the target don't necessary go-back + */ + public boolean isAtTarget() { + return ( + Math.abs( + pidController.getTargetPosition() + + /*+ because lift is inverted*/liftMotor.getSensorValue() + ) < + DEADZONE + ); + } + + /** + * stop the pid controller + * indirectly tells the periodic() method to stop update, which make the boolean isFollowing false + */ + public void stop() { + pidController.reset(); + } + + /** + * @return motor position in double + */ + @Override + public Double get() { + return pidController.getTargetPosition(); + } + + public boolean isLifted() { + return pidController.getTargetPosition() > 100; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java new file mode 100644 index 00000000..7b4431c3 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java @@ -0,0 +1,56 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import static com.technototes.library.hardware.HardwareDevice.hardwareMap; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem.VisionConstants.HEIGHT; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem.VisionConstants.ROTATION; +import static org.firstinspires.ftc.ptechnodactyl.subsystems.VisionSubsystem.VisionConstants.WIDTH; + +import com.acmerobotics.dashboard.config.Config; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.LogConfig; +import com.technototes.library.logger.Loggable; +import com.technototes.library.subsystem.Subsystem; +import com.technototes.library.util.Color; +import com.technototes.vision.hardware.Webcam; +import org.firstinspires.ftc.ptechnodactyl.opmodes.TeleOpBase; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvWebcam; + +public class VisionSubsystem implements Subsystem, Loggable { + + @Config + public static class VisionConstants { + + public static int WIDTH = 320; + public static int HEIGHT = 240; + public static OpenCvCameraRotation ROTATION = OpenCvCameraRotation.UPSIDE_DOWN; + } + + @LogConfig.DenyList({ TeleOpBase.RedTeleOp.class, TeleOpBase.BlueTeleOp.class }) + @LogConfig.Run(duringRun = false, duringInit = true) + @Log.Number(name = "Barcode", color = Color.GREEN) + public BarcodePipeline barcodePipeline = new BarcodePipeline(); + + public Webcam camera; + + public VisionSubsystem(Webcam c) { + camera = c; + } + + public void startStreaming() { + camera.startStreaming(WIDTH, HEIGHT, ROTATION); + } + + public void startBarcodePipeline() { + camera.setPipeline(barcodePipeline); + camera.openCameraDeviceAsync(this::startStreaming, i -> startBarcodePipeline()); + } + + public void stopBarcodePipeline() { + camera.setPipeline(null); + camera.closeCameraDeviceAsync(() -> {}); + } +} diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java index 66e6a9a4..0adb7c6c 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java @@ -9,7 +9,7 @@ import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; import com.technototes.library.hardware.sensor.encoder.MotorEncoder; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Loggable; import java.util.List; import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; @@ -22,12 +22,12 @@ public class Hardware implements Loggable { public EncodedMotor fl, fr, rl, rr; public EncodedMotor suspend; public EncodedMotor suspend2; - public Servo clawservo; - public Servo wristservo; - public Servo linkservo; + public ServoGroup clawservo; + public ServoGroup wristservo; + public ServoGroup linkservo; public EncodedMotor slidemotor; - public Servo armservo; - public Servo bucketservo; + public ServoGroup armservo; + public ServoGroup bucketservo; public MotorEncoder odoF, odoR; /* Put other hardware here! */ @@ -50,13 +50,13 @@ public Hardware(HardwareMap hwmap) { rr = new EncodedMotor(Setup.HardwareNames.RR_DRIVE_MOTOR); } if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM) { - clawservo = new Servo(Setup.HardwareNames.CLAWSERVO); - wristservo = new Servo(Setup.HardwareNames.WRISTSERVO); - linkservo = new Servo(Setup.HardwareNames.LINKSERVO); + clawservo = new ServoGroup(Setup.HardwareNames.CLAWSERVO); + wristservo = new ServoGroup(Setup.HardwareNames.WRISTSERVO); + linkservo = new ServoGroup(Setup.HardwareNames.LINKSERVO); } if (Setup.Connected.VERTICALSLIDESUBSYSTEM) { - armservo = new Servo(Setup.HardwareNames.ARMSERVO); - bucketservo = new Servo(Setup.HardwareNames.BUCKETSERVO); + armservo = new ServoGroup(Setup.HardwareNames.ARMSERVO); + bucketservo = new ServoGroup(Setup.HardwareNames.BUCKETSERVO); slidemotor = new EncodedMotor<>(Setup.HardwareNames.SLIDEMOTOR); } if (Setup.Connected.ODOSUBSYSTEM) { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java index 6cd42278..a206c5be 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java @@ -2,7 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -48,9 +48,9 @@ public class HorizontalSlidesSubsystem implements Subsystem, Loggable { @Log(name = "horizontalSlide") public double slidePos; - public Servo wristServo; - public Servo clawServo; - public Servo linkServo; + public ServoGroup wristServo; + public ServoGroup clawServo; + public ServoGroup linkServo; private final boolean isHardware; //not currently used, is there a use for it or should we delete? public HorizontalSlidesSubsystem(Hardware hw) { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java index ec46ae1d..554ee5ed 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.Range; import com.technototes.library.command.WaitCommand; import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -49,8 +49,8 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable { @Log(name = "bucketTarget") public double bucketTargetPos; - public Servo bucketServo; - public Servo armServo; + public ServoGroup bucketServo; + public ServoGroup armServo; public EncodedMotor slideMotor; private boolean isHardware; public static PIDCoefficients slidePID = new PIDCoefficients(0.0047, 0.0, 0.0); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java index 74e3c934..0e473fa6 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java @@ -11,7 +11,7 @@ import com.technototes.library.hardware.sensor.AdafruitIMU; import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Loggable; import java.util.List; import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; @@ -25,7 +25,7 @@ public class Hardware implements Loggable { public IGyro imu; public EncodedMotor fl, fr, rl, rr, armL, armR; public IEncoder odoF, odoR; - public Servo retainer, jaw, wrist; + public ServoGroup retainer, jaw, wrist; public CRServo intake; public Motor suspend; public EncodedMotor rotate1, rotate2, slides; @@ -53,9 +53,9 @@ public Hardware(HardwareMap hwmap) { } if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM) { intake = new CRServo(Setup.HardwareNames.INTAKE); - retainer = new Servo(Setup.HardwareNames.RETAINER); - wrist = new Servo(Setup.HardwareNames.WRIST); - jaw = new Servo(Setup.HardwareNames.JAW); + retainer = new ServoGroup(Setup.HardwareNames.RETAINER); + wrist = new ServoGroup(Setup.HardwareNames.WRIST); + jaw = new ServoGroup(Setup.HardwareNames.JAW); } if (Setup.Connected.HANGSUBSYSTEM) { suspend = new Motor(Setup.HardwareNames.SUSPEND); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java index cccdc0dd..277be804 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/HangSubsystem.java @@ -2,11 +2,8 @@ import com.acmerobotics.dashboard.config.Config; import com.technototes.library.hardware.motor.Motor; -import com.technototes.library.hardware.servo.Servo; -import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.twenty403.Hardware; @Config diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java index 87b3a9b9..dbff8206 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java @@ -2,7 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.technototes.library.hardware.motor.CRServo; -import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.hardware.servo.ServoGroup; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -11,7 +11,7 @@ @Config public class KidShampooSubsystem implements Subsystem, Loggable { - private Servo retainer, jaw, wrist; + private ServoGroup retainer, jaw, wrist; private CRServo intake; private boolean isHardware; diff --git a/bun.lockb b/bun.lockb index a12a9672b0a55e2d699503d951075696fcc924e2..d520567bab84a5c0432681cddb4715c5ff6a06fb 100755 GIT binary patch delta 165 zcmeykkn!U}#tqxeWsD3M)EO8U_!t-(W?Am{>5FBsW@3yp&@`I`l_`76l>v!9h0c9-p4E4+>D_BUH8Z$5y z6s4AwWTqC~i8Bb=q`k8jC~vH1st1&b0_qY0npwNQbjQ1^KRyCwj6jx7&a*INwAj4B HB2WSV%Kb0G delta 162 zcmeykkn!U}#tqxeWem+3)EO8U_!t-(W?Am{>5FBsW@3yp&@uu2Z1t@E%2ZV+U8KuS9l|WhXcgfb)@3t)i%2)zr%qA;XNSYckFccJ}mXu_s z7Tt+62->8*vll3DtOtY)3{gN`B0w{1_m}Q?clF0dpo|g7#>sgWhKy#L7gz*J0053L BFN6R9 From ebc44d66ee9a589ffdcb918af24c9113258926a8 Mon Sep 17 00:00:00 2001 From: KOOLPOOL <141171610+Kooolpool@users.noreply.github.com> Date: Tue, 17 Jun 2025 16:55:42 -0700 Subject: [PATCH 3/4] arm subs got rid of servo group --- .../ftc/ptechnodactyl/subsystems/ArmSubsystem.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java index b0a5443e..5da6730a 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ArmSubsystem.java @@ -14,7 +14,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.hardware.servo.ServoProfiler; import com.technototes.library.subsystem.Subsystem; import java.util.function.Supplier; @@ -43,11 +43,11 @@ public static class ArmConstants { ); } - public ServoGroup dumpServo; - public ServoGroup armServo; + public Servo dumpServo; + public Servo armServo; public ServoProfiler armController; - public ArmSubsystem(ServoGroup l, ServoGroup r) { + public ArmSubsystem(Servo l, Servo r) { dumpServo = l; armServo = r; armController = new ServoProfiler(armServo) From 81279b33337177f420d4af3fea939fa4e1f303dd Mon Sep 17 00:00:00 2001 From: KOOLPOOL <141171610+Kooolpool@users.noreply.github.com> Date: Tue, 17 Jun 2025 17:14:11 -0700 Subject: [PATCH 4/4] arm subs got rid of servo group and for some reason it spread to 16750 and 20403 files weird right --- .../ftc/ptechnodactyl/Hardware.java | 62 ++++++++++--------- .../subsystems/BrakeSubsystem.java | 6 +- .../subsystems/CapSubsystem.java | 10 +-- .../subsystems/ExtensionSubsystem.java | 8 +-- .../subsystems/VisionSubsystem.java | 4 +- .../ftc/sixteen750/Hardware.java | 22 +++---- .../subsystems/HorizontalSlidesSubsystem.java | 8 +-- .../subsystems/VerticalSlidesSubsystem.java | 6 +- .../firstinspires/ftc/twenty403/Hardware.java | 10 +-- .../subsystems/KidShampooSubsystem.java | 4 +- 10 files changed, 72 insertions(+), 68 deletions(-) diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java index 027e0a89..d76edd63 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java @@ -3,6 +3,7 @@ import static org.firstinspires.ftc.ptechnodactyl.Hardware.HardwareConstants.*; import static org.firstinspires.ftc.ptechnodactyl.Robot.SubsystemConstants.*; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.hardware.motor.Motor; @@ -10,8 +11,7 @@ import com.technototes.library.hardware.sensor.IMU; import com.technototes.library.hardware.sensor.IMU.AxesSigns; import com.technototes.library.hardware.sensor.Rev2MDistanceSensor; -import com.technototes.library.hardware.servo; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Loggable; import com.technototes.vision.hardware.Webcam; import org.firstinspires.ftc.ptechnodactyl.subsystems.ArmSubsystem; @@ -57,11 +57,11 @@ public static class HardwareConstants { public EncodedMotor liftMotor; - public ServoGroup dumpServo; - public ServoGroup armServo; + public Servo dumpServo; + public Servo armServo; - public ServoGroup turretServo; - public ServoGroup slideServo; + public Servo turretServo; + public Servo slideServo; public EncodedMotor flDriveMotor; public EncodedMotor frDriveMotor; @@ -77,33 +77,33 @@ public static class HardwareConstants { public Motor carouselMotor; - public ServoGroup capServo; + public Servo capServo; public Webcam camera; - public ServoGroup brake; + public Servo brake; - public ServoGroup capLeftArmServo; - public ServoGroup capRightArmServo; - public ServoGroup capArmServos; + public Servo capLeftArmServo; + public Servo capRightArmServo; + public Servo capArmServos; - public ServoGroup capClawServo; - public ServoGroup capTurretServo; + public Servo capClawServo; + public Servo capTurretServo; public Hardware() { if (BRAKE_ENABLED) { - brake = new ServoGroup(BRAKE).startAt(BrakeSubsystem.BrakeConstants.UP); + brake = new Servo(BRAKE).startAt(BrakeSubsystem.BrakeConstants.UP); } if (LIFT_ENABLED) { liftMotor = new EncodedMotor(LIFT).brake().tare(); } if (ARM_ENABLED) { - dumpServo = new ServoGroup(DUMP).invert().startAt(ArmSubsystem.ArmConstants.CARRY); - armServo = new ServoGroup(ARM).startAt(ArmSubsystem.ArmConstants.UP); + dumpServo = new Servo(DUMP).invert().startAt(ArmSubsystem.ArmConstants.CARRY); + armServo = new Servo(ARM).startAt(ArmSubsystem.ArmConstants.UP); } if (EXTENSION_ENABLED) { - slideServo = new ServoGroup(SLIDE).startAt(ExtensionSubsystem.ExtensionConstants.IN); - turretServo = new ServoGroup(TURRET) + slideServo = new Servo(SLIDE).startAt(ExtensionSubsystem.ExtensionConstants.IN); + turretServo = new Servo(TURRET) .startAt(ExtensionSubsystem.ExtensionConstants.CENTER) .expandedRange(); } @@ -112,7 +112,11 @@ public Hardware() { frDriveMotor = new EncodedMotor<>(FR_MOTOR); rlDriveMotor = new EncodedMotor<>(RL_MOTOR); rrDriveMotor = new EncodedMotor<>(RR_MOTOR); - imu = new IMU(HardwareConstants.IMU).remapAxes(AxesOrder.YXZ, AxesSigns.NPP); + imu = new IMU( + IMU, + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD + ).remapAxesAndSigns(AxesOrder.YXZ, AxesSigns.NPP); leftRangeSensor = new Rev2MDistanceSensor(L_RANGE).onUnit(DistanceUnit.INCH); rightRangeSensor = new Rev2MDistanceSensor(R_RANGE).onUnit(DistanceUnit.INCH); frontRangeSensor = new Rev2MDistanceSensor(F_RANGE).onUnit(DistanceUnit.INCH); @@ -128,16 +132,16 @@ public Hardware() { intakeSensor = new ColorDistanceSensor(INTAKE_COLOR).onUnit(DistanceUnit.INCH); } if (CAP_ENABLED) { - capLeftArmServo = new ServoGroup("caparml").onRange(0.25, 0.65).invert(); - capRightArmServo = new ServoGroup("caparmr").onRange(0.35, 0.75); - capArmServos = new ServoGroup(capLeftArmServo, capRightArmServo).startAt( - CapSubsystem.CapConstants.ARM_INIT - ); - - capClawServo = new ServoGroup("capclaw").startAt(CapSubsystem.CapConstants.CLAW_CLOSE); - capTurretServo = new ServoGroup("capturr").startAt( - CapSubsystem.CapConstants.TURRET_INIT - ); + capLeftArmServo = new Servo("caparml") + .onRange(0.25, 0.65) + .invert() + .startAt(CapSubsystem.CapConstants.ARM_INIT); + capRightArmServo = new Servo("caparmr") + .onRange(0.35, 0.75) + .startAt(CapSubsystem.CapConstants.ARM_INIT); + + capClawServo = new Servo("capclaw").startAt(CapSubsystem.CapConstants.CLAW_CLOSE); + capTurretServo = new Servo("capturr").startAt(CapSubsystem.CapConstants.TURRET_INIT); } } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java index 7d8ec6bb..cad54e6c 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/BrakeSubsystem.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.ptechnodactyl.subsystems; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.subsystem.Subsystem; import java.util.function.Supplier; @@ -13,9 +13,9 @@ public static class BrakeConstants { } private boolean isBraking; - public ServoGroup servo; + public Servo servo; - public BrakeSubsystem(ServoGroup s) { + public BrakeSubsystem(Servo s) { servo = s; isBraking = false; } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java index 9261c949..d20e9640 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/CapSubsystem.java @@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config; import com.technototes.library.command.CommandScheduler; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.hardware.servo.ServoProfiler; import com.technototes.library.subsystem.Subsystem; import java.util.function.Supplier; @@ -30,15 +30,15 @@ public static class CapConstants { ); } - public ServoGroup armServo; - public ServoGroup clawServo; + public Servo armServo; + public Servo clawServo; - public ServoGroup turretServo; + public Servo turretServo; public ServoProfiler armProfiler; public ServoProfiler turretProfiler; - public CapSubsystem(ServoGroup arm, ServoGroup claw, ServoGroup turret) { + public CapSubsystem(Servo arm, Servo claw, Servo turret) { CommandScheduler.register(this); armServo = arm; clawServo = claw; diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java index c41d2fa0..6630e709 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ExtensionSubsystem.java @@ -2,7 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.subsystem.Subsystem; import java.util.function.Supplier; @@ -19,10 +19,10 @@ public static class ExtensionConstants { 0.75; } - public ServoGroup slideServo; - public ServoGroup turretServo; + public Servo slideServo; + public Servo turretServo; - public ExtensionSubsystem(ServoGroup s, ServoGroup t) { + public ExtensionSubsystem(Servo s, Servo t) { slideServo = s; turretServo = t; } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java index 7b4431c3..b49cb368 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/VisionSubsystem.java @@ -12,7 +12,7 @@ import com.technototes.library.subsystem.Subsystem; import com.technototes.library.util.Color; import com.technototes.vision.hardware.Webcam; -import org.firstinspires.ftc.ptechnodactyl.opmodes.TeleOpBase; +import org.firstinspires.ftc.ptechnodactyl.opmodes.tele.TeleOpBase; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; @@ -31,7 +31,7 @@ public static class VisionConstants { @LogConfig.DenyList({ TeleOpBase.RedTeleOp.class, TeleOpBase.BlueTeleOp.class }) @LogConfig.Run(duringRun = false, duringInit = true) - @Log.Number(name = "Barcode", color = Color.GREEN) + @Log.Number(name = "Barcode") public BarcodePipeline barcodePipeline = new BarcodePipeline(); public Webcam camera; diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java index 0adb7c6c..66e6a9a4 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java @@ -9,7 +9,7 @@ import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; import com.technototes.library.hardware.sensor.encoder.MotorEncoder; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Loggable; import java.util.List; import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; @@ -22,12 +22,12 @@ public class Hardware implements Loggable { public EncodedMotor fl, fr, rl, rr; public EncodedMotor suspend; public EncodedMotor suspend2; - public ServoGroup clawservo; - public ServoGroup wristservo; - public ServoGroup linkservo; + public Servo clawservo; + public Servo wristservo; + public Servo linkservo; public EncodedMotor slidemotor; - public ServoGroup armservo; - public ServoGroup bucketservo; + public Servo armservo; + public Servo bucketservo; public MotorEncoder odoF, odoR; /* Put other hardware here! */ @@ -50,13 +50,13 @@ public Hardware(HardwareMap hwmap) { rr = new EncodedMotor(Setup.HardwareNames.RR_DRIVE_MOTOR); } if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM) { - clawservo = new ServoGroup(Setup.HardwareNames.CLAWSERVO); - wristservo = new ServoGroup(Setup.HardwareNames.WRISTSERVO); - linkservo = new ServoGroup(Setup.HardwareNames.LINKSERVO); + clawservo = new Servo(Setup.HardwareNames.CLAWSERVO); + wristservo = new Servo(Setup.HardwareNames.WRISTSERVO); + linkservo = new Servo(Setup.HardwareNames.LINKSERVO); } if (Setup.Connected.VERTICALSLIDESUBSYSTEM) { - armservo = new ServoGroup(Setup.HardwareNames.ARMSERVO); - bucketservo = new ServoGroup(Setup.HardwareNames.BUCKETSERVO); + armservo = new Servo(Setup.HardwareNames.ARMSERVO); + bucketservo = new Servo(Setup.HardwareNames.BUCKETSERVO); slidemotor = new EncodedMotor<>(Setup.HardwareNames.SLIDEMOTOR); } if (Setup.Connected.ODOSUBSYSTEM) { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java index a206c5be..6cd42278 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/HorizontalSlidesSubsystem.java @@ -2,7 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.Range; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -48,9 +48,9 @@ public class HorizontalSlidesSubsystem implements Subsystem, Loggable { @Log(name = "horizontalSlide") public double slidePos; - public ServoGroup wristServo; - public ServoGroup clawServo; - public ServoGroup linkServo; + public Servo wristServo; + public Servo clawServo; + public Servo linkServo; private final boolean isHardware; //not currently used, is there a use for it or should we delete? public HorizontalSlidesSubsystem(Hardware hw) { diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java index 554ee5ed..ec46ae1d 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.util.Range; import com.technototes.library.command.WaitCommand; import com.technototes.library.hardware.motor.EncodedMotor; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -49,8 +49,8 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable { @Log(name = "bucketTarget") public double bucketTargetPos; - public ServoGroup bucketServo; - public ServoGroup armServo; + public Servo bucketServo; + public Servo armServo; public EncodedMotor slideMotor; private boolean isHardware; public static PIDCoefficients slidePID = new PIDCoefficients(0.0047, 0.0, 0.0); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java index 0e473fa6..74e3c934 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java @@ -11,7 +11,7 @@ import com.technototes.library.hardware.sensor.AdafruitIMU; import com.technototes.library.hardware.sensor.IGyro; import com.technototes.library.hardware.sensor.IMU; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Loggable; import java.util.List; import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; @@ -25,7 +25,7 @@ public class Hardware implements Loggable { public IGyro imu; public EncodedMotor fl, fr, rl, rr, armL, armR; public IEncoder odoF, odoR; - public ServoGroup retainer, jaw, wrist; + public Servo retainer, jaw, wrist; public CRServo intake; public Motor suspend; public EncodedMotor rotate1, rotate2, slides; @@ -53,9 +53,9 @@ public Hardware(HardwareMap hwmap) { } if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM) { intake = new CRServo(Setup.HardwareNames.INTAKE); - retainer = new ServoGroup(Setup.HardwareNames.RETAINER); - wrist = new ServoGroup(Setup.HardwareNames.WRIST); - jaw = new ServoGroup(Setup.HardwareNames.JAW); + retainer = new Servo(Setup.HardwareNames.RETAINER); + wrist = new Servo(Setup.HardwareNames.WRIST); + jaw = new Servo(Setup.HardwareNames.JAW); } if (Setup.Connected.HANGSUBSYSTEM) { suspend = new Motor(Setup.HardwareNames.SUSPEND); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java index dbff8206..87b3a9b9 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java @@ -2,7 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.technototes.library.hardware.motor.CRServo; -import com.technototes.library.hardware.servo.ServoGroup; +import com.technototes.library.hardware.servo.Servo; import com.technototes.library.logger.Log; import com.technototes.library.logger.Loggable; import com.technototes.library.subsystem.Subsystem; @@ -11,7 +11,7 @@ @Config public class KidShampooSubsystem implements Subsystem, Loggable { - private ServoGroup retainer, jaw, wrist; + private Servo retainer, jaw, wrist; private CRServo intake; private boolean isHardware;