From a6c4c5bdbcac662abf354662af0a860f16105a3e Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 18 Feb 2019 15:51:20 -0700 Subject: [PATCH] Clear Master --- 2019robot/.gitignore | 160 ---- 2019robot/.vscode/launch.json | 21 - 2019robot/.vscode/settings.json | 14 - 2019robot/.wpilib/wpilib_preferences.json | 6 - 2019robot/build.gradle | 61 -- 2019robot/gradle/wrapper/gradle-wrapper.jar | Bin 55741 -> 0 bytes .../gradle/wrapper/gradle-wrapper.properties | 5 - 2019robot/gradlew | 172 ---- 2019robot/gradlew.bat | 84 -- 2019robot/settings.gradle | 25 - 2019robot/src/main/deploy/example.txt | 3 - .../main/java/buttons/XBoxTriggerButton.java | 61 -- .../frc4388/controller/IHandController.java | 12 - .../frc4388/controller/XboxController.java | 205 ---- .../org/usfirst/frc4388/robot/Constants.java | 72 -- .../java/org/usfirst/frc4388/robot/Main.java | 22 - .../java/org/usfirst/frc4388/robot/OI.java | 101 -- .../java/org/usfirst/frc4388/robot/Robot.java | 161 ---- .../org/usfirst/frc4388/robot/RobotMap.java | 36 - .../frc4388/robot/commands/ArmBasic.java | 99 -- .../frc4388/robot/commands/ArmSetSpeed.java | 44 - .../robot/commands/DriveAbsoluteTurnMP.java | 42 - .../robot/commands/DriveGyroReset.java | 38 - .../robot/commands/DriveRelativeTurnPID.java | 38 - .../robot/commands/DriveSpeedShift.java | 40 - .../robot/commands/DriveStraightBasic.java | 103 -- .../robot/commands/DriveStraightBasicAbs.java | 115 --- .../robot/commands/DriveStraightMP.java | 60 -- .../robot/commands/DriveTurnBasic.java | 151 --- .../robot/commands/InitiateClimber.java | 38 - .../usfirst/frc4388/robot/subsystems/Arm.java | 424 --------- .../frc4388/robot/subsystems/Climber.java | 68 -- .../frc4388/robot/subsystems/Drive.java | 862 ----------------- .../frc4388/robot/subsystems/Pnumatics.java | 46 - .../AdaptivePurePursuitController.java | 228 ----- .../usfirst/frc4388/utility/BHRMathUtils.java | 48 - .../frc4388/utility/CANTalonEncoder.java | 65 -- .../frc4388/utility/ConstantsBase.java | 199 ---- .../frc4388/utility/ControlLoopable.java | 7 - .../frc4388/utility/ControlLooper.java | 79 -- .../frc4388/utility/FalconLinePlot.java | 894 ------------------ .../usfirst/frc4388/utility/Interpolable.java | 27 - .../frc4388/utility/InterpolatingTreeMap.java | 91 -- .../frc4388/utility/InverseInterpolable.java | 25 - .../usfirst/frc4388/utility/Kinematics.java | 59 -- .../frc4388/utility/MMTalonPIDController.java | 137 --- .../utility/MPSoftwarePIDController.java | 156 --- .../frc4388/utility/MPTalonPIDController.java | 233 ----- .../utility/MPTalonPIDPathController.java | 113 --- .../MPTalonPIDPathVelocityController.java | 111 --- .../frc4388/utility/MotionProfileBoxCar.java | 184 ---- .../frc4388/utility/MotionProfilePoint.java | 15 - .../usfirst/frc4388/utility/PIDParams.java | 62 -- .../org/usfirst/frc4388/utility/Path.java | 260 ----- .../frc4388/utility/PathGenerator.java | 232 ----- .../usfirst/frc4388/utility/PathSegment.java | 89 -- .../frc4388/utility/RigidTransform2d.java | 131 --- .../usfirst/frc4388/utility/Rotation2d.java | 124 --- .../utility/SoftwarePIDController.java | 114 --- .../SoftwarePIDPositionController.java | 85 -- .../frc4388/utility/Translation2d.java | 104 -- 2019robot/vendordeps/PathfinderOLD.json | 85 -- 2019robot/vendordeps/Phoenix.json | 87 -- 2019robot/vendordeps/REVRobotics.json | 32 - 2019robot/vendordeps/cw.json | 21 - 2019robot/vendordeps/navx_frc.json | 33 - 66 files changed, 7519 deletions(-) delete mode 100644 2019robot/.gitignore delete mode 100644 2019robot/.vscode/launch.json delete mode 100644 2019robot/.vscode/settings.json delete mode 100644 2019robot/.wpilib/wpilib_preferences.json delete mode 100644 2019robot/build.gradle delete mode 100644 2019robot/gradle/wrapper/gradle-wrapper.jar delete mode 100644 2019robot/gradle/wrapper/gradle-wrapper.properties delete mode 100644 2019robot/gradlew delete mode 100644 2019robot/gradlew.bat delete mode 100644 2019robot/settings.gradle delete mode 100644 2019robot/src/main/deploy/example.txt delete mode 100644 2019robot/src/main/java/buttons/XBoxTriggerButton.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java delete mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java delete mode 100644 2019robot/vendordeps/PathfinderOLD.json delete mode 100644 2019robot/vendordeps/Phoenix.json delete mode 100644 2019robot/vendordeps/REVRobotics.json delete mode 100644 2019robot/vendordeps/cw.json delete mode 100644 2019robot/vendordeps/navx_frc.json diff --git a/2019robot/.gitignore b/2019robot/.gitignore deleted file mode 100644 index 61f25ce..0000000 --- a/2019robot/.gitignore +++ /dev/null @@ -1,160 +0,0 @@ -# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Java ### -# Compiled class file -*.class - -# Log file -*.log - -# BlueJ files -*.ctxt - -# Mobile Tools for Java (J2ME) -.mtj.tmp/ - -# Package Files # -*.jar -*.war -*.nar -*.ear -*.zip -*.tar.gz -*.rar - -# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml -hs_err_pid* - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json - -### Windows ### -# Windows thumbnail cache files -Thumbs.db -ehthumbs.db -ehthumbs_vista.db - -# Dump file -*.stackdump - -# Folder config file -[Dd]esktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Windows Installer files -*.cab -*.msi -*.msix -*.msm -*.msp - -# Windows shortcuts -*.lnk - -### Gradle ### -.gradle -/build/ - -# Ignore Gradle GUI config -gradle-app.setting - -# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) -!gradle-wrapper.jar - -# Cache of project -.gradletasknamecache - -# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 -# gradle/wrapper/gradle-wrapper.properties - -# # VS Code Specific Java Settings -.classpath -.project -.settings/ -bin/ - - -# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2019robot/.vscode/launch.json b/2019robot/.vscode/launch.json deleted file mode 100644 index c9c9713..0000000 --- a/2019robot/.vscode/launch.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} diff --git a/2019robot/.vscode/settings.json b/2019robot/.vscode/settings.json deleted file mode 100644 index 860e319..0000000 --- a/2019robot/.vscode/settings.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "java.configuration.updateBuildConfiguration": "automatic", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - ".classpath": true, - ".project": true - }, - "wpilib.online": true -} diff --git a/2019robot/.wpilib/wpilib_preferences.json b/2019robot/.wpilib/wpilib_preferences.json deleted file mode 100644 index 95c6671..0000000 --- a/2019robot/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2019", - "teamNumber": 4388 -} \ No newline at end of file diff --git a/2019robot/build.gradle b/2019robot/build.gradle deleted file mode 100644 index ea2312b..0000000 --- a/2019robot/build.gradle +++ /dev/null @@ -1,61 +0,0 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" -} - -def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project EmbeddedTools. -deploy { - targets { - roboRIO("roborio") { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = frc.getTeamNumber() - } - } - artifacts { - frcJavaArtifact('frcJava') { - targets << "roborio" - // Debug can be overridden by command line, for use with VSCode - debug = frc.getDebugOrDefault(false) - } - // Built in artifact to deploy arbitrary files to the roboRIO. - fileTreeArtifact('frcStaticFileDeploy') { - // The directory below is the local directory to deploy - files = fileTree(dir: 'src/main/deploy') - // Deploy to RoboRIO target, into /home/lvuser/deploy - targets << "roborio" - directory = '/home/lvuser/deploy' - } - } -} - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Maven central needed for JUnit -repositories { - mavenCentral() -} - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. -dependencies { - compile wpi.deps.wpilib() - compile wpi.deps.vendor.java() - nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) - nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) - testCompile 'junit:junit:4.12' -} - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// knows where to look for our Robot Class. -jar { - from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) -} diff --git a/2019robot/gradle/wrapper/gradle-wrapper.jar b/2019robot/gradle/wrapper/gradle-wrapper.jar deleted file mode 100644 index 457aad0d98108420a977756b7145c93c8910b076..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 55741 zcmafab95)swq`n=q+{FmFHU~x*tTukPP${;?%1|%+qP{@&)oNB=H9vQ&04jq>W_Wa zIlI<5`};OZPVze#DhLQ9BnSuy|6c(C0sUWh5D=)pPibK#et@`)2>o{uxqnjRs=Jxt z{;Qz$SN;zFKZ?@)GU6h_ib{0SB6rf`V^Wd;x*0f00QKbfRGk9DJoEO!?Kogqd_sDH zMx6E=^l6Y$(tf@MRWk-z;eNisaBoAJU;VAaw||-L?+pKYU0{FTZ5>SipC$d@IxzpC zW9p!9WM%x{s-pa}s;h&(ot?46|1+d?#bo(AI0(q;-_HX0_d^71ZJivQ{*IT{H@8uA z(syt&cAzt~(sy)>RMAvLQAPcXN;T5M1vljL5Az2i(}gxHs#MoYbP#?6e6tc-gC8M^ zkTIDZ>6g61@cj7E`B)&UUFHo{U%9%l^cnsc&SULwSS4O*-%KUb|_H^O>xPN8Z z#S3l+%}W`w>*x1PYSc|~P#CPe%5Vh$vP%ZeWR_ddk&@H`e;Mmq54Ji zSmXpxw=H0DJ^CDtBZ&lDUfppUSM5SFF*x#{r&n>rLtV-KC#Rn*-)OUabqwnojq;L z{O(6h(7MwPM$FJV1e_o4W&y5;tE<6*ffr%;YmD=kx&J@e6uZVwk1 z+sP~2BRo+VpQa}0<|e2Q#kHPyglj2rbqkJaUQ2~&7AxEk@faL$qUvgP3f7lBV`j?@ zfZoR!YVY`oqGj)z#n@(|pSsVfp|^M!?mBwq>`HnGn|Lwg}^{8bgNZDBdl)U zjOs}YG(F4YKXUhNf3<7FzBQc*w(_To!i~598rUS38!!$FLso@qZ#<+Tff$~pm8mPa zsYDZ5NeEedFt zgR&?))b^pY-dBZKufVo=>p@}NUGND8s9ft&tC{RVJ7JPo-l;My{+b&YtZ*|ZSs zS(bNl>#J2B{g)2QOKEtVrm)g{dov@fs3wF2ju5_ug_b>Cp%HOQL8_4eds+C&`uM{j z*p*FUthBi6Nm$HKmJ8P==^vU#K?wlgTFfHEvB-YoB*sZR7vGl24F)I*Nz9_*eNrJs zo~fG|+_~!NdAGd=NtPY2%yj81G^UVW)_esfF@VYBdp<>c1VUuF{?Iu}fWs_7j6=>0 zb}y???OBZFv6|pfN~608|Cx2UVFs8`x)LyAFW<}TE0H(UX4E}vF~WSJs!6P zzz8G+#34!rFj9dooY{Jq*a7g2eo52=mWrZlCSP-RLIuV#>!UTh?MakKGZux8oHMOF>uR_wSn4&-VbI8qQj4OTR_Xr zZ#ifgIWA7g6NrJ|Ef`9*yNGoNhtTgkxej-i1(sJVt|E!;^v zpp8h*8E<|DlNVLnxtc*lG+wT#^)uFT1c#zjg5z@xpb{|CgL%TH;YQC*WL?&bE^;$h zkf1J|@@X!1MCf##X`=>)k%QqA;4{sxg^X$8%kudzkhM73%7^n8OQrte70ds}L^%gr zH+Oz#Co^LkCv!u6C)@wPjYLIlSrmR$?mB>#cB7iAweY?m-+Hb~xbDn+N(?q~yoHz? za^Kvv%qr`c40EQ&Yq-?(4{xO0P~L6=<90=8wrSLj;LKFdsh>~lEvdR)U0+~!1krd> zdW*i{>Nt*6wX~LgI|gJC z180wyQF6A=GD1jI#ARHoyVNX4J`5!|`r!^Z5|_?PtF1%^cq%PPVLjehLjs8;n-);L zs5A--{;j(WVqka!>C!oV7_}&(aZ5>)&oFI|lFZoZ{ zNQP?%K`~>HL6x+@j`Nxsw(1yKn8{t}zWh>C>*jKNhdChQs!pBy!f!L|^Q zHIA9|&XKJ4b9G>297=XG`Ga+UaDdm(!ZI{EfSl1-(;6M~Zo>HVlcl`egIwDsE|S&` zKa5mSS~#>2h_&Cv1!LX~a=#&#*f!gN9F2u$h_dKq8}ZJ(FpHlfh#f`cL!}HV>Aq4` zsBB^2T+(=g9ltTPouypP*xX+;H_y^o%b&Sx1^HV-B#D1~`{(n7aw^T55C4}J;emh< z{r8*gAEzl(9ZC;%$^FyEyn$IGGC*ze;MTw&;DjB~gU-f=oXiKABTNi4_(&L;AGi?3 z#R)?xy~_@$NDrtt-ytpB@B~`N;1T0;z5phLr$xmESr)?=tTm@HS{nFb;fP0sRl)OGb?0w!t0ZGu4U2-@D_a6BAX$&d5ZAtWw_Nl@8OHtBB7kJY|^p5wr+0C2sJcP)$oo(^w zlLeGq2Vp1sCX(YF=94u*@)?NoRcZ1mn0YVFdMEw;t7QL$e!)wU^{XQ7OMaqB`3`TP zJ;&&^GW05t;Ww6|UKpkVTCa@POoD|E86R?qd{VEH=`_t%0 z-czLo{rH+V1x7F)$Q5LCO?DIZ@ET;v8t5*oGjVG>0u+5QTr>bA`M2%ZqfbmAj~N6mXe z1cYcOFeWa|G~L<>F2-1zeSMi-8Is)`Ia5ZhyEM>7t^@l6=rPEzkPWrF4e%X{BSH4J4 z4h05<8@6g;HBhjgBJ;?sn47LGa+m05egguPL&_yy*4YoATckIMh6r5E$CSyOvY69I zo1)gSPXMAQ7-o^{(=?*teMK^k@wcsMZ6DQ5LyvJ#!VTpCrI=#iy%%Hw{1@sjDhcP) z$IE3H$B7eu15&3uQnQblJ-K8y<>xUD4Gjkh*oWid_#TrF#nR2^?06M^1=47;kR>fd z6s)5|y%6HY^ z81d-%ex^hf*aTQGSKWOqTg_Lpjk46$a&b7d195BD=KOD<*Y*0JXo5Vo3o&be74kJe zi{NE_zM_ko8|qch zEXMt6vJVHL>im?Q;P4xY5Y(uMth%|GxX23$H5)~9UOjQCC~PLPkr>>%eE>2>cif&s ze;mpvoQnCjx&whu{eHg1rK$tZ9}k4mL6bg5S#`H>s$i_6p5~H4fsAfD%ay*JS)JZ9 zJuKe=tzXA8pT#1PW0f5vmH2LoY)TAj`I^x2}g?1 z{6Wo0QlowtCe?Ego$>!%#EkzeH$H&td zV}?4|ggqm1GdPJ2(eTq`NTxn|jTW!(f-CW?WDjv4G)It`Dx+HvTJoj`v(oU6_ATVD z+ZnhF+Ra`3M1`wSWwW-_)9hf}sA0sjUQOA|`Ld=y(QuHNM3JO6a+|s|0Mxk99xTukx6Lb8HYZbPYrL}lVd_(>G*n;530UD+A5i`y9$!&e%iBK{&TT-Ax`&xA_5x9iAV(^;cfY{gc(RK;{W_CH3IKaa?~ApY>L zJ>D0mT68M^cfMHR#)m>c#T^_^?@>dU%EU{gPFoWnAk@xZot$H<&j%nM^WDQrQzjne8* z?vXL#QSXc?^iC|g9kPDs4WMR_Tl;RC>r-vDS!8|tT!ZfUgQ>B+EVX{ zqM%M`oD~$pDz_X}?NV3Q5f^-bu-0K02^-G#%mq5r@6(`p*U83h7oSNlf8wH1tCjje zyHl4BVc+v_6NV^@ZL_?Yf+!J<+z5@Z2UdGfRTHR0gMUaLb$Kye;}Bn{XBc{B|DYEz)jgnch&U z&R;z(vdZR_wj4m0rms~JPUAvp@k=|Np@FsNjo)hj@V}7WZ}dH~7W!^8sIcw_nO2RS zUQes(NP%vwu9{$+sZuBLgg;wboE1mitkz$0np_uCBVPLgt8|rnd~ySm8oJJk##1UOtpPZ7A%daWDLwD!L0X&EAh?_BLG$(Z+1pp+ z28{EIhq1H!CL4FW4mSBWUy7O8chJ(l^jb5XLW;}_=L{$vQ8jOkq(C~_s$WO|{(9l@ zQ{x8pdLGU@{Ei|(_@mf_b4qL)EyrqQUMh1FPk1^9hO!QD-ot1{a_Z!dCtTH>QPm#K zYr5l!EMxg^&gM<=ur{;LHY`F;EVHNSnWtH*w(9TN?`x6x=lr~nv)gYB0Uww<<6!U% z-SA8?@H0f-&rm_LWD4B=B+u@!+wKZo0;fxeL&^5Ix&u&Na1=Mr>#|h4&*(>+-B3G* zd=_?HYSj&ITcYCF*GxXRSA~L2+fePn{FX6$;O!U~SkwqiZCgWHRb)-BF<0h=W)VBL z@ov1Q!#7;cp1h|eBW2I#UVH^*^BBFM9bW7;$uLFKC4Gj^gr&lPPBx;EDbI*JQe(y4 zKHv?^T+N%^*x%FPxNNrLe4{bldb>s8U@_}`!G=wPT3Qej%}THSSO&#G9HAg*nG{Q} zK+m^hx;X@mEVA1cTnB?$nH0+oI7-G1Ds}Y_n;!FP$QLZWS}x<-P8x()w?y)4zBqFJ zYP>OhW4UbB8ejRdY1az&eQ|G(<%Bj|Hs;nGLLh2@em}?A)NzdE^4!uB&G)D`6R@8+ z5G7gd1hO>%VgtA34ad1R*G%DMi{6$7)u;V#GBYurF_4Q(S4bHwsVF@ggRg?Pk_$W3 z7TQ#Y*-;ll#2JoRi;|u1Z%!KYS;RuuskrC7fK!|oorIpa%tze0=g2@M&IIRhKs-^y z8O|N7(dSvWjIpWv;eKyq|EG%8OD@v$JMK_fU$8;OIcz=D(pxS2x;hG!kVW${MwJcJ zlc{|GSg`J3xA7%csSAA4RRmZ*va{(ncF^vQA$0rZEwV66Dwyc;{I|%Hn)b*vb1XT- z_-~zY)D0*US4I+e`U-Fik%OsX2+X1Q`mdCp8=^4fqytmirtSiqwwc+6^xdh6QYHsI z_N(l`C&-S^@Mp3|2uQ@_6-77i8nd(T)k<1miq6yybAr5jGq^Idk#2=ye93h?^vN>p zRXm@47>Y-drcQU_JGgTw$-(%<4m*9Z;Le`an?}_hMDF^jSNpZc%EEH$Kxm^@yOxR5 zhHDEZ1rzU?fAo{6a3&baK3Rg;*irsT5;>4(MO1LFlmgM?7 zT=m94XR<0L6nZ>Ii^nZGg9x4MFdbw}|O1r`CAw%QZ%#Yc<3T||y< zn$o}91L-M3iW!~`sBtr_PK=yEZwrfEp&d8wiBGtrl6YoJVkIy&zJ@aCnJY)inrXe5 zf=XBdgcv@T|4oC5YR$C^Eb2|{VR?C2<0&ZxB_qD_PD%i6hEw{3DA^YT%Id-+vnDtvfN<=G= zCkDYvYP70#8p-u@oQRPDdl|wN#_F}`U#Pvu>0{~b>{`rG<&uO>(^0qr7&33umdn0x z{}R&Y`3J98LV$pH!GeG=|9g9d-3+aq9nD>gMa->?`3(*Knk=nvqi_137ALB%IpU~d zec`Fku9i7_nU|_eNXtNr$ygQa#1W?>4mBvg#5LFKEs~LHOHw=zmTA{$3@|>S3cv)~ zS^5Tn_Q81TrQ|RxiC@F+fT!$W1$F%%>b&lSg-zL7NyObkF2sL3|GD*YxpnpC@^SR_ z`RED~wPVV`6L7TlXAit#+}urG6g7L+j$agas;sYQ;W9T!vcgT=T3R?mRZ<>6)*uF# ztXcD?F`T1S?_}OZxKDw`3Qv3>OGsVxTU4vPv(j~{ikpOP1DX%$RBoXmtg18Zo(Ak` z(cb(Bn98*}T=v|xyYFOZUEf}^2n-5Ia^X~T6uJ)LaaOZ|4y$A13GIHN_ml%V1lVe@ zcF}2i6tCqfE7kO&rt!(*`$*|;O33rl=0-FEpeRi;fzc_)W-=0JT@Wp0SGqDa*Q5>nOZLwa6vQ;bXI0Z*6Q( z*hX|(cc*GJqdL0Z5Oz?((9hUv)HXoo+d-5k*fAhHXP>VP2g?vfmPpw$!%1#Zhe6F$ zC&BvTGnyB)VpOL%zpTZ3@b);hOzZSss%Sr{?^&>sV$nKt%|X;zqi@Z5ZjBfd9${bF zhHgZnKs||q;PPR6e7qYfF~EF_mQFe?kx!}*gT+g}FPX(lozL{eXrL*CqStIy!%nK} zQ2uFuTgt~1Nv6GuDG1)EWi}N2WD2n$KFOMfw!x^j+(30jD>b>VQ8RIBf%arg%J{Z3 za+??ojB!oH3LhMQ#W#3Hs*Sa3x%k2N2T7eS6wdLG^Y>u;em;UINK70-=4+--;_Kzb zOHSpw^SFChFEk*m7hDzOeMeO#;oR2WDsN)v;!V_8FTywhcruV2IE-!dgjO_OYw%~8wLyN*pc_G=oVny!A5c!SpU z*(ZC}JZWKzHD~Bq3R6+QSh=Z5&0}@pB`v|mwse7gpM0vex2)kIA)ZM&{EH7bV&M2* z+Hh0B>Sq9vu%ty*SKwZm*yMvVuM1A}kzt7%UWpeMTGd|YZ~Kd?l4rbg@quPJ;ju{0 z<+sPxtHiL*$iktWqJ6 zlhBcKtKE5cKZ;W*CH#}o+c`4A<4!*-B+jJFNaFI!8^@$XbZf3i|ZQnG| zdM;7e`QLH#CuQRn{UVO@f&LNG{aeR#RJkPqU3!>P*L%(ZyMwL!lw`8;OmG*x>?K5_ zTi%w-k!)}tc3y0x7Gf5Z*NhPFI7P1jxIN2rd}~o2hGlyxme(Wy6jBIyW*P68XqDz45>?m?|gzJG^zv@X?;KV z>MV?Kt{xw;Rsvr>moT|HxPJ}FP)@GyLO58lnD@D0S4kh&$A8Q zFP~3P27hpH`3P}0azm{iuw7(iXn~Z38?AC(p=m!VC~D;CsgVs90^qENpWbTCLB_r3nWXsI&ES-tyd`66jdPNz<+Cx& z=tSEhGXTW+&Idg)!Z%9zMhOEf2BQ?#pEU+1X@Oi%XWNG}b9d#Bz~AT?IgDO&S3Q-4~dc$g2h$IK9ey{tKD z{-A^klUva7RW*?s{jRBHUVROa&y;3i_tW%K4$b*2YaZAYe^AStc^g* zs{K(4w7r>3tp2GE;7lfvd8;GgjbYP>Eo-_jWERIlSPB>ZPU>ZfYjS> zTCgk|B9kf)FP-J<4MWKowgYnBLlR~Y`THBJLX;k4^jUfr*u-!1iU(ZlmWM4ldc;xr zvTIdoj^K63&|OaA+;SQSDkyo}d{Zu=)?lhAJ={E1e=;d#axb_aeVU)Sn!O|xb^G@bmC6{g}K8&^Pq8F7a)7vC(UAfR(=QL zO&pt2ddKaOyODgC=&4e^-vffexI8vG2XG+3-#b6zuP@XU-RVtjepY7LeA7J+@zJsk zJ*|mN;-SG0jkH2e*{~TzPQtH)crcqqghY*Xb$V=a4&>mh^|F|ye1mA^c&nQpaGxg5 zt(}Nf>t7|7Q`lml+%^XWQ&mu-hGK1u)Ch{S>++(q=Es?+MZ=%ogsd0&;#;!BA*!J< z?U4XT?7i77>N7&l_Vz^Djf*-quFfBiV=*IdBW`FIx<2`8MjD;Cc?5h|>4rcp((V#H zwEoQYgropCiRJx^68G%mgH`&)#6o@}1EtGI1lDWqxw9Ca4b>~NcFgh%HgIv~4Etu6 zJJ{f{5`!m}?U~}h&S0QkWyoj^t!!14;Qb$AvD_&So6+>&wS-n3f*g-SbTTRRBr$$I zl2wS+8)MU5IJv>et!N)&UeJc8k&q~?gs=VaJ*Ur z!lTWNoYGVrzxqmPd{)*ZGL~%7^kgT`^WL5_9qm2IYc}pjq+72E7m%d!4@Wd7q)ez> zTXQT!{7v;t5@A$MG}QQ5=^7f2tOk5a{%Vd9DaX2HK;&?9qHx`cEKSk2E3^-Nb>V04 zi{gHA1v4>+Zf&u5Bclw_1bykg36)$QYqLrpq|Kroip{(o7_e4aySSI-{oAknKIqI* zuMBY%eWOr~i$APnBh}^!;srah&vkxXX3sRAA{mCD1w(O2{_r|)zY?m0y7maLKSO^} zJvRk?ZFfnHp7^a7P>7_mVP_38V`mLGVk0*z_EB-k`Uke5il{>}O6tXiOKaCwiRRI& zCHKMK3DH`d_MH<=jzGcD4_Z4lQjR>?1Q}G>L^hHk4`Mp9)7@}6P@xq&@^4ONrBpY^ zb{f5|4Tdohu})49r=7{M$u=pEES7~hNEhb{pkRNeH&JAj4_#Xhs?=BMSA|sCj|-ue z2b>&I_7;U=GpZupu`ue=%JmCKKMxQKYvqRy7(jK{XBiZb6x|g7vFeBoMIPmDs}}kP z6kw+GsjEVNt5H`MXh#o7(J)v|4>JY<5u;8+``JIx*sTV?n`eW$WrM*FP1NwEsBRH) z>w}%Dke{aAIn*CbBav8{8>Fhy4OC3`OHfU28DKanu9qBgjj4umK$Ne+E>rrlND$6Z z77H-3Nl`g&V0Vz83j34$<;v@Qch6swCNz0U=QCJ`6onU!*x@5}f&ZLHQ;W1*snK4A zv8>pTilf}(#tP04WR;XmN9C6R>>{hAG%F!lveckAQ5j@Py`-P`IyNGSckslkoK&04 zkJyH}OFTvoPM}OW$@ft+%EI8@Z$=Wx|Ir&Azz8B2R+78XtvNjf!%VV*$<_#nG$7S^W1vO)ITq$zui`SpMKYLEJ z&%z)gu%tO&lF+om26S8r^p8+S$m|2#uWTAg-J%+HbnnGCeomM;(=!UvXbE-^h`Eso?5QGD>xkr0-6c zd+OCPHRf!+z{4u)ye`(~)s8R}=a%>}F*e|Oxhf5(isH-4Q^;(W+~)T68zxe7|FC6x zRoKupg7vh&sLAZNtz91HkldBMNn_wc<&lEMfKwIs0LER5`w@*Uc2rvJ)sLxcpBEmw z_U15bmcH(A8pqHUj*!i*^V0o5H`A&XW#+zp&ZtW9FJ1ViRA+M-?HX(@Wi*A3~%r!)uVHo7T%b;Dq;d60v4?*lKep39$Rxe&|Q9< z1HvUH9JTPX77KV#C*0j8Tzy!S@cv!=uqwjy8}V^yR+(HQ1Ps|HREc16xy92PPX)Qh zH|~PyUQVT5#kU}?&ah6@dQ=lxe;Tr83BM=VoU{Z-4atv!xW%IyT7Yln2a8YvK?A*m{>e^+Ip3ZkgmXjqpgP@}RloB}DOdzOk8 z=hl;=+6$DXusSY7r9cj7?Mi>>pe+up$}Mnq(*2H37(!;&&rv=|$6`p3>4HXAV7u5$ zcu&f5eyEYjk)AB#H<=kZFxQEvi?t+1z!s02Bk(onPnAAsk2uS0s=1sYyxA&aq7v*e9~{R?8eXIP9iB?F zuSEB!b3dCzBK>}S_ruA1sUD#!=hq~vk-Dl(lJc_8H}&-d>M~Q246<1&bRk@_TH#CmRR`5y=l@+c3>OqlcGvWxo7xZ?23jX`A}JWb71>}`7birHn7L*^li)Pk|p z6aLnY)Njkf0cOS`wmYskUE1R(G^taw2>cSzai=b?rsys8J7jJp7kwh_5$<#$>ik>n zTo(*E|&8B|uyep>=L>TlhZXO0v5%Pnr={4)_N7v(tJv+nB+- zCpu9*j0q`bDC$Z`DfR#mDRyHKVNq-4@>`&=C!{<{O`%L_|Je(fsrvE@*BH-d^PF*RBB0P0BDoQ7I&8XfH^MFN1(=-?RsD8FPDwqg#&^H{{w8c% z!+t1x2@U`akNcbU2b&5SuKL}62cvS8>xuM`fL07STUHzxIa#icUjYGv+hO;NAI(`z znF$$Jh-OJl>UG->sg>P{iK&({sN8ckqFQC8MmRbaM#b3@H2G?SHPLA;xn+_+eJAVp z4i$c?PHBjo29zD$1*K!dyl!g9pAMaEz{;McH(KG*$-vZuPttBo76ei09Tg)!kWcik z!hx3IxSuq^WZ9@I*c6$kZ`%02p-4i}wFijQjjGeX$c4+hMnMO7X42YvyRX(q_UzsV z?BLH*7~!tYGSTL>T8_HAGt$aZ{drhpo6z*g(X;coK&DV{iw%aKL(ZE|J1$4C8Bd>7 z6(krPg@lPxQ8{=;IOj}dygjJyFLZVtwfla9zQ#}+$a556{nH~UY%gDwpV#7kVNUQa z!)uk@MlB!lrV(PX#kOEJArVr0=mu;R1OHX2)8RUbBQX;Op*EddBZ!-unf1?+HyU^} zwVPDW9+);aKYtGjsH$>hPxOFqsG*36fEt_Cq16UWt5cI}4IdoHEr+6$cHg5yN4wfd z<+rTmYDFhz5j$0lpST+e5j&fvrnE(T$gr53`SHrv`9W8^p4cvX=y00V%6)7r?F4JA z_jK+wfX@i{k195(i*oSsHeK5^`wv(}a z0x?+I1Cm+avAbrjp?%7BTxWa#!ut9#Gi_h|={{D~yuIwq^wrG|@L%)S*q`}3&hP4d zy4R}EKtjUQxii!XtKbVaJmEJT<}Ln1CpUP`8NeTSERUWVPqPO&_gvr&5bjso9~~$w zTeW8FqzvH`e2L+-xj5!_|5h$ZKAFHfe(%*{*~ph&YU?$rn@S;n>PEAYac=6;EEHNO zt!y(T;j=OQI`!kSn7-h#G-OtoZ<7c8Ev~uSYQj_O#h&r)nB4J<(B*G9YA=6N;4?V5 zhM$!xtPE_r=cww(B3khaz?y6Rwx3(oX9*a*(xvLs8;?R4K4b+{c~Yl={ClKz%vNvOe_;ZdZ|MMp@YOjv zWs6^AzO#|l_MPjCTCGux)>R8MNds9+Vpav$DiPl0eQQg(lM3JLcjLX5bf;C@xnzPc zU2&@aAXG~Lyw8hGQ1?VgW8>%#biJ;M^}Eyu&(38VN$Hkq&#mec20LtJPfIxsFbJHV@ELb0sj`y%s1+L>V-?ffjq)#COqA^WIT{kRX<+d;0x zk6%2x&Z;%j%mCFU(-O=%I5Ce7Wgjqu|Zs2{p&bY|72J@ynNVItyn%Dd$ z{3%I`doEGA^KIduKZbd0tHx&n+#vkd(b$P)c0u6d3e;nV@Pu%P<9-=H_Cd}KLF49X zfsxG5QWBg;&h}xd+WdZnpM{=J;@;JE6X?mjGAQutgC6_}+%$yd*dniPqr^^{)Ocbd zyM$K&rQ#Ltey7&oOASgb00%XmS7x^2-5>jf`Sjco1@()*4`*{qK0+b=U911szmO3$ zZ4ChF-~ZgKXh1*+{|hqm4~YKX?bTjN3k$pyb?qb9%b-NCXCYv0yLIN%?gy|kO{*qH^MX)E@M&41-vMzVMfEz>1 zs97h4P+HrC+**{}!n3F}TDyfM8E1zIbdHK>$zw;f<|dMgj1lm+xPeElZdtu|#)F{1 ziT(KeUpe742Q6aD+O&ogZB)_8&CjUrbQ6cvI){fAI+uf*k-aqQ^&p=5=St=(5{fja zGAPhRZOt+sr)V~EZi(TlZO^bLYI}tR>y#53z>d)~RGq482zTx|>4x?F!bGp6UYrdj z`gzq`pjNv$&ty`ez|Mq?#XbPq(#FOln+0H_LBZXcp2og3P;qM&U)VcwaqfEalWvpR z>PPL*VIm~D^W6}xDYUFy=%Mv_a%gKqLrm{+d-F!B^_@bHC5bsIls44O%4^_;Q{{N% zwR`QhCh^sa&1(RC%oNQ2g#hCOIP|Y{@d~)WdoyRg@PT{GcvJU@uu^AdOCWufwN_i9 z%SvlQLm4((Ri#E-997XJu64Q!{$Q9Aw%zL_h_HLbq?-MYScQ=XQehrndOUr?t;cR(me*h4x)3KsmWs2FKnSlTLkgqYABguZ4l65WQI_Ao$N^k4oN{H zH|V_;cm1T>I?Mf5h=l$UW0O#&sTfYxa-xX6))3}?hqK3DIP(qZFCqHVyT$Liv29b> zBD!#UPgGW2N6bn8B&sO&Fjk(Ho_!EK40M9ZI7U)OS9JR^?#Hjo1@chH$W@_7nAj_| zX=u5`nYtX^rJ+{4K$?vfASu;YIOL?ubqZ`9J@XX?v2W>;j>f93RgTC?U(I7f4aNCW zX2~DHG{0>X)zk<*U&lu0-^A+L^O}~!Xl$q${=M)T5?K(4on~v7d>L*00g904y7W_Nm52UfANAzE)4pFr# zdR%fkM?f?Nn?LkU!f~J5cr30r-5W`?3eU*2TP8Kx*W;%y8%?a_F6pWa2|IbvU`5J) zNr8WKCrtYreuw&vTTLumAR;AY*K1q*aF0_nj^M1EG+pTyd_Xdv8|GJ_n2 zLj{?pSp_NTpQfLJa{f(-emaze1i7fh%9(}M-dTN}pqkP?zhn>6DFRw|vn+bGaIYKQ0)q)D)_qbCQ`)1_1nDT<;~aQ*a0_m`DFZ$Lg+k2QFp zDgU5dIe+}X3c!Oec^hlu53Q7vfjzVOZ)2|(uZtK!>Gw`UHj{gg-x@hhskk@OF1-EO zX*0&N6Fbv_BK0)JNQ!G}8c``qBqYfBdQnQSh+ksJ>WPc=_A+8GWGhDnNM%q1{vOnF zYf_EpfL0?IV^5~*1GtTieYu@6)X&b|WRm{3-q^}|y?!N?7)CK;lPN!Z7E|UPW0(;1 zjkJ=DlRA?LgVM2-wz$yCWE>f4g}9sd9^IVGyliu8$cu)Xa?97b z?<_38hRXt58317c*hyw8hZW*Rs#Qpjjl?mY4>F{?e95jZ7CNd}5}}_Ts;t@mHi^GR zp2D&&72OfG2I@~jq(~p5^uw!x&kyW!?(|z#vlC9?qX^eLiETM(Ho!8ft|{2=O}jE6 z3PJ_8)SQEW4<+wBn3+ii%6x14#VXszTKO4KRSq~wTw@2GPhJNtuMztG%%3H*C0LeL zSCc^p&q5$FoMFz)L11B<5j~7v0o3nAvO%Io)i9MRUWfxBVG+zi;Uu>PN;OF^Mv(q> zx|;PhY`7mCX-gr4hPm`*V$>)-?I+*`7|x0XK;9$*2@%06C2uAp=OUI%$$8tv!W_Oq zO=EO@K?tQwB;ab*?KhrcyQ)!yupNS3!QsV^nObV>g;r9d(szcA%3G56;?d}lwY_Sr z4stOU;F}=S5Xv)`@3~h#lPSn~Ttide5F&Ze z?Qp1!c86uOC4cK3wNDik3p|Zs#G-J+sBYRF%rlFfD+zWLbOE%YTg&&>-Y(S_xKIC4 z+gk$0MSV0c>aGFXvUdufAK<2g$f5-Z&)SJwC}8iYUvVrGF&t6{Ph_(n%GZqM)ekCg zl+%MnKi2Pw7}tLo40V;<%f2R$65OIue`VSVUvoK>i-xA3BV~)YtJ=R0ZIi-F-4zTR zgzDO3N)O3+Yyb`PE_`S0_O^E|?-74t{rRd-OE9-8^soo%vRupCfj@%{dDRX(^jp<% zpX3_Efvyh8yWsPVpr*w25@z(7&aW5zTK0<=^17O#?Wuopt2F9d{1BNWY@C>rl4$qI zYi6W(UZ~l#OxFG0;4+QRxLwF zv}qVrfr19>48N@0YoEWvXuw#x?#@&VR}WAQD^&#c5+wwFlPtCe$D%_Wm;1pkDlJ_| zQ~O0Nd@}tGam(9V8~RxfnU;(W3b9EXHRa`y{4scsKvo%$zeMIEZ@T$L%R@H~+rk}3 zsFd9e#>6u`U^~R?b(;Ay3IX z!A|}f9=EF&D!*I59#>&0_0bu}Ry_=h{684`3fRcDBw4qq&CJZq%*+gJW_FpGnVA{d z%-CjTW@ct)YP)TJ&(7@b%-h+omFh|*RjH)9=iC#Sk(m+ByKo|s3;ge%h0~)gp#gnj z*V>C7px-N5ua6DU^qIC4-J#}WnQvUojoZ;G;Kd&^{P2Qcf&Lf}cFs$9!Lf;rWulgj zx5+z#H)}q({q(_rZ3U;2Pj&9mS&qLsy8-LVCl&vm-y}yq<3dG3^xgf0=M{}HJGO?V z&ClT=DCQZ5vpQ2Ar(u#@ZqqHMcSHKr)11D4IXG8;*wI4~-UR(Ip`Frf#oVJUbitdY zqtp$bQk3*kHo+|;U-)a8FA;`^wXF(M|wxGm1{$+xKwvXUfOxF9PK~O zn~si;bR4=Sm-nWZHK)4D3x{p`{WM!}BB)kGy!_LdS(fUNqYW_U4Dvz}Rl^OU2YOWQfnC?MGKv6lk3PC}28;aCq9>5Z7Q;-uc9VA!wXxN4; z6=12qi0ee2#GKGHOVXJ0$jx>5GQb_r?V@96Bq8W>qpX^o1cO;BA)-Xse>Xm!zb0_cb1oNoO1f%DGIZhc+1VBa&egW zg>^pY3k{&>2^-`WKS-9-QJv=9NqLsNOje!);0Rbfh2??`rIa>SjKznuhRDPLMv3!A z=%ZA)o`7lsho9d3liP^_vhm@Ek+r*USl|UQq9V5h#S?y?Q@!^jFH=e{lMXMVhsEri zvn6xFaby0d(5DDnBCi=q53CPgcy}PnLpko{Iq=ZpIr%d53Eu0(iC;8~MCU8_$fLc1_YEeAwL&&1^OV7p`Sk zi}Zad(-3I`5=amJd{Y2y#{#WMFYH+v=&le9dPk@^^Jna(^4t$ntJ6NOU;3(J0jksA z9>RVgk1mChfgJ=w@lGl99mo^jAe+9tqJI1=EU9G3X2roaDfnd2cV#l1`Kdc*Nj0T0 zCtR}NC`?al?Z79ZBHNT@!eB1PIssWhXM2%{MQu|pun{CEzbQ%W0WeXfUt}A72Jkw0 zz%NP$m4yOhQ=!hzlov}aCng;$I;d*7J-1-BpGLuki6coYv0UU!qOCT%i5hRopSJ@7 zow8PeMo@{^s5=)xBg_u1E{bg-`fg5&WjCFgV2-SY#_=T5pfPl$qL1z^G*f}rR+B=X z;L`TwD6Z%VE87|O@%*gVRfzCtghf_>L~ZhT!(==WRq%RVbrsG+nmmApbrsfQd%S^; zI3VhIAuQm(v4`$_#HNWw0mWhOq{+`S&s?IB)R;}q%*Z94GVC1p*;t-%df*LJ7tWu8 z%~#O%Z={~EpR}WydWY+>?Rs&5`Ste{0_nrJd{jXmYv0_rqhwi*9G>-eRTm!Kc1z_I zL3NhTNClsT^OaDZoF{f<@r9eG3q_YuHrTikj&zCDkTbb)cMVvQ9ayc(ujyJ`k?HB! z%Sp4JYBVSIcda*ZANF#RwZ{fNlAGfYu;BqAn?%)9kPIjQK;O3>qzqO?A5>2#hI}2JMJUhJ?aquKft25xJY* zo&x^*tX+)lGEOe__mMqs$9Ewcp^WH&b_&d+dDvWOFjvU(W@&KJdv=CbvgiOR9)%$% zkMCoisd6EO(x=`EuRslaB4;1m6y9N04~XX8Q5+wT6y8zg$N0qw%L4fGr!ue1?_e2g zK><77uhcZWP`@ugUa?(*s+FX=ppq&loKa7pR%!-PS*Kym{)Bx%D}_O_D0Ss3LU~sz ztRMfVR>+MtO-u-}#T0A?4vCxnyu~yzd+c3{SwQh>NEi05O*5L2vWlNTh}PfFd8iQy zX+iV`p8qrJlB5MYSVU2uakOrmi%42z8o$_DaZL))sy&;jjd-VVpJYwUo`9Mxvp3_=Gf~_&No`eoTShPWQGPwj8>R z(S#1{P86zG28d!sb{BTRL}m{+VBB;5YGFbw%`)9hc5g%t%c3mHO8JqSIM^iPbkQE0 z7?X_)$yyWQLW0wlYl?$s<;kQ}H^!WV90oP+TB{a=fKH{D!MwqO0O zEMFQ^`2U_^l(9AYuhEX+L`&N_eU#x(_*4}eqpwDo6*5OEbRG7NNJd5yX^ESJTKziL z>9NG>`i4rBXuU!CJTbF9XnsHZF}I&em@%x<^Yh0JQ=hJMr`P=azTeDqF~wn;4O;@% z8Gk557lgF2xE8ijPc{Lo^u`3}gq)17Akcwu>#Mt&s)xK~5h`((KAp_hy#)|%GSb{y z@Nl3^@$s{q8n+r*Zd}+8$9aA-?BQV&hZqjKIH|b0ZKIrh4}}lyQ{--{hAtUJV6)C9 zR6E7Ff7WJgEvnF2W~Aw)^dA$564QQ$aD;;?s~&H3rT;xcPA6esFtQn1F>M$45Cs%oXTC7X-(u-hDuc>U|T%9ZW!iz`KutD zR~Rih?ZHB8h$LQ!I2<$q>Wb7JA0CBAW)IOp66HocgM5yMR8D^xGiBUD(TN`L*K zE#tFx=BP$7zl{LGnz3cFD)*|K-ymlYZmKZH$gc=81|} znIMISX?yIRsEKx?s+2qwj(XrV>8;1C^A<8gKG031%`Ykf6>1g~si2Vm_!?#BL6udU zsDd=%sVH-#)x(-Ia}SvgNytbjvc1HYW0VyBOOjBFIcg#)J`YFAEUIBx{T`ES5jW4) zrB6aq3~-48eV+uSSYD|5OwlO0l9cdB>ChMhOCL&8Jx}!~HtTleaOW(dMB%8F%HEZt z5K&>1%;t~YW(xay=3m)xTN_Norf)z%`XE3+9RGf=Di}DK3pzTQSQ}cIIFQKL8W~uL zd`)9H01Ta-0Ji_=a!pp!lExN9`Is`TuQ*lRFYjEH&gUC=tP_1*Qi6z3fM}Z86W%5p zjg!%K?KduXsX>7e!hZ#MEs&a)5n@z@@J?ese#*Yi?EmxnzEAGwZ8cF4hED3H&b%dU zr2E@#zd-0_-6GCc&AU&ifP6}{vhGEK^zOaTsg$L6yxpSqcS`Nnh!gph`K+9=SDt7$ zz4IT@gUwfcg6WL#g{XVJ(jSNTkdJE@Pk^~o^Yfq`JY#D~b*s`Jc<|F1O1{W2Btg-Y zqzTA*$>HRZ%MheAt80Yg%jKLCDk(?-@&KwwL87z_t*yzVrr*;DK3XgBzc-U~rRW;ICVO2gb@jlamg$n#TXCQ#bOo(Sy-4fjJ~@a9X@-MAnHFoS@0qF!K;D zV|H{Mw|eul4Lnff<~8#w8SvWLSm_!{IB(z6JFIZFH`_|pRzUNb~UA0t(IAb_qgsVdAs6tb$_1Z|GXdK z2g(}UCdP628bIU)MqVaPvKfg>H4qLaT?f-ZQQ^TA1m#@)B}x^%JE9#UFetm)McQ8+ zQ3VmRi;U={^bxtA&cKJ-TXLHY;pC-SXr|&NcQh?-hol#`+Z1v9g9m>Ioj7b)+Q3UA zshg-@ZE$8;8V#7{S{;lhQBA6pm}GBwq~livu1pJqrSt7vci+cOgJ?babDrk;Ob?Wdin--=Uba zfm0HT(TQ3BdcV&$^0=K+q~ocu5W%ia!+m4^)?)E3(YU}MJc@|9YnG%V@|>@m-x)d) z0q0TgZ=qFPX5+-c?wU%mgP;&ap3>cZj+(ttEV7%rkm1{)kXR#CVXXO%bx&H$7ew9J z+6dOIyFi+uaZ^6a6)YbbF8*G5%%r4{oKXuD{^c9!c%N`Jlq~OOQF=C?qP=A-Q-<-# zpXpu#gB4glf!h`@xIQ(z@Gk`ca;b~p)VXr~CuP@D z$+W{83{QVnxfLmn>lrI{29`8R8Rw}|veVS4hz;MSol^VsZrVb)OU+9o+Fe2Eo?Nb| zPB>PWSvLmtH1K_x4|HhjKckIhwFa@VTu1~9Ua-eisP?_07SWldf7LnnQ*97!$Fwom zS1d=fnr$z2=|lUOCmc$ zwew~1%_L%DXx?2Rtfvjn>bcFcEfDD|Oe2s9cGztVRtjhMR3;O2#}kD5;w zY#{bWjyeMksSP(+Zj{D5&9H1{9K5Mjy@8=)LYL)nPSiZcNI|ZBdCf91C&L&M#|R*} zY}i$lBDA8#aAfT{BPHRqfA|DGg--@eyrN#JqNZ_<+@Dwu!D!mH zq83(S^+!!UF%A)uRFXMl?+V{4Ywin^bj_GN+5-GTJ@mvFTC=ULs&+cpudET}6qoD8 z(;ET##u)D!snQeNTEgc8b&m$YcGwY+sJ%Z^*oepo){%>Ou`8n{?Oi_ z_-lA)&^#)=@6HeV4;T-O5}zy{D}p)s4|zYUeqsgU@r0uqg3bn)Y)H+ywFXJoj~9WB zbt9_eLF4+gzc}gUVnS{hGjXX4?vVx|*AxJ_F=g8-;99X~^V%`04UM(xj#E`w1|1wM z&%N7I_a-vGhkfZul1uDK7Fz_17GAe*CbI4&jw~UG(uuddbnl{_vKDKPC0&!%psde) z{smc|I8MX6zoy^Zz7%H}{ynlPyMGP1TYrtae<{{F0Zbev3~avSZvRVkc2oIFboNsx zp_Y(_h`^8nDw%4Rd2PueT%jT)0S%ZGWx!{Jv~tvNMV1{q-e>h%Jcld$7rgiROuC%e zg*47sL)Wu>o!sTMjSvB$CceW}>+zqd*5i+B2c6q z;)4Pzd^)guz7%I8;0s&GX7Sk!VKM?t{Ferl4EeactzB$PQ;4__+lBTQ%3YM9%~+UG z9!Ak6 zYDm)7=dOlirAFzJTOWXi6w`2{%B`+RiNV*bYqC|W?+%Df%`Tf-M#Ta;}O$TXFm7qOEf zwxo}hM(SkVKe1oIkpV7L-Nl)5q~gru6&`bhX97*ysWS4yCIH}Q(gASlL@Sk4tdXnL z)m`4JOJnad96w53I%0RF;_6i6p-LYXF9!h=*c zr6VH))~v6zGp$;W+FMRhtlqKp1s>Aq_hP1^&0rn{P1dWt=YY+NQS~(_Z{Bv;%hoB) zp&ymKNqtK3gW`1rTB2|<0UZ&U>T34T095Q3b^V>yuaI+Q8oA3zv+w_QOd#yqA z7o|b97tKNT7u6m8$JIl~Qu9K;w$vWnjE%704c1kuS5Gzy6SLNnztqJ#6}kR+VN};S zE{cosp(Wt>6Sr4)vMUmKjYcIW)v*5fK+5|yyzVE>Lbt2>WNRYAiq z_`SmNz+}JPn!8oBf7csOGf2GtBWHFp3AEOdQTJ8bhf^)W7JkqIX-{%=PyPdb_Hc~n zqPC|M>&8sV?-=o*8>`1r_?=@|cj|0rG$*bG^iWc>4+l(qyT?0hTSo%lq|*|HNTdwy z+sQD{FgT217zKDF|1K%gi#4{h^z&{y7Ddk+u~*=8({4eYj|cV`67$=>a6cH!4f;wD z*IBaE%$q(k@2z#I^tw6GtAM5Bx9{osMSe@WZ|0s1KYH?L8=wb5o9)zh=;)MB5ilOX zJHc=Omoz)A?HE+@ZBQIsXV9;u??F$!xzoLqOyAQo6$I z(TL5CT#if*&x*OD_}~aLx6SU=t&W!>KRn0#Ar!n~Fpre_h)7Hjg&53xWJYZnEaBT> zb?=z`iD0G+Py8dq8RL>K*5U)IGmxF{5{kx0t>O{ znbluG;?uB@kWVjk+3{9&;2VSnW)z#gR-vC&4xH0x5*)CM*XOG!XtYesLeX*qPlm@d ze{R!G&(u$<*8b-&jvZ{FN)a} zI>{t6jf}aT`9A0U^6GJVTd(_DF7P;4Vb9!-0q{|RzJO?;IHa4*E>Aibll%w|X?(tXJzWb&B$l?Xndytj(0qoyWa| zfkLk3J|kaI|Kk)R$Yq5il59T3Su{O&HF71x34O9F#(-`EeC^L_&g%DS$Bo)f_>Nhq zXPfK$CAeoC%}8S!4ks`0L(O$dP1#ud2CMVPVR<>L>9y!Tkxg%-02dxPny^fpwcgjF zL^COq-xmNq8!S4VE5fBL7Ah{y537ws%dy|(M?$9-H5N`zYIji-+(eHqIxk_{^@Rmz z`>T#8KF>tuBK@MGG-sDF&9>c6Cs*Ps=seOTjJXiY7*W)C!AF&^UEWdC$GeN&YnnPW zD$h;li&dS=nYI<+#`=M_49%k)HjQ84 z(}j-=b7ly6#U7P}EKr6@ z@T!j&RYC@wvS8@qn-IJ(wmg*}3#PFcq=O!8{wRTCY1sqNC{!FJ=}0aNQw=C(nIO1H zoah40BhV;hTpbb#VIYrMK$=Bd5KLV}Mk5}9`!}X4P{}a@719w9RtY8WRe{M&JOZ$9 zOvo0de;tHKs8@+W`icQCzWNU-{(T(qkF782W@6<0-{Qe)&%VgVn4dpse;@3tGl5&l z5Ji0>T*ZJuP)#s2iqL{ztRoC*IHJkE1kotTlG87liApIZ){&eKaViO%8feg;Z-LC7 zDfYbHNA2Wzfr}sRYn-l z07KfEB99$+6Nk7?x}^gvxwQdn{A$hw&Pu+;1BJ( zG<6HKo<3^#NCo}v**KwB@S^WeOx-xf{`-3wAE6;IFzy=_GGAFiEOg9=gaC~R%9f~_ z(3||AkoV2eUjM6@yxyyiuJ}CvpYa0%zWJ8=cIrYc$1N^Omeb~|w@-!En5(zENN6q@ zs~uG)m%^IGxzgq|ASWxS7k30n$jEQt@E&2*<6)zLZ(>b0+l&b~XUVmD60U|MoGbh@ zD&Z%2KYA!R$0trxtWVq_$8i^-o8kfTDASVfqNLRzVj&ND2XTd)at9<+<6vn~Ttw8Y4;(QJwLbn7$6Yac?`~f!F_T`b|@J8dB1Qd;ng}u6nh?M~% z5|+>FHA3o+K`KJc-fOG%jmu)Cc4fve+XVCOGt;$ZHw>j)Yp+l&*U=+?p(Qr5N5e8n zb7$v>q#k0ULK)-!wm%^fYlYTJ4aBb~D$`_CnMkUi6s}aL+k?;QYbq9e%^JK^x}nPz z9Dd8?9AP%7Smz#~;wswx&yo^r%H_D4x@xx|9r;ADiBvu#31b{c8N7LW;uE($M*sf83LF+n+ukH;pU-(S#WUPzJ8yxK_JYv*Z<=lY~K z@`*~ka4gaj`|>hZl)IJeqpaUqK|aB|Rc^U;-|@uD6pO|)%tKJg@N@jVc{Mg-Bx_38 zU3t7?MgIs6;@^^^`X0$(3@c*#D&DI7ksaDr`2+HqR9iD06($m!#dHxq0_nXL?#5SA z62&MiFCJA{L%Hl2m|&2smUrN^v{cOFQ+9tkYQl$;RXd?P^`^3S`Ak4lMxzR8>Al@t zXCug4&oyQgEwR?IqNKAt7$q=*anx;A%=tWdXod?`$iE*=1SGNE#q7t zbR%e@jO@uDV44Hhpf23HrulD?IWR@GEw4nLbEA_eI=Kws*2)XLb7Sz#J}j+BqhqDl zq2C-ql`#{oEjoVJOR^P*ZKL6{5n0*z*Xs$s!A<+1UUKukahrE<*V`6*rnhZyaiae^Kp;`|hz#5utHwk%)J-+~8;rE-k4MUZRB zr~rx+N@HI2OC#v#;F$XYTxfESv^s+}uC#6lS7@-Bx_YIySYzC;FjE3C411QmN#cq{ zR$@xGBC?E=UZ)_b=lNiu?1_P++Au$KQ@+4U+@8i^cQmsy*;S(}o05fg^djw=^9)u>UlQ>$FZR-GK{ zc6ia=eX!WL>;tQ?){bF1tpcy&H!ME|7n}vYr_8jZS87OqqBrYfzMI+P@+To$m84w} zjBTPkXSd+DGlXxtp?IErbOzO;AKSs}=Dv^zL4!2t#RzlIXmbg(hkW~GC@y67V^)rep{iAUXr%2i75O6S~8KfcV2+c{qC5&g?Ipi+P>4@lDZ(fino5Wpj zo)exbgoPG=N;2@iBOhj(kYh)}lv1IF;JWv%BmTkAS}0Cev|h%gJKW_P7cSwIk*F*l zf*ArKL@4{T;W#VBubU%ID_)-1_K-n`b(|(OQ&sx1nNTjJ+$0T4ZPbm4^81k=FT%sg zzfllZ6Bh4R7-jHU+S#|bBC{XXo$;jbr>%){8%ZjH@80rdUfC3wH!Oty8Y&mYw;P26BLZ`tRVKDz7o*t@UJtx4kk)KjzT`z zhDlMvW8PysR%Mj$7s*6X>Ko}pSw#|qgP+q0UT7LI@XrUy7b$_KU-s*?DGo8TrxX~} z1PD(h`sd{jAVB2@GR;Y%p8^r8a7$GANHmd$u@l{%p;VQF9wkNo#<>j0kD>4^GjF

ZXAnwBU6wIQQH2nhh3!NAAU%O!%D)gr+wGw}*N4Dc4-HH~ibz1r7h^EM(*)Y-?@(Me@i1Y)rlq9hWcK z=RfJ5k$`o``N5 zxSY7|xL_e}i;vnGMq#FoB`|C8+rKbXv8&DS;XSRj8r<5YCCYcTIy}dMu`kGT$@tqr zLJa%SLoRH&38 zX4*s}hV)zQN9EirrFzPp;s%3OCSzU{dYfw1B|r-o21AE7RMO_>S|&WKrS$UBj6^@T zVypxhSnBpn^ln#RMCdw=oxv<6^*!Pg(Mn-qMlI{nwA1TsW1p-TwJbW}#*XLZQgf?k zM3*{5TnRTiW2h=Z3#)}eI`^8t3LC82Zsf+MSG+diFEASHunE7moiiy8bcG|q$!zpX zhxEac1rc)Agd2E5D(*5dD?0DOcs7xj$mWI2K=?S|F2-$MJTfDA0T@}CVQSFetZjaZ zn7Ejho=k0rNCg+9Wcbhu`v!wbK)iDk8JBT3WHeMHniw1XhOWgw5U~$kY5`VO&s)U# zu8%5N=SojVSr^6CEav`glf4dWby6?Pt3%*{Lk~xA937f?;7KgZH%C>S5PD2nKl=&0 zT*)Vky9~wc{2_$$nvWMTLT>LHo|27SOvBzvR+bUYr#aSV+TIs!fp~{9o%F!MYeXMN zYtsDJgkw3dv0nX)c)WlD0^``8HVB2U+I|xbjL&x|)bu^jttWi?rIySn``nk#NO%YxrZ)+YzJ(LWkECN1 zhj!7?JCn*tYt-7&4B&uom?W|n!qi%hu*@)WXd6oEu%0(*N&l5Cl)q#CJ3oFV($vsr z{t-v^Srxb4nBym)1^Q|0r|DtqD%koOdGRg@CODLs&WT;oL3-0514g*1OM^G9;zFem z(K%VrNrcCQCbsZ1@!??bJg}O z#i-*$dxE2k+G+w&jrYacU>8k-8xekjl&nOfiTf-8c}E8vG>gawV)f?bs8ZBsF)+F# z9vNNISbZ^3p`Zr00_^3vCOrAMrjr3h?nI^AT@BdWB1pXCA8)uINEy_b+#y$_1ywX5 ze|%BZ+7qO*q3OjdjW6p8u%Bh{n$*p}c>1l&+)#;O-b*ga-C*~r$QbDMrBQ7O$oHvH zV+FAiz08o?qIkCRfNaYs^tD3D48ONiMXV?r^cHt?RpkfOVCMGuSM&*|!@nhw%BHFy z4zd(&w0aIiA;+me+`IF>Od|%1%17-SG=rl_7-uAaBxoz#`Yalu7V_?8MKvO~4n5PZZ5z z)X@#9Rlh62-Nhp4ixtw#&xAqmjzy1@oSSZe?0=fm<5L(6 zU%&zm#vH|X(}oJ%5;X2da@$346u!nYYL?)Rt7ai4O>$ZC+k)l>rVIAd3GCw{L?kh( zil7#yhM)z7T&X(O^8IB2fdoq&@&v=1-XKEm0Ai?OsT^X!BEl!?;EM?qk;uF~5>)#0 zuzm7CFxiU1-yCoSl|=l&K%>VUFm{8B2C(h zg~uQr!-xBUfEC~O?bIHuW})fhH|Ix?TpZ}x_<>R!d+`V|^Fgrd2>gAJZD1TPk@g?( zWypCL!-wd3i-;3Tk^A){tmZLAhAot?sa@JvXJE1!Wz#TB1`Cc4rY<0v+?B0#5_FeIC+IQHW$ z)Q+I@$F3*I%PabD5S-E7yUb)tqxfxnq}4y@XBtknn5v49^vb0Ct^H`o_MG0R5wU7SND);4<6~$l0g`eot&JP=H2D4<-RmL+3xWX})P_b1G%C76Y zf&S~N)Nk*-Vt%cP+1IL2{!bYBr&ak&BonPDE8Q=E;61-Iv`|w{K25{|CC0;6L?EgN zK|$iSU3^|gx;f&a{JPHb0`f<3k1ZHU21-ZWMmb+BI(NrOgEkUkSxp zPbG|0DlZS@I(r6t;Jy5|)1Q5b*Y{;$J(4-sG zomoPQfS5Oy`CgpPkZy-~?fU0?$4ha?4FR3MipKb1B7E0x^goz%sV&pSTFJw{H9OV067~iriMe$QK?Z4SVpXOt1h3_EHiQYibMG7YW z!V$#6#LS3t?Kh|29D@RgN_ar1^<~vW1S9s9bBu=fq6q75bdiBeiwTC%3bNfd*&IP3 zaTQmWE-(I=U5Z}FGL4lo&z`U6T%^;?2--|8F)Av`JE|+=T%U7Xo$c?)D^+c3DL=gU zt>vz}b})~{z98gJm09sTgtTW$Qh+*_a>YSRs0mF^Sb01*XtH=D70@8^S(lsdpFYxNk#SPLCts{v@dznyBm*)nB4<(8gsM(3g@ zY*u(>X|NHU775I-W$9dZycKSFEBQ%7UWMl#wvw`!57U9m5dUt;+0mmL_2`-*AUMBw z#KTf97TuY+KqH^Pg0JR~X?}DE`Sbn`diI>B=i0$)^2yk)Eh+Ta7MtcB5gtUi z#J8XRhcg~iG@5|RLORXzb}>tZT&k5=Y!dI)K0srw5H(bjQD{^ql#M}ifR5qGFdW$R zZd1TBW5EuH7e!uu8w?Yl(hVb@!R%PBvWI^|I4eOwK&P8vFA1MYoJiJbhk{Q(9Ti9L zJk(P%+}JfKJYRoM!1Qf!%Ue-E%}rwfci+R5Sy5@?mONjGk#W)wk8n`4?x&FdkY>NjU0n_XEeC49 z`;&ok=1B5{B)J9j{2n&<2-DvaXWD@TG$!@|hV#{|5~V z7ze-&H?wUp26a3)1yK~3)Q;db7MEfCFv>x49|_@$Sc!5|@dIlx4cjz=+P+ic>JI8oBV>JSj|U=@Qh-1^m=y?AT$Xk$BgQH>$}4LwJBg0o@1(nQjOd zZEDs#z{MfgD5CZJ53H2ZL~Jd9k+9JGp>=rWB1~u2CSp`PELpieIHad$387-XH~)d5 zq#g5uAds%aVuTf+NL)S+yXZB9;5`~I9l!kBZA`1p=8+UbQ}Z>QA^?K>2?3W1fW|5e;~UzTgXlxY?#_Yy!F$1a2zCb)131;c^3XMLAG>B2DC5q-*V)yivTer^V2d za{m^QuyJR?DG&SKo}3=t<*Cfh9vX(nOYIvf-tI+HCh>P3HDTKC5g4#~@tnu@X|94S zscekj`ox`~b7Tq0mTHp?(>pJxjMB&*YvP4cVx<_To73J(6nr!WEJhXWf6(UCZcIY0Va9s2jxioyMqx7tAI&1@Gbwbp0J)zr5uUy$j4UKa$tKFf!>gLsfELEz5^r#k7Bu z*Zvb={^PF-ntrh>VgLt6CuL{De|-G+{?$R%b2(&1gbz6_4il~1AA%jkszFUN&_o1b zXi1SMP=b;-+jW)Bc^3(nL|p;XyU_aif~1Te%(seO;6_>XTDj`_(1f?soQ|ePUXH8j ztM~WQ%eFvsX{j@QBDl1YkkJwn5`?${^jG!|_8aI4-ym@Uao{;ZNI@Bh1pA0%hH)@M zG|wAVqew8<>O<1y4`khoa>z_t%u%9NCm&4~z3AxC!;q)2BDB?$WM#9YWpeCNcMXq{i!$~zG#b#@ zQl;ZydMS0(CVvHJ)07u3UY{8qP~b!`=i->-#b=C3V4T5v_a?|vHyZ##rWl824rXE~ zku-xI>zLcAy5o_-G^WYXXq4ygg_E=BT&JmPN59ut9f|Hy-JFbLHt}()plMbQaG$Po za}f#DOHsFy%?NL=j4x5(7>A=e1 zgKRrw<+hIjJ3@tNR{WvZ$Uv2j$-9=}Rh5;A8JtM5Di1Z{l8fW>o>#bRSDOtN-Jwv- z9*xLHi;VXtQ32SZM$aBLoE8_MA}y%o_ro!kX5W_&&M5LRI*eD88(wbGn%PgxZg8Qj zwHKI9ayA(Ko+)c`FR94zT+Dyx>fhmP?UPrckM8+QZhHYJ%!+mY5%a*QRgi#T$?v!c z`wjkTJqJjb(p?LjT_JbN|Mddbfq!x#$Xo?0G(@J*Pll+kY)+^PJt~2pY6n(JnISV^ z)FQWpv+#V=FYZb9iuiN-X0SVVDDb*8`|`v8hVDfvisXZ0U2yY>u8=gZIkHEIMy^u- z`Dd-qx{G=X5}KbCxU3c0BuywpX7gq^SIE=m=nmT2D6HTn*_f~(Ibttv-zh;g{slV` zP9vfJs)hrHxRQVWB}{D~%mNezo33fZb>`j)k)5fsBKP-z2tu3Vmi3{NUXwNPQ7aJM zAEB~q$G<SEI{?2E$~2`r?J$=BVw7<)H8J&JYU*n9cE8+=4`gb; z?iZs70*G=>WT`GNCkU4WQLc58IZrbf`O)E#9ceL&$kwkgn#fu~=Dyfi6>+XE-)gn8 zSmd1p7P5dV&heoW693EX`Ibr8VYC8?rv!$2{ZUqnZ$FbxLoTTU%5_|uOA0<((svxd zV0_*AVEy|b`r1vXY+Pu?Ve~LMM7G?S!GXjyE(j;)s-*JX`NL;k_p$XMQ!M1;*Btdn ziWVI@tL0*9Oet-YEdxnQW!b?R8m0#iqTorI$%*CIWNE{R@>ls~1lMp7eRfFo_&WHv z8PBi1aQ;Q(k1_pU%GS5H`SDKLM(TWhxWh(f0emwq#ft(u>|RNet5}iMP%@ zjp304_AjiB@+q0Y7dkdq)z#cHicBWLUWtSdwq*P=-hjgh^t*>ELUjyy6W z`4OU%0|!L?t#-!A`kN!6g5oc=`{80e#!TiH_b3@=E+kXY&!r|eaZ(odSCNvNb&;K`7{s%G%nV8x-eA)i|duUQx z7eo12LaQW>9B4@kYAS?d{pQuXk)WSR0+yj=Z>LG#K-M`bWRGgaNjxsAn-@FN{3RAo zNI75l$u-}6vfU-j%}dRxZx|>;#{6Ee$@_MiWy_z<%jNs=b`A~X=tf`!5lMa$ACK5e zBrX>cM2e}Q4YvnX?cha%HjYCL{PYV3bD5+1yItUZag^4J$PmyH#~o9j`Z0ywhQUd5 zH#62R%#dGRo8^VnDQzl?^O}niHA|MPiyLFu#XjPVSZ&F~4+#cXPpBc-iCZ_zh%xRJ zu&J4A`f;k~T3r2<2I{s;V{OiAv}&VBQ_HN4ZF!m@>djFJx#I{Lv-k_N?`;OG7l!p4 zccZx*OtPDrRprqkaQYKkk$H6+ok#~G+w<;IiPAK|tKdTTUM33WpGUtOHP%M-OCy9TBNgN!@X z_&?*oH%Y3P4{}gtYDXbNsavi$FN#kMA5t(`btiY+@%2?EdUlaB?=e#2Il}zX-7<=c zd@L?foC*uVx$2RDzC_LH4GS>HI%*k@<4P&sJM@HNyWt#0hmu+`4)ZuI+u`dn9&ri1@#3E2C#=>@X|Tx!VIcPA{Nez5>#a8v7=Y^V05Z~<6Ye+VOIdK7gG*t#m%TT1RW@^L z@&!ope}MIP;y3(1$u-|@@Ob4Tc-dn4n*0db{hmkJWITh0DSn4A7g$KMg}~c)9X`^7 z&WIOuL6;>-)Zso<4f}>%W4)t_HFQ_pBP889aC^kP5M#u~!mgEzc{E1Rr<}Azo{qa2zCv*T21Ik|{%Nn;?Ks{XS z>SXxo@fqf`kOiD#vsVfYBi1nK ztgKmlZn3*q3ltbZ+o?*JE@$L=P~^-@*+IV=ix2JSUyC^nR27=CUub&$b-alEUyc@G z14k1{8%GlxM}X6pwV3kP%$|+eSMY0R@YPcJpT_h>%6~VecYXV%(5y5b8bK7MVlIg0 z##kIwDl3z*5L+1ZoQHe1YBV@RMc7^Bg=&q=G`^@K)P(3nYLH*# z_1kM^JJCb@!aa+!IUwf7C0O++i>!W@8WrgXn*fP_^&X%DrE6N%)Ksi&lE5t@t;QU*c@o3O zPw%(1FL6X?6mzGSFk7rebrTd=&P7}=q&e{IX;1lIHXMD!$3VMO_ewQ_Bb)rbx3p08 z#iEeB&d=v;sAxna4gFK2Q79IY^>8HnLCDcQL#LPC0FE92&hr2;` zVHfo2=+42VGovbzP#w3#V6Z8oueh4nM#s|Vb@-YhNzA^tJ)?i-MR5AV`>Y%e`B!XmR3%dzq^0GKON=mb?ps*cLdi}o}7^8 zv45-?#4Ve@2lD|&L=ut|sc}KL@X=dI_625!WJbfd?5^Ule9z2aW5f*Jv8rZq{3QG7 zjSouvCsPnJ1i5U*sQU6Cue|-68|j0s;?4816U$i5r9Ry6dlUDb=l2esldVtli(cO` zy`?0Arhx0i6Tjp6!^gqF;=PHWi@J>cVi=5?k@QZ&ONNg=SQlIftg#1*cL0n(3RE|t zH?UWr1P+6CWR(s)h)*Se#Lt=BAB7XUhru~fIulZt#dE};2XcEq+@%8x_6urT+lMVd z?iV85AEj8G1J?*zdJ9QJZTMS?Z}L<0DAz?qoHJ-SjCD;`&CpWk;}k5^Ye7~B z3KLAERX%Z+)DOnGx<#>ZmSJ25Tbi^gj4qV5EI$KwIlE#2TQ&;%;P=v*`j5BQ!G%@mc>z~6; z76!aH&arWF8dn-)a(}+wznzY^UUA&?GR_5=>{@GiAeGdW6cI3tiXPCDNpo`0&sJ;t z@xzqy{M2OVM2tnzw-y;dD=Qve>sbEy)J?>eGvv&!WkIup`12 zni+H0Ruwqh;qBocJk`BUp>X}Dbapp9MA;&j-i?`rva(hxRc zFB-ajbik^rCcCz$H4E-wx0US|#3ezuneXl~Q~v-7&S|51IynDuM%@gxt>^nvwhbi9Avl_B1xY*O`Z4d*bsvg)4;qBoBk0F*`%XE^3Z=7 z;F3N=Eq%dlBtF!*WUrf%v0@>$k*`JWwMP=>Dkdw<$wzFDSkYsib~@w9?jNez+{R|M z;v0a+LS9<4AYHi#wj=E=3@>S**GtKB)d-`AN=Za$4Lo87A2etS+o5;F$KL_YrI*4d z-*N(KM74LxvRsW<)cQJ=@V!Ac0d78m;YPIx%am*Q~Pi=1#C zTf&w{IZ@#v*RkM+1iJAV6v}7H7s8!M_opap%gDv{o*0`<>=(Kh{4K<{1v2Qv$$jx% z*qhZ}5cbqn75lVQlYVBJ3Vv`VsuB*6W$G&Gd5Ib2HCn>83WIefVyG3Ld;)tcrq1!^ z(oLxfRA4{PeP-E9VRs|Ah75f#o2e_8V5L`mk#9tpj;E|lGal4jiNN>y@`3ZL4kHya zfl;a@PSlQ)LDuA1tff8R_`bz-Z(UyJ94I}8BNFwf%a#dc*1Xm7`vA<*02ky!0{Wo~ zC)*D>HgrB%E?YHrX`JDmhaPzf<0@^lwwb)ag)E&6q|moNPF0te;-4PN#{0Ujw(?b>N?+NPgQ2neorMAB#L77C3hKV*2b_uOESuMDqsWiX$7P1Sw}|Z z%41@2|?5seQp1H)uL z7dt6*fF}Hi#VgY0M{a{uzm2vK>Lg1OnWviG)hJfjpdxXemRE1py>jhZu2MA@FQ2%CHQ5C0$Mv9w+ zwdpmK5|S<~xSC5OAxdjwu_0`*d~}~_&5K7FqVW_F_s7bIsdnD_A}%{W(uSNQ^HnM9 zH-cu1aBc74H&;CT5pT*#Ji5sgB(npVCbQLafW)cw=0CK43$(L~P#KAY!-bz`Bi+_l zu08?3Lbh@;4d4@T9eP+{KO7a{)m~keFc(9Y4z2O^?fB3=`94a!cvt=I3Hk067{e&# zh^>aWl3>)?>ur7J!aRJ`sgSFt#(k( zCZ?4&_J9_K&F4i#c^>|l;hFX5LbKo+UV#wn<1Arpz$y9+M+T+IO0%|P zFWCnAP;V4|fD-p5u@b)oh;MH`@VN(gg;G>>_Uy(rl0iDjria;ta5{Bnl_8DJvXn3B^Wm)D}$f2NdmG`5dkJr4WL6V<%`dw@L&n+S*RsOftLB0Au^PExLU# zwQ4+XlrSSaOXeE{D#i_@$besmD*fFf2;mN1qc@r>T2#~COYvu2+!c+ac)6H}JY>8z zq0QBI&!8i$o&rek%8CKLSe+NpGwwL67Nmd2tF>~%zK+}ifIoNxSULPxubPskc6I;{ z@b?ESVEz10PuC1Um}OQ5AVT7VxxRpmP97X3A+Y?e208|>7VJ!6w z#fl^MGQ#R+_JI!}pbypa4Nv^#TEo~|@%N*#XX&eUp8M6OS)HAp?~?mtBY|z%V~0pY z$w?j5gdtewm&0R2ZQ^&54X4tnCT#_<8qor^AFjlyHRUT@*BH>li};djKG`iYX$Cf@ z5@7ZR?mDbeTCONnA2FtB%$X(9zUv$&h$afPT|`K9%F2m71&vnM4_3@z-($#!QdS#> zmhafcWX?#hU`t!m+L@7abw<11P>VksvKw`vng)v=t$|mnpW+Cs$W#z(8RoG}&KIR= z5|*v0cXCfdwV=l6SCo!jm9xyi8!_bHbJ`{D%_e~Pu+aLU<>S+_=LkGS>Cgoz?9-k;@CoI0GeUj13M>6Y}pdxI{l;5Sj)TgHI3B zno5g=O?i<|-oXu{dg8b6fASyW{VIF4*7L>2t5|>ukluoH-g-+25iABMLK2WH zxgJZ-ON~skk&#lnA8u>}m;oJtB;dYwR)nFFDOz%;MFrRJk|BMKfJktVIC3Nj3ew_W zeI;JmZihhsEylF9iUi74gP}w$S(-VJN~SU2=_&~6X6x~lBiu0r{y8_(=H?Wf0~n2` z?C1l-(S|k-DA?=n3!QJ55c~%7C9*Z5hf5|*p9`G_K0{wY#bf!gkZY*S+-1P0Q1d?y zHyE?sjliIGs4ef{^w&GAGWUP-YpoD%3h1J=Yfm}Ql$U&I_!LXj<^k0!({U5p`8D8u z<8eR@31w>s<4Zc;n4l){8ghOj>s_EeE*xPbp<5@aruBgE>`75X^=-1%6SCZ>lsMYg zRH6v`#Y5AQV!V*}z}e*4sVI32OFfnDwm@|inW|!4@KGo`pOmt;3yEqY@I{KoQqN@F zswDN&b)#f?EIadM;Mj! zIBl1}l@~&z&!Yy4EhQ~;C0lE>5{u*G<5N^PTm$X#h^@EUXXiF%wHpPFwiV{I)Ui2} z1wyzpaPRlr-<8CtJ74+3=ip(y=|!&*^9P$vogs1Tw!cHh^RPwTl73O0QVp%8RABZc z9MuhWHw~ReSDSoynlT3kzYhZSG!|wcuAq?h;5!t=-$B~neTB_A&7Vn1y^YY1PED|L zeUe=nL4K44Ph85aw#*{1iPsKlD6dA(mom6P&>ZwoXjO{4YDK}MINVhTr$39`($Bx1 zTj`c?2f>#Yg+#6ew(K}Vz$*M%%9QZHg&83?Ig$eY+mC5F!C=;M2146B&9J3c;l^a5 zFJrBMNl*%y1T6pN73=p&@Hz;PpzxpDzvtP)v@APTB}9ie)H;M8{YSV^FzRVa)h>g; zz9>V++I%z7Rd}yJ9)B9ow*(0!n2=@8k&xkMCRtJ`i`q^4u4aLebX$k7kV z=Bw7DiDxeF@e)fTS7k6OJay1Nt> z9EIRn@Ee3)1@|r@_n3(lMwdCATN$1OMe#<=PvcIKtvLZ^z{993C{FjV5iOMtuJNH@ zRBr|s^7Ah<2{&kHB{qS_r_>oR8_pV?r%QjVNRF~l-AWg*Y(rLy&dH6NHaqMT40G_T z#CT@Pj}Ga59C)7@lAWT~uDjUF*DyJ0at|_*g?OnkfYM-kQE}(1OTlcYdLJHoir!%S z__caE#SQ;Go<0-}8u3o)fXxn6!4I78oiHelk-AOl$0ngWgT88uw9mBQ9%1i53M`;L z1y*XhB}ND6=5uu#PG|D95RP*vw9A|}Y$Oljc+M47kP*-M<*=CO=;t&**7Xs4D5O2McpDa(*I$cJ7K-*!_8jCu@IUv}|hnPNkVxy#%< zg!sGmq~wza2-KvvZM-M*;6y+gMWtlN=}G10SPyST3y03&At82C44`K<&{?Zm+fs<}gK@rLq4;ec(xRA^7(lP6ARs$Hgt{$_ zu$R3iC1E+_0m1{T5}4-kjYsheLz;JR0AnlaW=iemH|&Y@@89>gP`wG}_>ijLdj^HP z$KNROU8M$HL#0yFSJ&DX4ltKtdpi4zs& zVz;om^rkCHDlZC;74?kc0tP7tqk~GHwn>?DvxbV3ii=ZV-t*mSX31)vqUZCfh%f~W zWR9pm_4mDP0K1lc#9Fu~&dcKKRbVhrr%D=T#b6xb!ug^RJuhyyxBbH%n{1#%J%$Nmc1OUOKC@8Raf$PlgX{xrMRzM+-v z4|Pk}38V*)AQzrKe6y6>gxVzI*Bc;m2kr$w18#;5fW(V`wcPmo#s57n`g@cqLE(oK zAiwJ>@0&h2JrFnLkihz~S`Kq183 z-skx?j_PpcaeQ$TXJZrY*W+Vn-yg5PlX$an9t!eEWc1ZVGKTU|70~AGU{FHWvNIwD zkV4WW^;m3iRBlxSx&bcBC%%`8T&4!aAbyn^kbiraipXP^7dXgMdBMKdoJ?a+ zUgt&i^)5)c4e!vTX)-$0?`w4+C=|`4f|HY{;WFmlY#E#(-d8kR;}HFWorY5Z*$Pe` z@vVesw$|xtYY(a=aEtwBia^@6Kw1JmD2ba%bhia08i z9r)fwRT+F?A3#H6)9y|V9E2*cyEGbl?$usjb3A#7KO8d4b%Q;u*cW`dCH<<4)o!;# z6_^7PN7YtDF^LI1Sc$MM1!GaDG(IP}HAy2Wt(@a1M`N0VOjXE4X5p6|Cbsh4 z6kH8-jGu7q(4*MM2G?9V5N4PbS?9(z*;gy^V`>Q+@sl$H6hRZzP=-kFt`wo1iS(>V zK}bFfW$I?OQ5@*OH{WX9DQ1`$gikcf=v1Inoq9J1+2?cJ)cqucML0hFjKjSq+YQbd z1)c8=ugZmRiHXky!@LDQTT7&Z(mFQDg|0jr^ex1=D?2J zq+c^{!D12U75I#YVL1{QtO*k*Vj$NPP{M4aaA+xfKwyN3pvJ2;0wlEOj)ZZGt>G)h zkfTj}i~84fM`K=_ns|WiI|FS0|A#mH>otK4g-L+Q0@90-<7$8*BytY9+Q+gfIRml} z{_$`kh_S?q5}!nFnByf?H5`Y}5+5qNhSQR?+CJunOn>%@qw83%AVn#^oC4%;rrmek zI~aAkyZwOg!dJ4t(H9D$Mum0(*uTQq@Lo)yOS9y{Tvc{XU$jP}CdZnN%vFHq`}*xu zbtrzO)%wha$UDj(%}aKq3>w!MJe4_5!yTcEG!*cc%VZFN&{0}I{2ptCBv|m+F6QACxr8D#$GBu~ul0_39us(b2r8f&d2OWf zeG<~XKt-4`Fy?qpFZt-Z^c0Z|Wg|L{z+|B1ia{lVO14vxpRTu72?TXOfk`xe93??o za{C)EVVFU^GJ={*-Z7eL+0@Ih<9BoI$xAK7}0trRHPbG0KaVc64w+lg9&_9tw zQlD?8HDDEfm}(6#3bC0ee2aR13_r+iw`A8xiX=kv4*C6YnX7Bq0D9D*Dp4CBGn4u= zutlWj{(;#K!8nB=KDB{3n;51VPLy~}8k53zO}klL9um_XzM$~jK|wJY1CllR@#n&C zdv_&8oLiy_$+Cm1&;M%fI+C7>zyNb^0<4A6{~N#eZ*P1F@|#vOGJt$0G9*Yl@6|ar zWOx=d%=vcoaAE{Wi6JB3<=zeIsVeHdg*8W^A0*oD(|w>n-uxilX{JhB|G=ACw=djz zopGMN`tovpjq3fb&cZ;pHin-nYLNNJSxS01lS3S7aio&2KVH;!6|-D7ELyU z$yQS(zGO=I)n#E?*uvdYtoR3Ps?NQi9h3d0>*E}Y5v>K6Bs|I$@aIc%^Rq^E6g9z< z9CO{Qs>U&M&XvwnEa-_fT>(}V&r|=E?X3z%@kn3eRtu_;AM zvwv8yYF8YU18gkha*7r0q5TL3cNq{&DhlSG1PfdpJu^cNQO>7Y)k-LDV^7a7NjyUH zKtr7$Dx3hx)0Kd!4as7;c0i-zC|$_sFM~(DX1}@GIC< zVRFF*?%j}&6FBnH<7v!Dy6cskv_I+G<>ZAN^f`RY5bSNG84a_nmZLY1nv*P8Bs*D( z-*TlGNNgOkmCm0jpAfY>dAGs=J!g?ZM{MRUAhn#9*ez|Y^kv>FS?l^l)T?J?0XR*R zS>C)5Bv2Zt(5##tnZ;_2!R2n4fNjTJ7@OmL#=1PvbNnNh`b2pwj0p_tu(fv{_2np*doZ6(!^SMHfyKSLKs z#`k^j09ic{N9Xee<@*dAHFVDi5C!h5=Y#~}8NV%WvkW9c5BSk= zqrp4823UvN?!id~yHA?^)@)O3ffPOybm0%|*W>|=m*kANMwo)2-|#}~5aV#Zc8;Q_ z7gE#EsitR*LsbOT^ZfO0Y@n7lhk^j8sRu|7BK{XM=XYN_L)BDAqd(?FEH#D2)wRKJ zxrR%}eCyqDB3(n(o;>#(baCdy>KNED71vlHoXOx6_y$(KrXKi6xJVsDARnC|VBcl~ zNS|fNrXGZ$GlZq_8r@|-E^lZ&^EZQop5HgprpFG@L!M=vq@5HU6dmRl=cOyL-R|JL zRj(9q*e|O;y(lwvE>@#tnLPNx&~Q&*lD!n$Ju*Q5DB6<#_5jVjn6m?k?eZYks|!en z*muY3o_0SOq?^ZD^hZ(-dms`XL>r#(%hPXj{=O~W$u$3fESMRonfpYfJ&sn2o?|5R zXo%zx1JoJ5kS{Z6@7|MP7*4G35dQ_S6RA4MrWC!BmAIHV8#q0GH?u4?L`*vDJl>s? zCT{6VF>##3oJcis(NQvmJY!1fFeCQ0VN0YMo22Z-RKCEVo-41I!(2%M24I($#-J_H zg+Bex_en7MrV4-um)3lQA647sC&UZP$#8A zC#q?UiZBDnC<4}=$dNPfNSP6O<*-~QrMx56Vzr`D@wBQZo4U(BmgbRR0rma}c_*U` zVrCHIQH|Qd!yadSlTsY9k1TFTa<$=>4yWY+$c~Z|5#O|kJU&Wj9ZlHT0mOiBh>1j; zgYWRDuT3pC0t>;V48DCxiX)#6~jj$URi!poc)EnPnu=xYPnPn7T^p#!?pz^he%0aQM+N=x<8;K8J^luhFin`_g@0Ec>jz-c(;*q1(T}}(_W>6h!!br=Y`WvcD;drvD)X6;4v-uX(rS@tb-59{8_KYTR57o!xutgfG<5YM zFpz-Hxm4X3k2;k3o*Z$R0>Z(uMeYGD4Yeg@jp7l&#);+_=2oT+z+vS~k}yQOjZn|| zY}9(eq7;tx$-`62_{r+ALW+~ua-pzR9KCW&mt{J4(Lz#~Nc-{I3Yw{1@;<+ zA-}PtI|Y){%I#o3wFg#jFOTCx+E5LOlMcC{`H<$3b}Tu4rUGe35HPg!UbIxs7HTb29cs-R6MC&oXtVoNpxB3M z0(^j@DT0_PurAc}i(8Yl3@6eFOl)@@UB2W>v!8u&V57!(^kRgWLs1h4)3T+AiX$3A zOfjCOEgd}3(*;xrcrR^H%}lhvJtTS18eRK^0;VS4QhC{${Zcr2vCYoy9(wsG#O(P?|1_$|9n1J}qK z(XX(H$e-5U_t)SxjykETGWei2C1SzbCSfHoy&}81g;S|kwn;6 zG1RP^I{*QMN+q_IqtN&}NbU`iq!Q2+2dC+x)y}4H`9Qyz$(2wfH+|?oDj7 zd|38C1J|rr>1Z3mlqhUVecLjH!Vu?K+g(i5pt=F$J)Jxt! z(5#;U(Ek6P;Gln5cn)mz;{}sLx>=W1gfqcmS@_=dnFXhK*cP{2_H@r>J*58-OoU7tbqQ2 zQn9#O6v#;)WqQSs03+L7s4GSpDb&N&Pc*5S!#O@y-Vw-=c*W0f%R!Kc8Tpb^Ml>!l1 zGz_z{!Sc|&@Kar%6Prr8Yt|*IGSOwU8_ja6N~}jvnF&rqU??OGy`N3;=`cibbbJI; z81(~YXc{$nM@=oO_feLb+fvXuPpeIw76YSKifFlx=aYbtjTr)^K9S(pek5=?RyE_c zR6>RE0;!?pvd4&aHI14O(SB+TxLUMj*g$|J;_-ASA$ie2jSD&i-gK-TQ2OIlN+21F~?hzdJ?a-k%25}3~*+)v3)t>ds$ld;! zJ>3wvXn4&YR5j#dYKzyCJK!ik(WwZ>wi$KCJz7_+zr+unR)x_g7CZT)88ZMez7 z;Lz6^2sUroh&>O|FOWTm7dN_fI*n}=LIJ*=J;`cThFz;+)vbHg-i=kos5_2%*|N_nL>cxe_Fd<4i;Q?MnrZFWK{vFfShE?BlZf=D0}lZyDvzc|CawuSuocd?*P^ zZ^>%kb#RXj<+Kivsd5_&i*vN?4OPrjijC*AmDb=AAt|V-EQHw#YHWl-%uAQufkRpeoFnq$Q3}3;aDFC&>b$c><_X8kHaBG< zb!MfYw zyM`U0n{0GtJ%U*sM12beoB&T}u55vrcDBihZg;gq9_H45u_-&`UV` z5PdpNfk@6z_2X){cAK2W3Zcqmhj_N`2N#c~%SB70Y`90eswX=`pKHicJg`m&qEc)S zcL_LsvuIh0MxGwXn8y3{hwU4SJqMXV1T8s7@%gD4-nhO!;KGtQYRS9ob) zM4G{3@TaK2H;4}AAl;Tig;%Cy&a4&;PHHeqUoE@%`o@SUo<#|#^m2E>Ijl`E6EtN+ za6`4eBh)}DGscg5Y@$2RtUdf`Q}ISOGP`=_JzjXFJ#DcNp1dH<#G2EyUyYASW{*73 z_brmIlG`QY2HL$%tWo=IKKmUp#)UT-lH(*fQ*7hc4~ybZfKs;dDlO=sRFt<2h>sVP zR8pBaT(gtt4uSGpY!fm!rU!Eu>4E#p5bKv9h1jySPq6yHdN;#{9fWyO7M>^0xsp>k%^^Hv1py{hq_BHKGCPqG zS-%RLSg}eXQcXIa5*z}fx|r67$(S0g2rTnOsO$9NbwAOzcMh@6_$sAnys!=~hXmZrgj=@-o zZ#wynu^`x_xx?DeBA&&sZFxte4n1mfjJ54chys+>$dKGY{Gc;9?W{ODQEe5jS#lQd zS7ZOf`Z#o37Q0=~fpxrrWh->;AL08KZ!-r6)7i#!*aEY{BDLYfwc~->2x~8ED7z_Y zpen^UJjpmNQ`Z6)_lVl?Dz_zFGMt@?5|#+a+}KWuMr;O!gJ&k`L7n7UvvyF!H(+U@ zX>6GQ+FQO!lr2p8%cEEqHn_cfA`z_`!AT*0sSpbWTGlt>Z38kBaj2Ip&Kat3(M6XO zSNnR2Yhf%Z>@LNZ*UoXO`}H+KduipvmGY_2RCMDTTj#9CqX*K%4{omVhKtZ>E?D(a zj_n1y6$v_>x@tboZ9tD-K%kix9xTERz)%qQVRsDhzsgF~UD!*U*{~0%^+RA8j_VUq zGC27M4DM=rpxGg<933In$Y**CYz%rD@}#2EV`6fKjcEH+TJM$CaGQ%&rqQmKq+>$Th-s@dPMQh#z@+g2RZde zd@nXtn!kGMk3mkkB;LtRtXTN5PycYQL_}}WO^GSq%%w9>km~|(V4Glr7`ImD2N%~p zxLLOJbRf^RZG~n858WuYaikaT?zdPMH&zD-{b}8Ga;=KUswDlff^I)R6?mM&44~;U(H#Zb*Vkj0}WDC=ZwC53u`26-JdXQq8RQ(`Aj_>2(~- zqvrb)1~1ajNsx9@cfCCs-#Rs&5_M0;hMuzK5KTksfq)@6+^REBeT;EAo27L>={|gL zCJZmFTcR>(tbv+kjanO3oi)il!QI`FwoR?u z8JFI=bnk3`<+x2h5oPSf_C&Gd<#$Z5tUdgac2aq@dr`+>1ASO&=!Jc#gh-}!x%d55 zWCV#>G>?yQ5bk!GjMC2H!AgQmO zxQ7<|=2EUPGA)|5L4JE+fQ(9_ETl|ffiy1$u`!-n#$-9i+;o}c9e6ZStd6ZpNB0)T z=EFc%c6xg9zK1cP_(Bt%@c!7^{e(x>{l-2u!{cB(&kK6j+h=ND$7J#=_y?T@ypS;R zGBDqX+z-?Zp?bg$kw(oph)oJnaZQxe6y%x8mwdpIv;-!7B*&Nd)FlrJTv|P&x^21J z^I(l0?7k@OLj7j&_B~KLwc9Vvv|6b5EdKrb{S!2EJ7Go|~iz2Pu&exgPOmRsg+W|6{?F-Tv z%&SCMTHA&^as^y&{Ut`1e6<~$Y~UKe%lNWLf)7UcXhT7i>ftuRWr;~?SIQU?@J)wg zDOD9eL5juzj^Y$fB`vEiW}+vx8Pxp*Iaqia8Nc)7MPZouKGZSUr3A12jK)-LZA${Y znX>A>^#p-G?|2GCUEA&$xDc;O(%NR&*QgyI`M+`|}*@y8XEN!P7IJZllIt`Pm4_C3|*pC>)T_AFhu+eP_XR z@f0wN0kZ=EzW(8`NlV2b%NPTDfbawSz+>Fm~JaU)%4LwE+iq8VQ(w7V%pzD{cNXO)a`tvln!ph$R z5n9N*NwDNBIfiKtB`wz0dqbX&VD9X=o(AClkn*%o4pqwk}8eCixR)_*P!JZ>u8dY?Ic z0aLLMtKN&%n&(KJ5Kr$RXPOp1fSaVxCa+Ydcq=n36D~TDVQNcpUsdvst+^Oy2PUR2 zvdVZ=zNi^u_FLVOY(cSFEsjfY`fAWl{g-+SX;{8}Ni$KBfo$Pk?(Z8PFwc>=vRG_O zB4U)edKx@S{87i!%Z&wY%5mtxfYjTJNZ(0AvbVG|y96xDa9uQI)rzrZibWXKBBjp^ z_CC{F><8Jsk)1x4q3ue5J>N;C($d1+eVi&}Qg3T}fje^H3FjT*_oj#8QTVhWvKAQI z^ltOn;=v04Bb^AHJ>pRyTA_+Coh{<6huIX81kqv@e>ZY^{#5ptN|W>sKjIZeSdd36 zyp}*vBq$IL-wI6$k(nw(()b51AncsTE)2Er$?BRv7LOh|SPY_juXub1%t(~WpF6Db z<|ZaDGoZflz923Ipq~Ura5*ExZZY*;u!~}BMe;wx>gK(T(nG>i!|u{|PA5ea!0v^A zKW0rAN%DpayPt06T6YBFJn}1R0b=|;UX|3uGkGjIvBJxF7nl8K6NTx%d9SJO+6ad- z9K&4cByF)O>08S{?d)*2zHvFt5VBK!`0LSGR zOsVWS@AI*j^hOW=;;85n&2a&$E+JeFs?CCKEV6SnFDxChep$y(zO!#CnVh!RM=ZBX zc;XMF&lGVUsXV-n$#)FSp)bNsPf$iF@e#mapV?U6mS1f0D+m_xwVdM@=I&|QH-&q} z3#Bho?~oKoN-BvS|Or&ws6G z1=*u;9w{OtV}_;J31MzV#}d*H`fDGY0kw$6Ni;H<9?toQ23wgS)b<%VFg0YEL3Y{jcqw}0=(ryO5%OxevG5OYtMK&@kccI zwV-Cqw3$?6P>nK?&q|KU`e=Dq;ZW2~!E7w$Zlx-zDE-OtJ5@=-H&zd;AIRW#N{3<^ zC?`c(!S?L#hGYvxM4@PL%hL#X_0H|W7ucRc-R^X>O)nBZlWWN;+8201gX7La=cYCv zT!&mfO=9kavk~2KlAJ)=?+bSDX>E(|?Ab0j+QIM~B`)bKPxBsX3Gxc6$bierKqMLr zHNQ@Pb=d^5UjZ!83Ig`HQ2u?H>2J=!@7ue6cL^c@X|8i0k+{|?&xCypzsUgWBKsT4 zmKHy}#Sbu%I_l6&8JT6l`ERdiv#9R{0R3@kcl>=l10rLLn&c$qVsvg$U; zwZ8@^w7Ti_Cb=h$v4*rXS%kn>4+6U>vv4mkbeQml5uquin@WPnM!n;_xQSwO*0#B5 zyCW170zrMu+*+Bpp>DV#dE>&rB+37HoKz$sK$os^%Nm4$D$Cffc9@c%#x1fcJcW&1 zbPPh=*$yH%;8GE!TCi2DADc+`GAUw(-ASv~Ar8MeR#W#Yxt%{}P!JBbq zi?gp(tD{y`RGG#LjvJj=aW#NKdqIHWd7ra+BuKt~Q|rAESl{I>6fcSTI6K)uK7WCB zje$a#GyMbeoGd=!R#FQOc2znm@GP9d5$HV8v|wvQmXbv~AUnP+a;8h|J+fz9$LAi+ z()o4^jT|phaUL{ii58J_YIeb?2-eeKTQwRP0~pzrx69ky@!NR#W>3MMf+w|E9x+Pa zw0+6-6lR4vsD=iKZk`_AHDT$U`m4LmnkOzu}C=7_jU!}dG^x?8ofZ-WYNifptCKDuNgOVOFQ27 zb+i(-b)IH7_1uzh&6nBg8}A(f#6gX?Iad5-7jU_YknkU`l7ciG2l(6p8(<*;8(`Ug zF-L5zJ{juU{~N*Ij978$o{vZYlOVtX?mRNRxhGw~sb~&61hU~2xrAu<{2YmN7C}(& zz<-D-6H`CGeIeaJDy3Tz*r7Z-OlRA?aV)etr>T5%SFQdL1=Gd+YjuJ!SGxkn6ZhFgzCizoPvy5=LSuKT2g}&y z0tYDv6Hy05ixLlOMDd6V?;{Z|BLgC_$b+;)iuc-iZWn^tN%4a?eOCPSyu{`s?!Zy8 zE6SHb?XQfEwaCTqo93}0HtP$aRTMWaFqGP{!}HcmX>sVhhRmRQy1if`tXcgsoIus8 zH(3>Ejn8;6&}D9P3n$W2EaJl_PPzOt$M_*YkyNyT)$n@XTZIW?eEw0b?LZ2 zy$-#S%j>7Bpv!0NdQI-eppEBJaxLLlp9h&rRyDJ!g?y0aVms}sK-q4; z$c_SqOyq9YV5E>ECfGgEe9Tld1{Wj$s$UIbh)iMUH$p+z0WlAJMrd8waZ~5b+?kU? zV^0M#PMOP5VgmR}ovQ{>%58R{_TyD*JnWBNWk3<2-(G9~QzQHj#d^RV^?z2Y-+Qf5 z4+aJXfZzpuel|3~p`L@OxdD}}p}wK1wf&!U@5QOeC&nhPDaIwmsA`zz0sEw7#6iK1 zFPShA0jB{n^#R}C0wKKt^zKj40L%MN1L&>2YLbEqyi}r6LNu>WEMszo|n1Y-s>n{|7ige~oPo?d&7~1lpaAHX1~YpHAezX+mF{3%@k10Dz% z@V^>I=!aht2;1rcs(&g1L>fh`EDR;Atjrv&e?2szwVQzTSHMPCz(A1y>^R^^3ScY0 z7@FUq|0px|o9+D?_i-ETYbgL_74X6T3vL+oFL6cxYjwZIv;Mess}Ha)aJbia4m7{U zv$V6PSo}%$f z^goOg;KaNx1Nj>Ibz!}q(DB;;1pU`Cd#_PnmzDX6dSU_)>;BIi{l_9RuK|Ap09e_7 z+HZwqel~H_zjX7rT(#FHzb-5D^JLe*pZwp-9{<}Hd=3A)RLDJ|7-ZyTJ=BSC2ao*{*|lz8uqnB`%l<1yMKcHa}44&=4-*; zpO{xJ|AhH_F2n!2Grrcw{fWBn`Y%xbw^Huw?!Q*|{7K~R`EQASAD%z7K3@~O)>8aQ z;NwA?@io(HX|0b%0z2wMy$BzJ&! zkUzN@zumF_bhG+nivIq4e*f|(wUyT_uM<~)vRtSBYnI!b&TsL&uIVOcwW1m zf0}sz$7B3;+~p@t=HUNx>Hb#~=D&UZZ_MS-K@-61*Pq%s0PKH7U}VHW0PmW=r7hCG O(FVj@M23F*_WuFL)$*MH diff --git a/2019robot/gradle/wrapper/gradle-wrapper.properties b/2019robot/gradle/wrapper/gradle-wrapper.properties deleted file mode 100644 index d08253c..0000000 --- a/2019robot/gradle/wrapper/gradle-wrapper.properties +++ /dev/null @@ -1,5 +0,0 @@ -distributionBase=GRADLE_USER_HOME -distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-bin.zip -zipStoreBase=GRADLE_USER_HOME -zipStorePath=permwrapper/dists diff --git a/2019robot/gradlew b/2019robot/gradlew deleted file mode 100644 index af6708f..0000000 --- a/2019robot/gradlew +++ /dev/null @@ -1,172 +0,0 @@ -#!/usr/bin/env sh - -############################################################################## -## -## Gradle start up script for UN*X -## -############################################################################## - -# Attempt to set APP_HOME -# Resolve links: $0 may be a link -PRG="$0" -# Need this for relative symlinks. -while [ -h "$PRG" ] ; do - ls=`ls -ld "$PRG"` - link=`expr "$ls" : '.*-> \(.*\)$'` - if expr "$link" : '/.*' > /dev/null; then - PRG="$link" - else - PRG=`dirname "$PRG"`"/$link" - fi -done -SAVED="`pwd`" -cd "`dirname \"$PRG\"`/" >/dev/null -APP_HOME="`pwd -P`" -cd "$SAVED" >/dev/null - -APP_NAME="Gradle" -APP_BASE_NAME=`basename "$0"` - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m"' - -# Use the maximum available, or set MAX_FD != -1 to use that value. -MAX_FD="maximum" - -warn () { - echo "$*" -} - -die () { - echo - echo "$*" - echo - exit 1 -} - -# OS specific support (must be 'true' or 'false'). -cygwin=false -msys=false -darwin=false -nonstop=false -case "`uname`" in - CYGWIN* ) - cygwin=true - ;; - Darwin* ) - darwin=true - ;; - MINGW* ) - msys=true - ;; - NONSTOP* ) - nonstop=true - ;; -esac - -CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar - -# Determine the Java command to use to start the JVM. -if [ -n "$JAVA_HOME" ] ; then - if [ -x "$JAVA_HOME/jre/sh/java" ] ; then - # IBM's JDK on AIX uses strange locations for the executables - JAVACMD="$JAVA_HOME/jre/sh/java" - else - JAVACMD="$JAVA_HOME/bin/java" - fi - if [ ! -x "$JAVACMD" ] ; then - die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME - -Please set the JAVA_HOME variable in your environment to match the -location of your Java installation." - fi -else - JAVACMD="java" - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. - -Please set the JAVA_HOME variable in your environment to match the -location of your Java installation." -fi - -# Increase the maximum file descriptors if we can. -if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then - MAX_FD_LIMIT=`ulimit -H -n` - if [ $? -eq 0 ] ; then - if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then - MAX_FD="$MAX_FD_LIMIT" - fi - ulimit -n $MAX_FD - if [ $? -ne 0 ] ; then - warn "Could not set maximum file descriptor limit: $MAX_FD" - fi - else - warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" - fi -fi - -# For Darwin, add options to specify how the application appears in the dock -if $darwin; then - GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" -fi - -# For Cygwin, switch paths to Windows format before running java -if $cygwin ; then - APP_HOME=`cygpath --path --mixed "$APP_HOME"` - CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` - JAVACMD=`cygpath --unix "$JAVACMD"` - - # We build the pattern for arguments to be converted via cygpath - ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` - SEP="" - for dir in $ROOTDIRSRAW ; do - ROOTDIRS="$ROOTDIRS$SEP$dir" - SEP="|" - done - OURCYGPATTERN="(^($ROOTDIRS))" - # Add a user-defined pattern to the cygpath arguments - if [ "$GRADLE_CYGPATTERN" != "" ] ; then - OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" - fi - # Now convert the arguments - kludge to limit ourselves to /bin/sh - i=0 - for arg in "$@" ; do - CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` - CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option - - if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition - eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` - else - eval `echo args$i`="\"$arg\"" - fi - i=$((i+1)) - done - case $i in - (0) set -- ;; - (1) set -- "$args0" ;; - (2) set -- "$args0" "$args1" ;; - (3) set -- "$args0" "$args1" "$args2" ;; - (4) set -- "$args0" "$args1" "$args2" "$args3" ;; - (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; - (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; - (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; - (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; - (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; - esac -fi - -# Escape application args -save () { - for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done - echo " " -} -APP_ARGS=$(save "$@") - -# Collect all arguments for the java command, following the shell quoting and substitution rules -eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" - -# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong -if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then - cd "$(dirname "$0")" -fi - -exec "$JAVACMD" "$@" diff --git a/2019robot/gradlew.bat b/2019robot/gradlew.bat deleted file mode 100644 index 6d57edc..0000000 --- a/2019robot/gradlew.bat +++ /dev/null @@ -1,84 +0,0 @@ -@if "%DEBUG%" == "" @echo off -@rem ########################################################################## -@rem -@rem Gradle startup script for Windows -@rem -@rem ########################################################################## - -@rem Set local scope for the variables with windows NT shell -if "%OS%"=="Windows_NT" setlocal - -set DIRNAME=%~dp0 -if "%DIRNAME%" == "" set DIRNAME=. -set APP_BASE_NAME=%~n0 -set APP_HOME=%DIRNAME% - -@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS="-Xmx64m" - -@rem Find java.exe -if defined JAVA_HOME goto findJavaFromJavaHome - -set JAVA_EXE=java.exe -%JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto init - -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:findJavaFromJavaHome -set JAVA_HOME=%JAVA_HOME:"=% -set JAVA_EXE=%JAVA_HOME%/bin/java.exe - -if exist "%JAVA_EXE%" goto init - -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:init -@rem Get command-line arguments, handling Windows variants - -if not "%OS%" == "Windows_NT" goto win9xME_args - -:win9xME_args -@rem Slurp the command line arguments. -set CMD_LINE_ARGS= -set _SKIP=2 - -:win9xME_args_slurp -if "x%~1" == "x" goto execute - -set CMD_LINE_ARGS=%* - -:execute -@rem Setup the command line - -set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar - -@rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% - -:end -@rem End local scope for the variables with windows NT shell -if "%ERRORLEVEL%"=="0" goto mainEnd - -:fail -rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of -rem the _cmd.exe /c_ return code! -if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 -exit /b 1 - -:mainEnd -if "%OS%"=="Windows_NT" endlocal - -:omega diff --git a/2019robot/settings.gradle b/2019robot/settings.gradle deleted file mode 100644 index b0f4d48..0000000 --- a/2019robot/settings.gradle +++ /dev/null @@ -1,25 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2019' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - frcHome = new File(publicFolder, "frc${frcYear}") - } else { - def userFolder = System.getProperty("user.home") - frcHome = new File(userFolder, "frc${frcYear}") - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} diff --git a/2019robot/src/main/deploy/example.txt b/2019robot/src/main/deploy/example.txt deleted file mode 100644 index 70c79b6..0000000 --- a/2019robot/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/2019robot/src/main/java/buttons/XBoxTriggerButton.java b/2019robot/src/main/java/buttons/XBoxTriggerButton.java deleted file mode 100644 index 7abfe7c..0000000 --- a/2019robot/src/main/java/buttons/XBoxTriggerButton.java +++ /dev/null @@ -1,61 +0,0 @@ -package buttons; - -import org.usfirst.frc4388.controller.XboxController; - -import edu.wpi.first.wpilibj.buttons.Button; - -public class XBoxTriggerButton extends Button -{ - public static final int RIGHT_TRIGGER = 0; - public static final int LEFT_TRIGGER = 1; - public static final int RIGHT_AXIS_UP_TRIGGER = 2; - public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; - public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; - public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; - public static final int LEFT_AXIS_UP_TRIGGER = 6; - public static final int LEFT_AXIS_DOWN_TRIGGER = 7; - public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; - public static final int LEFT_AXIS_LEFT_TRIGGER = 9; - - private XboxController m_controller; - private int m_trigger; - - public XBoxTriggerButton(XboxController controller, int trigger) { - m_controller = controller; - m_trigger = trigger; - } - - public boolean get() { - if (m_trigger == RIGHT_TRIGGER) { - return m_controller.getRightTrigger(); - } - else if (m_trigger == LEFT_TRIGGER) { - return m_controller.getLeftTrigger(); - } - else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { - return m_controller.getRightAxisUpTrigger(); - } - else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { - return m_controller.getRightAxisDownTrigger(); - } - else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { - return m_controller.getRightAxisRightTrigger(); - } - else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { - return m_controller.getRightAxisLeftTrigger(); - } - else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { - return m_controller.getLeftAxisUpTrigger(); - } - else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { - return m_controller.getLeftAxisDownTrigger(); - } - else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { - return m_controller.getLeftAxisRightTrigger(); - } - else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { - return m_controller.getLeftAxisLeftTrigger(); - } - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java deleted file mode 100644 index d8f08dc..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java +++ /dev/null @@ -1,12 +0,0 @@ -package org.usfirst.frc4388.controller; - -public interface IHandController { - - public double getLeftXAxis(); - - public double getLeftYAxis(); - - public double getRightXAxis(); - - public double getRightYAxis(); -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java deleted file mode 100644 index 040282a..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java +++ /dev/null @@ -1,205 +0,0 @@ - -package org.usfirst.frc4388.controller; - -import edu.wpi.first.wpilibj.Joystick; - -/** - * This is a wrapper for the WPILib Joystick class that represents an XBox - * controller. - * @author frc1675 - */ -public class XboxController implements IHandController -{ - public static final int LEFT_X_AXIS = 0; - public static final int LEFT_Y_AXIS = 1; - public static final int LEFT_TRIGGER_AXIS = 2; - public static final int RIGHT_TRIGGER_AXIS = 3; - public static final int RIGHT_X_AXIS = 4; - public static final int RIGHT_Y_AXIS = 5; - public static final int LEFT_RIGHT_DPAD_AXIS = 6; - public static final int TOP_BOTTOM_DPAD_AXIS = 6; - - public static final int A_BUTTON = 1; - public static final int B_BUTTON = 2; - public static final int X_BUTTON = 3; - public static final int Y_BUTTON = 4; - public static final int LEFT_BUMPER_BUTTON = 5; - public static final int RIGHT_BUMPER_BUTTON = 6; - public static final int BACK_BUTTON = 7; - public static final int START_BUTTON = 8; - - public static final int LEFT_JOYSTICK_BUTTON = 9; - public static final int RIGHT_JOYSTICK_BUTTON = 10; - - private static final double LEFT_DPAD_TOLERANCE = -0.9; - private static final double RIGHT_DPAD_TOLERANCE = 0.9; - private static final double BOTTOM_DPAD_TOLERANCE = -0.9; - private static final double TOP_DPAD_TOLERANCE = 0.9; - - private static final double LEFT_TRIGGER_TOLERANCE = 0.5; - private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; - - private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; - private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; - private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double DEADZONE = 0.1; - - private Joystick stick; - - public XboxController(int portNumber){ - stick = new Joystick(portNumber); - } - - public Joystick getJoyStick() { - return stick; - } - - private boolean inDeadZone(double input){ - boolean inDeadZone; - if(Math.abs(input) < DEADZONE){ - inDeadZone = true; - }else{ - inDeadZone = false; - } - return inDeadZone; - } - - private double getAxisWithDeadZoneCheck(double input){ - if(inDeadZone(input)){ - input = 0.0; - } - return input; - } - - public boolean getAButton(){ - return stick.getRawButton(A_BUTTON); - } - - public boolean getXButton(){ - return stick.getRawButton(X_BUTTON); - } - - public boolean getBButton(){ - return stick.getRawButton(B_BUTTON); - } - - public boolean getYButton(){ - return stick.getRawButton(Y_BUTTON); - } - - public boolean getBackButton(){ - return stick.getRawButton(BACK_BUTTON); - } - - public boolean getStartButton(){ - return stick.getRawButton(START_BUTTON); - } - - public boolean getLeftBumperButton(){ - return stick.getRawButton(LEFT_BUMPER_BUTTON); - } - - public boolean getRightBumperButton(){ - return stick.getRawButton(RIGHT_BUMPER_BUTTON); - } - - public boolean getLeftJoystickButton(){ - return stick.getRawButton(LEFT_JOYSTICK_BUTTON); - } - - public boolean getRightJoystickButton(){ - return stick.getRawButton(RIGHT_JOYSTICK_BUTTON); - } - - public double getLeftXAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_X_AXIS)); - } - - public double getLeftYAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_Y_AXIS)); - } - - public double getRightXAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_X_AXIS)); - } - - public double getRightYAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_Y_AXIS)); - } - - public double getLeftTriggerAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_TRIGGER_AXIS)); - } - - public double getRightTriggerAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_TRIGGER_AXIS)); - } - - /**Returns -1 if nothing is pressed, or the angle of the button pressed 0 = up, 90 = right, etc.*/ - public int getDpadAngle() { - return stick.getPOV(); - } - - public boolean getDPadLeft(){ - return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); - } - - public boolean getDPadRight(){ - return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); - } - - public boolean getDPadTop(){ - return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); - } - - /*public boolean getDPadBottom(){ - return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); - } -*/ - public boolean getLeftTrigger(){ - return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); - } - - public boolean getRightTrigger(){ - return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); - } - - public boolean getRightAxisUpTrigger(){ - return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); - } - - public boolean getRightAxisDownTrigger(){ - return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); - } - - public boolean getRightAxisLeftTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); - } - - public boolean getRightAxisRightTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); - } - - public boolean getLeftAxisUpTrigger(){ - return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); - } - - public boolean getLeftAxisDownTrigger(){ - return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); - } - - public boolean getLeftAxisLeftTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); - } - - public boolean getLeftAxisRightTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java deleted file mode 100644 index f6a18a1..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ /dev/null @@ -1,72 +0,0 @@ - -package org.usfirst.frc4388.robot; - - -/** - * A list of constants used by the rest of the robot code. This include physics - * constants as well as constants determined through calibrations. - */ - -public class Constants { - - public static double kDriveWheelDiameterInches = 6.04; - public static double kTrackLengthInches = 10; - public static double kTrackWidthInches = 26.5; - public static double kTrackEffectiveDiameter = (kTrackWidthInches * kTrackWidthInches + kTrackLengthInches * kTrackLengthInches) / kTrackWidthInches; - public static double kTrackScrubFactor = 0.75; - - // Drive constants - public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0; - public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0; - public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0; - public static double kDriveStraightBasicYawErrorDivisor = 20.0; // steer parameter will be -yawError divided by this - public static double kDriveStraightBasicMaxSteerMagnitude = 0.8; // keep absolute value of steer parameter below this - public static double kDriveTurnBasicTankMotorOutput = 0.4; - public static double kDriveTurnBasicSingleMotorOutput = 0.15; - public static double kElevatorWheelDiameterInches = 1; - // Encoders - public static int kDriveEncoderTicksPerRev = 4096; - public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI); - - // Elevator - public static int kArmEncoderTickesPerRev = 256; - public static double kArmInchesOfTravelPerRev = 3.75; - public static double kArmEncoderTicksPerInch = 126.36; - public static double kArmBasicPercentOutputUp = -0.85; - public static double kArmBasicPercentOutputDown =.7; - - // CONTROL LOOP GAINS - - // PID gains for drive velocity loop (LOW GEAR) - // Units: error is 4096 counts/rev. Max output is +/- 1023 units. - public static double kDriveVelocityKp = 1.0; - public static double kDriveVelocityKi = 0.0; - public static double kDriveVelocityKd = 6.0; - public static double kDriveVelocityKf = 0.5; - public static int kDriveVelocityIZone = 0; - public static double kDriveVelocityRampRate = 0.0; - public static int kDriveVelocityAllowableError = 0; - - // PID gains for drive base lock loop - // Units: error is 4096 counts/rev. Max output is +/- 1023 units. - public static double kDriveBaseLockKp = 0.5; - public static double kDriveBaseLockKi = 0; - public static double kDriveBaseLockKd = 0; - public static double kDriveBaseLockKf = 0; - public static int kDriveBaseLockIZone = 0; - public static double kDriveBaseLockRampRate = 0; - public static int kDriveBaseLockAllowableError = 10; - - // PID gains for constant heading velocity control - // Units: Error is degrees. Output is inches/second difference to - // left/right. - public static double kDriveHeadingVelocityKp = 4.0; // 6.0; - public static double kDriveHeadingVelocityKi = 0.0; - public static double kDriveHeadingVelocityKd = 50.0; - - // Path following constants - public static double kPathFollowingLookahead = 24.0; // inches - public static double kPathFollowingMaxVel = 120.0; // inches/sec - public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2 - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java deleted file mode 100644 index 91b4cae..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.usfirst.frc4388.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -/** - * Do NOT add any static variables to this class, or any initialization at all. - * Unless you know what you are doing, do not modify this file except to - * change the parameter class to the startRobot call. - */ -public final class Main { - private Main() { - } - - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java deleted file mode 100644 index 4fc1133..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ /dev/null @@ -1,101 +0,0 @@ - - - -package org.usfirst.frc4388.robot; - -import buttons.XBoxTriggerButton; -import org.usfirst.frc4388.controller.IHandController; -import org.usfirst.frc4388.controller.XboxController; -import org.usfirst.frc4388.robot.commands.*; - - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.buttons.*; -import org.usfirst.frc4388.robot.subsystems.*; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; -import org.usfirst.frc4388.robot.subsystems.Drive; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.buttons.Button; -import edu.wpi.first.wpilibj.buttons.InternalButton; -import edu.wpi.first.wpilibj.buttons.JoystickButton; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import jaci.pathfinder.Pathfinder; - - - -public class OI -{ - private static OI instance; - - private XboxController m_driverXbox; - private XboxController m_operatorXbox; - - private OI() - { - try - { - // Driver controller - m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID); - m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); - - /* XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); - CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - CarriageIntake.whenReleased(new IntakeSetSpeed(0.0)); - - XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER); - CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); - CarriageEject.whenReleased(new IntakeSetSpeed(0.0)); - */ - JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); - climbUp.whenPressed(new InitiateClimber(true)); - climbUp.whenReleased(new InitiateClimber(false)); - - JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); - shiftUp.whenPressed(new DriveSpeedShift(true)); - // shiftUp.whenPressed(new LEDIndicators(true)); - - JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - shiftDown.whenPressed(new DriveSpeedShift(false)); - // shiftDown.whenPressed(new LEDIndicators(false)); - - - //Operator Xbox -/* - JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - openIntake.whenPressed(new IntakePosition(true)); - - JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); - CloseIntake.whenPressed(new IntakePosition(false)); - - SmartDashboard.putData("PRE GAME!!!!", new PreGame()); - */ - } catch (Exception e) { - System.err.println("An error occurred in the OI constructor"); - } - } - - public static OI getInstance() { - if(instance == null) { - instance = new OI(); - } - return instance; - } - - public IHandController getDriverController() { - return m_driverXbox; - } - - public IHandController getOperatorController() - { - return m_operatorXbox; - } - - public Joystick getOperatorJoystick() - { - return m_operatorXbox.getJoyStick(); - } - } - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java deleted file mode 100644 index 5d66455..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ /dev/null @@ -1,161 +0,0 @@ - -package org.usfirst.frc4388.robot; - -import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.CameraServer; -import edu.wpi.first.wpilibj.DriverStation; -//import edu.wpi.first.wpilibj.PowerDistributionPanel; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -//import org.usfirst.frc4388.controller.Robot.OperationMode; -import org.usfirst.frc4388.robot.commands.*; - -import org.usfirst.frc4388.robot.OI; -import org.usfirst.frc4388.robot.subsystems.*; -import org.usfirst.frc4388.utility.ControlLooper; -import org.usfirst.frc4388.robot.subsystems.Drive; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;; - -public class Robot extends IterativeRobot -{ - - public static OI oi; - - public static final Drive drive = new Drive(); - - - - public static final Arm arm = new Arm(); - public static final Climber climber = new Climber(); - public static final Pnumatics pnumatics = new Pnumatics(); - public static final long periodMS = 10; - public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS); - - public static enum OperationMode { TEST, COMPETITION }; - public static OperationMode operationMode = OperationMode.TEST; - - private SendableChooser operationModeChooser; - private SendableChooser RRautonTaskChooser; - private SendableChooser RLautonTaskChooser; - private SendableChooser LRautonTaskChooser; - private SendableChooser LLautonTaskChooser; - - private Command RRautonomousCommand; - private Command RLautonomousCommand; - private Command LRautonomousCommand; - private Command LLautonomousCommand; - - public void robotInit() - { - //Printing game data to riolog - String gameData = DriverStation.getInstance().getGameSpecificMessage(); - System.out.println(gameData); - CameraServer.getInstance().startAutomaticCapture(); - CameraServer.getInstance().putVideo("res", 300, 220); - - try { - oi = OI.getInstance(); - - controlLoop.addLoopable(drive); - controlLoop.addLoopable(arm); - - - operationModeChooser = new SendableChooser(); - operationModeChooser.addDefault("Test", OperationMode.TEST); - operationModeChooser.addObject("Competition", OperationMode.COMPETITION); - SmartDashboard.putData("Operation Mode", operationModeChooser); - - - - - - - - - //ledLights.setAllLightsOn(false); - } catch (Exception e) { - System.err.println("An error occurred in robotInit()"); - } - } - - public void disabledInit(){ - - } - - public void disabledPeriodic() { - Scheduler.getInstance().run(); - updateStatus(); - } - - public void autonomousInit() { - updateChoosers(); - - controlLoop.start(); - drive.endGyroCalibration(); - drive.resetEncoders(); - drive.resetGyro(); - drive.setIsRed(getAlliance().equals(Alliance.Red)); - - } - - - - public void autonomousPeriodic() { - Scheduler.getInstance().run(); - updateStatus(); - } - - public void teleopInit() { - if (RRautonomousCommand != null) RRautonomousCommand.cancel(); - if (RLautonomousCommand != null) RLautonomousCommand.cancel(); - if (LRautonomousCommand != null) LRautonomousCommand.cancel(); - if (LLautonomousCommand != null) LLautonomousCommand.cancel(); - drive.setToBrakeOnNeutral(false); - updateChoosers(); - controlLoop.start(); - drive.resetEncoders(); - drive.endGyroCalibration(); - - updateStatus(); - } - - - public void teleopPeriodic() - { - Scheduler.getInstance().run(); - updateStatus(); - } - - public void testPeriodic() { - LiveWindow.run(); - updateStatus(); - } - - private void updateChoosers() { - operationMode = (OperationMode)operationModeChooser.getSelected(); - RRautonomousCommand = (Command)RRautonTaskChooser.getSelected(); - RLautonomousCommand = (Command)RLautonTaskChooser.getSelected(); - LRautonomousCommand = (Command)LRautonTaskChooser.getSelected(); - LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); - } - - public Alliance getAlliance() - { - return m_ds.getAlliance(); - } - - public void updateStatus() - { - drive.updateStatus(operationMode); - arm.updateStatus(operationMode); - } - -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java deleted file mode 100644 index 0ef7d9e..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ /dev/null @@ -1,36 +0,0 @@ - -package org.usfirst.frc4388.robot; - - -public class RobotMap { - // USB Port IDs - public static final int DRIVER_JOYSTICK_1_USB_ID = 0; - public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; - - - // MOTORS - - public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; - public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; - - - public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; - public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; - - //carriage motors - public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; - public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; - - //Arm Motors - public static final int ARM_MOTOR1_ID = 6; - public static final int ARM_MOTOR2_ID = 7; - public static final int CLIMBER_CAN_ID = 10; - - - // Pneumatics - public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; - public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; - public static final int OPEN_INTAKE_PCM_ID = 4; - public static final int CLOSE_INTAKE_PCM_ID = 5; - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java deleted file mode 100644 index 8d724c4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java +++ /dev/null @@ -1,99 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class ArmBasic extends Command { - private double m_targetHeightInchesAboveFloor; - private double m_speed; - private boolean m_goingUp; - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - public static boolean isfinishedElevatorBasic; - - public ArmBasic(double targetHeightInchesAboveFloor) { - requires(Robot.arm); - m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; - } - - // Called just before this Command runs the first time - protected void initialize() - { - Robot.arm.setControlMode(DriveControlMode.RAW); - - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); - // start out at half speed, as crude way to reduce slippage - m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); -System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); - if (m_goingUp) { - m_speed = Constants.kArmBasicPercentOutputUp; - } - else { - m_speed = Constants.kArmBasicPercentOutputDown; - } - m_commandInitTimestamp = Timer.getFPGATimestamp(); - - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - Robot.arm.rawSetOutput(m_speed); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); - double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0); -System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor); - if (remaining < 0.0) { - finished = true; - - } - /*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - }*/ - - - if (!finished) { - SmartDashboard.putNumber("EB Dist", currentHeight); - } else { - SmartDashboard.putNumber("EB finDist", currentHeight); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp); - - isfinishedElevatorBasic = isFinished(); - - Robot.arm.rawSetOutput(0.0); - - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java deleted file mode 100644 index 21e328a..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import org.usfirst.frc4388.robot.subsystems.*; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class ArmSetSpeed extends Command { - - private double RaiseSpeed; - - // Constructor with speed - public ArmSetSpeed(double RaiseSpeed) { - this.RaiseSpeed = RaiseSpeed; - // requires(Robot.elevatorAuton); - } - - // Called just before this Command runs the first time - protected void initialize() { - //Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java deleted file mode 100644 index 5c25540..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveAbsoluteTurnMP extends Command -{ - private double absoluteTurnAngleDeg, maxTurnRateDegPerSec; - private MPSoftwareTurnType turnType; - - public DriveAbsoluteTurnMP(double absoluteTurnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { - requires(Robot.drive); - this.absoluteTurnAngleDeg = absoluteTurnAngleDeg; - this.maxTurnRateDegPerSec = maxTurnRateDegPerSec; - this.turnType = turnType; - } - - protected void initialize() { -// if (Robot.drive.isRed() == false) { -// absoluteTurnAngleDeg = absoluteTurnAngleDeg * -1; -// } - Robot.drive.setAbsoluteTurnMP(absoluteTurnAngleDeg, maxTurnRateDegPerSec, turnType); - } - - protected void execute() { - } - - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java deleted file mode 100644 index 9d2d3e4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveGyroReset extends Command -{ - public DriveGyroReset() { - requires(Robot.drive); - } - - @Override - protected void initialize() { - Robot.drive.resetGyro(); - Robot.drive.resetEncoders(); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java deleted file mode 100644 index 0bc270b..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveRelativeTurnPID extends Command -{ - private double relativeTurnAngleDeg; - private MPSoftwareTurnType turnType; - - public DriveRelativeTurnPID(double relativeTurnAngleDeg, MPSoftwareTurnType turnType) { - requires(Robot.drive); - this.relativeTurnAngleDeg = relativeTurnAngleDeg; - this.turnType = turnType; - } - - protected void initialize() { - Robot.drive.setRelativeTurnPID(relativeTurnAngleDeg, 0.3, 0.1, turnType); - } - - protected void execute() { - } - - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java deleted file mode 100644 index d3e8293..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveSpeedShift extends Command -{ - public boolean speed; - - public DriveSpeedShift(boolean speed) { - this.speed=speed; - requires(Robot.pnumatics); - } - - @Override - protected void initialize() { - Robot.pnumatics.setShiftState(speed); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java deleted file mode 100644 index a477dc7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java +++ /dev/null @@ -1,103 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class DriveStraightBasic extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; - private double m_speed; - private boolean m_goingBackwards; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - - public DriveStraightBasic(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - m_goingBackwards = (m_targetInches < 0.0); - } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.drive.resetGyro(); - Robot.drive.resetEncoders(); - Robot.drive.setControlMode(DriveControlMode.RAW); - // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); - m_commandInitTimestamp = Timer.getFPGATimestamp(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - if (m_useGyroLock) { - steer = - Robot.drive.getGyroAngleDeg() / 25.0; //TODO: tune - } - Robot.drive.rawMoveSteer(m_speed, steer); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); - if (remaining < 0.0) { - finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } - if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); - } else { - SmartDashboard.putNumber("DSB finDist", position); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.drive.rawMoveSteer(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java deleted file mode 100644 index 21fede7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java +++ /dev/null @@ -1,115 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class DriveStraightBasicAbs extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; - private double m_speed; - private boolean m_goingBackwards; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - private double m_targetGyroAngleDeg; // what we want the gyro to read as we're driving straight - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - - public DriveStraightBasicAbs(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - m_goingBackwards = (m_targetInches < 0.0); - } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } - - // Called just before this Command runs the first time - protected void initialize() { - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - if (m_useAbsolute) { - m_targetGyroAngleDeg = m_desiredAbsoluteAngle; - } else { - m_targetGyroAngleDeg = currentAngleDeg; - } - Robot.drive.resetEncoders(); - Robot.drive.setControlMode(DriveControlMode.RAW); - // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); - m_commandInitTimestamp = Timer.getFPGATimestamp(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - if (m_useGyroLock) { - double yawError = Robot.drive.getGyroAngleDeg() - m_targetGyroAngleDeg; - steer = -yawError / Constants.kDriveStraightBasicYawErrorDivisor; - if (steer < -Constants.kDriveStraightBasicMaxSteerMagnitude) { - steer = -Constants.kDriveStraightBasicMaxSteerMagnitude; - } else if (Constants.kDriveStraightBasicMaxSteerMagnitude < steer) { - steer = Constants.kDriveStraightBasicMaxSteerMagnitude; - } - } - Robot.drive.rawMoveSteer(m_speed, steer); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); - if (remaining < 0.0) { - finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } - if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); - } else { - SmartDashboard.putNumber("DSB finDist", position); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.drive.rawMoveSteer(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java deleted file mode 100644 index e205818..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java +++ /dev/null @@ -1,60 +0,0 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// Java from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - - -public class DriveStraightMP extends Command { - private double m_distanceInches; - private double m_maxVelocityInchesPerSec; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - - public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_distanceInches = distanceInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - // Called once after isFinished returns true - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java deleted file mode 100644 index 82223d9..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java +++ /dev/null @@ -1,151 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.BHRMathUtils; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveTurnBasic extends Command -{ - private boolean m_useAbsolute; // if true, we don't reset the gyro before starting the turn - private double m_turnAngleDeg; // either absolute or relative to current heading, depending on useAbsolute - private double m_maxTurnRateDegPerSec; - private MPSoftwareTurnType m_turnType; - private boolean m_turningLeft; - private double m_targetGyroAngleDeg; // what we want the gyro to read when we're done - - public DriveTurnBasic(boolean useAbsolute, double turnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { - requires(Robot.drive); - m_useAbsolute = useAbsolute; - m_turnAngleDeg = turnAngleDeg; - m_maxTurnRateDegPerSec = maxTurnRateDegPerSec; - m_turnType = turnType; - } - - protected void initialize() { - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - if (m_useAbsolute) { - m_targetGyroAngleDeg = m_turnAngleDeg; - } else { - m_targetGyroAngleDeg = currentAngleDeg + m_turnAngleDeg; - } - if ((m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.RIGHT_ARC)) { - m_turningLeft = true; // gyro angle will be DEcreasing, so make sure we aim for an angle in that direction - while (m_targetGyroAngleDeg >= currentAngleDeg) { - m_targetGyroAngleDeg = m_targetGyroAngleDeg - 360.0; - } - } else if ((m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.LEFT_ARC)) { - m_turningLeft = false; // gyro angle will be INcreasing, so make sure we aim for an angle in that direction - while (m_targetGyroAngleDeg <= currentAngleDeg) { - m_targetGyroAngleDeg = m_targetGyroAngleDeg + 360.0; - } - } else { // MPSoftwareTurnType.TANK -- need to determine based on values passed - if (m_useAbsolute) { // this is the only case where we have to DECIDE which direction to turn - m_turnAngleDeg = BHRMathUtils.normalizeAngle0To360(m_turnAngleDeg); - m_targetGyroAngleDeg = BHRMathUtils.adjustAccumAngleToDesired(currentAngleDeg, m_turnAngleDeg); - m_turningLeft = (m_targetGyroAngleDeg < currentAngleDeg); - } else { - m_turningLeft = (m_turnAngleDeg < 0); - } - } - System.out.println("Turning " + (m_turningLeft?"left":"right") + " from " + currentAngleDeg + " to " + m_targetGyroAngleDeg + " degrees"); - Robot.drive.setControlMode(DriveControlMode.RAW); - Robot.drive.resetEncoders(); - } - - protected void execute() { - double output = Constants.kDriveTurnBasicSingleMotorOutput; - - if (m_turnType == MPSoftwareTurnType.TANK) { - output = Constants.kDriveTurnBasicTankMotorOutput; - if (m_turningLeft) { - Robot.drive.rawDriveLeftRight(-output, -output); // left backward, right forward - } else { - Robot.drive.rawDriveLeftRight(output, output); // left forward, right backward - } -// for (CANTalonEncoder motorController : motorControllers) { -// //motorController.set(output); -// motorController.set(ControlMode.PercentOutput, output); -// } - } - else if (m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - Robot.drive.rawDriveLeftRight(2.0 * output, 0.0); // left forward double speed -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(0); -// motorController.set(ControlMode.PercentOutput, 0); -// } -// else { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - Robot.drive.rawDriveLeftRight(0.0, -2.0 * output); // right forward double speed -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// else { -// //motorController.set(0); -// motorController.set(ControlMode.PercentOutput, 0); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.LEFT_ARC) { - Robot.drive.rawDriveLeftRight(2.0 * output, -output); // left fwd 2x, right fwd 1x -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(1.0 * output); -// motorController.set(ControlMode.PercentOutput, 1.0 * output); -// } -// else { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.RIGHT_ARC) { - Robot.drive.rawDriveLeftRight(output, -2.0 * output); // left fwd 1x, right fwd 2x -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// else { -// //motorController.set(1.0 * output); -// motorController.set(ControlMode.PercentOutput, 1.0 * output); -// } -// } - } - } - - protected boolean isFinished() { - boolean finished; - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - - if (m_turningLeft) { - finished = currentAngleDeg <= m_targetGyroAngleDeg; - } else { - finished = currentAngleDeg >= m_targetGyroAngleDeg; - } - return finished; - } - - protected void end() { - Robot.drive.rawDriveLeftRight(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java deleted file mode 100644 index 8a6e37d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class InitiateClimber extends Command -{ - boolean climb; - - public InitiateClimber(boolean climb) { - this.climb=climb; - requires(Robot.climber); - } - - @Override - protected void initialize() { - Robot.climber.setClimbSpeed(climb); - } - - @Override - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - protected void end() { - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java deleted file mode 100644 index d70b56d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ /dev/null @@ -1,424 +0,0 @@ -package org.usfirst.frc4388.robot.subsystems; - -import java.util.ArrayList; - -import org.usfirst.frc4388.controller.XboxController; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.robot.commands.*; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.ControlLoopable; -import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.SoftwarePIDPositionController; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SensorCollection; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -public class Arm extends Subsystem implements ControlLoopable -{ - //PID encoder and motor - private CANTalonEncoder armMotor; - //private WPI_TalonSRX ArmLeft; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerMaxScale; - //private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPMaxScale; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowScale; - //private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowScale; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerSwitch; - //private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPSwitch; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowest; - //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowest; - - //PID target - private double targetPPosition; - - //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; - - //control mode for joystick control - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; - - private double periodMs; - - //Non Linear Joystick - public static final double STICK_DEADBAND = 0.02; - public static final double MOVE_NON_LINEARITY = 1.0; - private int moveNonLinear = 0; - private double moveScale = 1.0; - private double moveTrim = 0.0; - - private boolean isFinished; - private DifferentialDrive m_drive; - - //private LimitSwitchSource limitSwitch = new DigitalInput(1); - LimitSwitchSource limitSwitchSource; - - public boolean pressed; - SensorCollection isPressed; - - public boolean armControlMode = false; - // Motor controllers - //private ArrayList motorControllers = new ArrayList(); - - public Arm() - { - try - { - //PID Arm encoder and talon - armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); - - //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); - - //ArmMotor.setInverted(false); - - //Setting left Arm motor as follower - //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); - //ArmLeft.setInverted(false); - //ArmLeft.setNeutralMode(NeutralMode.Brake); - armMotor.setNeutralMode(NeutralMode.Brake); - armMotor.setSensorPhase(true); - //Limit Switch Left - //ArmLeft.overrideLimitSwitchesEnable(true); - //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - - //Limit Switch Right - //ArmMotor.overrideLimitSwitchesEnable(true); - //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - - - //Change This boi - - // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here - //ArmMotor.configReverseSoftLimitThreshold(5, 0); - //ArmMotor.configForwardSoftLimitEnable(true, 0); - //ArmMotor.configReverseSoftLimitEnable(true, 0); - - //sos - //ArmMotor.enableLimitSwitch(true, true); - - - - - - } - catch(Exception e) - { - System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); - } - } - - private double nonlinearStickCalcPositive(double move, double moveNonLinearity) - { - return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity); - } - - private double nonlinearStickCalcNegative(double move, double moveNonLinearity) - { - return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity); - } - - private boolean inDeadZone(double input) - { - boolean inDeadZone; - if (Math.abs(input) < STICK_DEADBAND) - { - inDeadZone = true; - } - else - { - inDeadZone = false; - } - - return inDeadZone; - } - - private double limitValue(double value) - { - if (value > 1.0) - { - value = 1.0; - } - else if (value < -1.0) - { - value = -1.0; - } - return value; - } - - public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity) - { - if (inDeadZone(move)) - { - return 0; - } - - move += trim; - move *= scale; - move = limitValue(move); - - int iterations = Math.abs(nonLinearFactor); - for (int i = 0; i < iterations; i++) - { - if (nonLinearFactor > 0) - { - move = nonlinearStickCalcPositive(move, wheelNonLinearity); - } - else - { - move = nonlinearStickCalcNegative(move, wheelNonLinearity); - } - } - return move; - } - - public void setArmMode() - { - setControlMode(DriveControlMode.JOYSTICK); - } - - public void resetArmEncoder() - { - armMotor.setSelectedSensorPosition(0, 0, 0); - } - - public void moveArmXbox() - { - double moveArmInput; - double armSafeZone = 15; - - double armPos = getEncoderArmPosition(); - - moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); - - //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); - - boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - - if(armTuningPressed == true) - { - armMotor.set(moveArmInput * 0.5); - } - else if(armTuningPressed == false) - { - armMotor.set(moveArmInput); - } - } - - -// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range - - - //PID encoder position - public double getEncoderArmPosition() - { - return armMotor.getPositionWorld(); - } - - public double getArmHeightInchesAboveFloor() - { - return armMotor.getPositionWorld(); - } - - public synchronized void setControlMode(DriveControlMode controlMode) - { - this.controlMode = controlMode; - - isFinished = false; - } - /* - public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); - } - - public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); - } - - public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); - } - - public void setArmPIDLowest(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); - } - */ - public void rawSetOutput(double output){ - armMotor.set(/*ControlMode.PercentOutput,*/ output); - } - - public void holdInPos() - { - armMotor.set(-0.43 * 0.2); - } - - public void stopMotors() - { - armMotor.set(0); - } - - public void isSwitchPressed() - { - pressed = false; - isPressed = armMotor.getSensorCollection(); - - if(isPressed.isFwdLimitSwitchClosed() == true) - { - if (controlMode == DriveControlMode.JOYSTICK) { - Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); - } - pressed = true; - } - else - { - if (controlMode == DriveControlMode.STOP_MOTORS){ - { - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - pressed = false; - } - } - - } - - //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; - - - - - - @Override - public void controlLoopUpdate() - { - if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) - { - moveArmXbox(); - } - else if (!isFinished) - { - //PID control mode - if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) - { - isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) - { - isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) - { - isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) - { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); - } - /* - else if(controlMode == DriveControlMode.RAW) - { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); - } - */ - } - } - - @Override - public void initDefaultCommand() - { - } - - @Override - public void periodic() - { - // isSwitchPressed(); - } - - @Override - public void setPeriodMs(long periodMs) - { - //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); - - this.periodMs = periodMs; - } - - public synchronized boolean isFinished() - { - return isFinished; - } - - public double getPeriodMs() - { - return periodMs; - } - - public void updateStatus(Robot.OperationMode operationMode) - { - if (operationMode == Robot.OperationMode.TEST) - { - try - { - SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); - //SmartDashboard.putData(pressed); - } - catch (Exception e) - { - System.err.println("Drivetrain update status error"); - } - } - } - - -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java deleted file mode 100644 index 3cacf76..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ /dev/null @@ -1,68 +0,0 @@ - - - - -package org.usfirst.frc4388.robot.subsystems; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.robot.commands.*; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - - -/** - * - */ -public class Climber extends Subsystem{ - - private WPI_TalonSRX Climber; - - public boolean on; - - public Climber(){ - - try{ - - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - - } catch (Exception e) { - - System.err.println("An error occurred in the climbing constructor"); - - } - } - - @Override - public void initDefaultCommand() { - - } - - - @Override - public void periodic() { - // Put code here to be run every loop - - } - - public void setClimbSpeed(boolean Climb) { - if (Climb==true) { - Climber.set(1.0); - } - if (Climb==false) { - Climber.set(0); - } -} - // Put methods for controlling this subsystem - // here. Call these from Commands. - { - // TODO Auto-generated method stub - - } - -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java deleted file mode 100644 index 3d29c64..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ /dev/null @@ -1,862 +0,0 @@ - -package org.usfirst.frc4388.robot.subsystems; - -import java.util.ArrayList; -import java.util.Set; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.OI; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.utility.AdaptivePurePursuitController; -import org.usfirst.frc4388.utility.BHRMathUtils; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.ControlLoopable; -import org.usfirst.frc4388.utility.Kinematics; -import org.usfirst.frc4388.utility.MMTalonPIDController; -import org.usfirst.frc4388.utility.MPSoftwarePIDController; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; -import org.usfirst.frc4388.utility.MPTalonPIDController; -import org.usfirst.frc4388.utility.MPTalonPIDPathController; -import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController; -//import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import org.usfirst.frc4388.utility.MotionProfilePoint; -//import org.usfirst.frc4388.utility.MotionProfileBoxCar; -import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.Path; -import org.usfirst.frc4388.utility.PathGenerator; -import org.usfirst.frc4388.utility.RigidTransform2d; -import org.usfirst.frc4388.utility.Rotation2d; -import org.usfirst.frc4388.utility.SoftwarePIDController; -import org.usfirst.frc4388.utility.Translation2d; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -//import com.ctre.PigeonImu; -//import com.ctre.PigeonImu.CalibrationMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; - -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -//import edu.wpi.first.wpilibj.DigitalInput; -//import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -//import edu.wpi.first.wpilibj.Solenoid; -//import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -/** - * - */ -public class Drive extends Subsystem implements ControlLoopable -{ - public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW}; - public static enum SpeedShiftState { HI, LO }; - public static enum ClimberState { DEPLOYED, RETRACTED }; - - public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches; - public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch; - - public static final double CLIMB_SPEED = 0.45; - - public static final double VOLTAGE_RAMP_RATE = 150; // Volts per second - public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full - public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full - - // Motion profile max velocities and accel times - public static final double MAX_TURN_RATE_DEG_PER_SEC = 320; - public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 120; //72; - public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC = 200; - public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320; - public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400; - public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270; - public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC = 400; - public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25; - public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80; - - - - - public static final double MP_STRAIGHT_T1 = 600; - public static final double MP_STRAIGHT_T2 = 300; - public static final double MP_TURN_T1 = 600; - public static final double MP_TURN_T2 = 300; - public static final double MP_MAX_TURN_T1 = 400; - public static final double MP_MAX_TURN_T2 = 200; - - // Motor controllers - private ArrayList motorControllers = new ArrayList(); - - private CANTalonEncoder leftDrive1; - private WPI_TalonSRX leftDrive2; -// private WPI_TalonSRX leftDrive3; - - private CANTalonEncoder rightDrive1; - private WPI_TalonSRX rightDrive2; -// private WPI_TalonSRX rightDrive3; - - private DifferentialDrive m_drive; - - //PID encoder and motor - private CANTalonEncoder elevatorRight; - private WPI_TalonSRX elevatorLeft; - - //private DigitalInput hopperSensorRed; - //private DigitalInput hopperSensorBlue; - - - - private double climbSpeed; - - private boolean isRed = true; - - private double periodMs; - private double lastControlLoopUpdatePeriod = 0.0; - private double lastControlLoopUpdateTimestamp = 0.0; - - // Pneumatics - //private Solenoid speedShift; - - // Input devices - public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0; - public static final int DRIVER_INPUT_JOYSTICK_TANK = 1; - public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2; - public static final int DRIVER_INPUT_XBOX_CHEESY = 3; - public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4; - public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5; - public static final int DRIVER_INPUT_WHEEL = 6; - - public static final double STEER_NON_LINEARITY = 0.5; - public static final double MOVE_NON_LINEARITY = 1.0; - - public static final double STICK_DEADBAND = 0.02; - - private int m_moveNonLinear = 0; - private int m_steerNonLinear = -3; - - private double m_moveScale = 1.0; - private double m_steerScale = 1.0; - - private double m_moveInput = 0.0; - private double m_steerInput = 0.0; - - private double m_moveOutput = 0.0; - private double m_steerOutput = 0.0; - - private double m_moveTrim = 0.0; - private double m_steerTrim = 0.0; - - private boolean isFinished; - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; - - private MPTalonPIDController mpStraightController; - private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons -// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni - private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); - - private MMTalonPIDController mmStraightController; - private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24); - - private MPSoftwarePIDController mpTurnController; // p i d a v g izone - private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels -// private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni - - private SoftwarePIDController pidTurnController; - //private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008 - private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008 - private double targetPIDAngle; - - private MPTalonPIDPathController mpPathController; - private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100); // 4 omni g=.3 - - //PID target - private double targetPIDPosition; - - private MPTalonPIDPathVelocityController mpPathVelocityController; - private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); - - private AdaptivePurePursuitController adaptivePursuitController; - private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); - - private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); - private RigidTransform2d currentPose = zeroPose; - private RigidTransform2d lastPose = zeroPose; - private double left_encoder_prev_distance_ = 0; - private double right_encoder_prev_distance_ = 0; - - //private PigeonImu gyroPigeon; - //private double[] yprPigeon = new double[3]; - private AHRS gyroNavX; - private boolean useGyroLock; - private double gyroLockAngleDeg; - //private double kPGyro = 0.04; - private double kPGyro = 0.0625; - private boolean isCalibrating = false; - private double gyroOffsetDeg = 0; - - public Drive() { - try { - leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); - leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID); -// leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID); - - rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); - rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); -// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); - - //gyroPigeon = new PigeonImu(leftDrive2); - gyroNavX = new AHRS(SPI.Port.kMXP); - - //hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID); - //hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID); - - //leftDrive1.clearStickyFaults(); - //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); - //leftDrive1.setNominalClosedLoopVoltage(12.0); - leftDrive1.clearStickyFaults(0); - leftDrive1.setInverted(false);//false on comp 2108, false on microbot - leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot - leftDrive1.setSafetyEnabled(false); - //leftDrive1.setCurrentLimit(15); - //leftDrive1.enableCurrentLimit(true); - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); - leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); - leftDrive1.configNominalOutputForward(+1.0f, 0); - leftDrive1.configNominalOutputReverse(-1.0f, 0); - - -// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// Driver.reportError("Could not detect left drive encoder encoder!", false); -// } - - - leftDrive2.setInverted(false);//false on comp 2108, false on microbot - leftDrive2.setSafetyEnabled(false); - leftDrive2.setNeutralMode(NeutralMode.Brake); - leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); - - - - //rightDrive1.clearStickyFaults(); - //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); - //rightDrive1.setNominalClosedLoopVoltage(12.0); - rightDrive1.clearStickyFaults(0); - rightDrive1.setInverted(true);//true on comp 2108, false on microbot - rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot - rightDrive1.setSafetyEnabled(false); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); - rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); - rightDrive1.configNominalOutputForward(+1.0f, 0); - rightDrive1.configNominalOutputReverse(-1.0f, 0); -// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// DriverStation.reportError("Could not detect right drive encoder encoder!", false); -// } - - - rightDrive2.setInverted(true);//True on comp 2108, false on microbot - rightDrive2.setSafetyEnabled(false); - rightDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); - - - - - motorControllers.add(leftDrive1); - motorControllers.add(rightDrive1); - - //m_drive = new RobotDrive(leftDrive1, rightDrive1); - //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); - //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true); - //m_drive.setSafetyEnabled(false); - m_drive = new DifferentialDrive(leftDrive1, rightDrive1); - //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true); //TODO URGENT: verify - //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify - m_drive.setSafetyEnabled(false); - - //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); - } - catch (Exception e) { - System.err.println("An error occurred in the DriveTrain constructor"); - } - } - - public void setToBrakeOnNeutral(boolean brakeVsCoast) { - if (brakeVsCoast) { - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive2.setNeutralMode(NeutralMode.Brake); - } else { - leftDrive1.setNeutralMode(NeutralMode.Coast); - leftDrive2.setNeutralMode(NeutralMode.Coast); - rightDrive1.setNeutralMode(NeutralMode.Coast); - rightDrive2.setNeutralMode(NeutralMode.Coast); - } - } - - @Override - public void initDefaultCommand() { - } - - public double getGyroAngleDeg() { - //return getGyroPigeonAngleDeg(); - return getGyroNavXAngleDeg(); - } - - //public double getGyroPigeonAngleDeg() { - // gyroPigeon.GetYawPitchRoll(yprPigeon); - // return -yprPigeon[0] + gyroOffsetDeg; - //} - - public double getGyroNavXAngleDeg() { - return gyroNavX.getAngle() + gyroOffsetDeg; - } - - public void resetGyro() { - //gyroPigeon.SetYaw(0); - gyroNavX.zeroYaw(); - } - - public void resetEncoders() { - mpStraightController.resetZeroPosition(); - } - - public void calibrateGyro() { - //gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature); - } - - public void endGyroCalibration() { - if (isCalibrating == true) { - isCalibrating = false; - } - } - - public void setGyroOffset(double offsetDeg) { - gyroOffsetDeg = offsetDeg; - } - - //public boolean isHopperSensorRedOn() { - // return hopperSensorRed.get(); - //} - - //public boolean isHopperSensorBlueOn() { - // return hopperSensorBlue.get(); - //} - - //public boolean isHopperSensorOn() { - // if (isRed() == true) { - // return isHopperSensorRedOn(); - // } - // else { - // return isHopperSensorBlueOn(); - // } - //} - - public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - mmStraightController.setPID(mmStraightPIDParams); - mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); - setControlMode(DriveControlMode.MOTION_MAGIC); - } - - public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - mpStraightController.setPID(mpStraightPIDParams); - mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); - setControlMode(DriveControlMode.MP_STRAIGHT); - } - - //public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - // double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - // mpStraightController.setPID(mpStraightPIDParams); - // mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); - // setControlMode(DriveControlMode.MP_STRAIGHT); - //} - - public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { - mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); - setControlMode(DriveControlMode.MP_TURN); - } - - //public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) { - // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); - // setControlMode(DriveControlMode.MP_TURN); - //} - - public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { - mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES); - setControlMode(DriveControlMode.MP_TURN); - } - - public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { - mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); - setControlMode(DriveControlMode.MP_TURN); - } - - //public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) { - // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); - // setControlMode(DriveControlMode.MP_TURN); - //} - - public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { - this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg(); - pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); - setControlMode(DriveControlMode.PID_TURN); - } - - public void setPathMP(PathGenerator path) { - mpPathController.setPID(mpPathPIDParams); - mpPathController.setMPPathTarget(path); - setControlMode(DriveControlMode.MP_PATH); - } - - public void setPathVelocityMP(PathGenerator path) { - mpPathVelocityController.setPID(mpPathPIDParams); - mpPathVelocityController.setMPPathTarget(path); - setControlMode(DriveControlMode.MP_PATH_VELOCITY); - } - - public void setPathAdaptivePursuit(Path path, boolean reversed) { - currentPose = zeroPose; - lastPose = zeroPose; - left_encoder_prev_distance_ = 0; - right_encoder_prev_distance_ = 0; - adaptivePursuitController.setPID(adaptivePursuitPIDParams); - adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); - setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); - } - - public void setDriveHold(boolean status) { - if (status) { - setControlMode(DriveControlMode.HOLD); - } - else { - setControlMode(DriveControlMode.JOYSTICK); - } - } - - public void updatePose() { - double left_distance = leftDrive1.getPositionWorld(); - double right_distance = rightDrive1.getPositionWorld(); - Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); - lastPose = currentPose; - currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); - left_encoder_prev_distance_ = left_distance; - right_encoder_prev_distance_ = right_distance; - } - - public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) { - return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle); - } - - /** - * Path Markers are an optional functionality that name the various - * Waypoints in a Path with a String. This can make defining set locations - * much easier. - * - * @return Set of Strings with Path Markers that the robot has crossed. - */ - public synchronized Set getPathMarkersCrossed() { - if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) { - return null; - } else { - return adaptivePursuitController.getMarkersCrossed(); - } - } - - public synchronized void setControlMode(DriveControlMode controlMode) { - this.controlMode = controlMode; - if (controlMode == DriveControlMode.JOYSTICK) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.MANUAL) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.CLIMB) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.HOLD) { - mpStraightController.setPID(mpHoldPIDParams); - //leftDrive1.changeControlMode(TalonControlMode.Position); - //leftDrive1.setPosition(0); - //leftDrive1.set(0); - //rightDrive1.changeControlMode(TalonControlMode.Position); - //rightDrive1.setPosition(0); - //rightDrive1.set(0); - leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - leftDrive1.set(ControlMode.Position, 0); - rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - rightDrive1.set(ControlMode.Position, 0); - } - isFinished = false; - } - - public synchronized void controlLoopUpdate() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time - lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; - } - lastControlLoopUpdateTimestamp = currentTimestamp; - - // Do the update - if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { - driveWithJoystick(); - } - else if (!isFinished) { - if (controlMode == DriveControlMode.MP_STRAIGHT) { - isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_TURN) { - isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.PID_TURN) { - isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_PATH) { - isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MOTION_MAGIC) { - isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) { - updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); - } - } - } - - public void setSpeed(double speed) { - if (speed == 0) { - setControlMode(DriveControlMode.JOYSTICK); - } - else { - setControlMode(DriveControlMode.MANUAL); - rightDrive1.set(-speed); - leftDrive1.set(speed); - } - } - - public void setClimbSpeed(double climbSpeed) { - this.climbSpeed = climbSpeed; - if (climbSpeed == 0) { - setControlMode(DriveControlMode.JOYSTICK); - } - else { - setControlMode(DriveControlMode.CLIMB); - } - } - - public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) { - if (snapToAbsolute0or180) { - gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg()); - } - else { - gyroLockAngleDeg = getGyroAngleDeg(); - } - this.useGyroLock = useGyroLock; - } - - public void driveWithJoystick() { - if(m_drive == null) return; - // switch(m_controllerMode) { - // case CONTROLLER_JOYSTICK_ARCADE: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick1().getX(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_JOYSTICK_TANK: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick2().getY(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_drive.tankDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_JOYSTICK_CHEESY: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick2().getX(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_XBOX_CHEESY: - // boolean turbo = OI.getInstance().getDriveTrainController() - // .getLeftJoystickButton(); - // boolean slow = OI.getInstance().getDriveTrainController() - // .getRightJoystickButton(); - // double speedToUseMove, speedToUseSteer; - // if (turbo && !slow) { - // speedToUseMove = m_moveScaleTurbo; - // speedToUseSteer = m_steerScaleTurbo; - // } else if (!turbo && slow) { - // speedToUseMove = m_moveScaleSlow; - // speedToUseSteer = m_steerScaleSlow; - // } else { - // speedToUseMove = m_moveScale; - // speedToUseSteer = m_steerScale; - // } - - // m_moveInput = - // OI.getInstance().getDriveTrainController().getLeftYAxis(); - // m_steerInput = - // OI.getInstance().getDriveTrainController().getRightXAxis(); - m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis(); - m_steerInput = OI.getInstance().getDriverController().getRightXAxis(); - - if (controlMode == DriveControlMode.JOYSTICK) { - m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - } - else if (controlMode == DriveControlMode.CLIMB) { - m_moveOutput = climbSpeed; - } - - if (useGyroLock) { // If currently in gyro lock mode, - if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning, - setGyroLock(false, false); // turn off gyro lock mode - } - } else { // If not yet in gyro lock mode, - if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning, - setGyroLock(true, false); // gyro lock to current heading - } - } - - if (useGyroLock) { - double yawError = gyroLockAngleDeg - getGyroAngleDeg(); - m_steerOutput = kPGyro * yawError; - } else { - m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - } - - m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75); - // break; - // case CONTROLLER_XBOX_ARCADE_RIGHT: - // m_moveInput = - // OI.getInstance().getDrivetrainController().getRightYAxis(); - // m_steerInput = - // OI.getInstance().getDrivetrainController().getRightXAxis(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_XBOX_ARCADE_LEFT: - // m_moveInput = - // OI.getInstance().getDrivetrainController().getLeftYAxis(); - // m_steerInput = - // OI.getInstance().getDrivetrainController().getLeftXAxis(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // } - } - - public void rawMoveSteer(double move, double steer) { - m_drive.arcadeDrive(move, steer, false); - } - - public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { - - if (elevatorRight.getSelectedSensorPosition(0) >= 3550) { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5); - } - else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); - } - } - - private boolean inDeadZone(double input) { - boolean inDeadZone; - if (Math.abs(input) < STICK_DEADBAND) { - inDeadZone = true; - } else { - inDeadZone = false; - } - return inDeadZone; - } - - public double adjustForSensitivity(double scale, double trim, - double steer, int nonLinearFactor, double wheelNonLinearity) { - if (inDeadZone(steer)) - return 0; - - steer += trim; - steer *= scale; - steer = limitValue(steer); - - int iterations = Math.abs(nonLinearFactor); - for (int i = 0; i < iterations; i++) { - if (nonLinearFactor > 0) { - steer = nonlinearStickCalcPositive(steer, wheelNonLinearity); - } else { - steer = nonlinearStickCalcNegative(steer, wheelNonLinearity); - } - } - return steer; - } - - private double limitValue(double value) { - if (value > 1.0) { - value = 1.0; - } else if (value < -1.0) { - value = -1.0; - } - return value; - } - - private double nonlinearStickCalcPositive(double steer, - double steerNonLinearity) { - return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer) - / Math.sin(Math.PI / 2.0 * steerNonLinearity); - } - - private double nonlinearStickCalcNegative(double steer, - double steerNonLinearity) { - return Math.asin(steerNonLinearity * steer) - / Math.asin(steerNonLinearity); - } - - //public void setShiftState(SpeedShiftState state) { - // if(state == SpeedShiftState.HI) { - // speedShift.set(true); - // } - // else if(state == SpeedShiftState.LO) { - // speedShift.set(false); - // } - //} - - public synchronized boolean isFinished() { - return isFinished; - } - - public synchronized void setFinished(boolean isFinished) { - this.isFinished = isFinished; - } - - @Override - public void setPeriodMs(long periodMs) { - //mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers); - mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers); - mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); - pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); - mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers); - mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers); - adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers); - - this.periodMs = periodMs; - } - - public double getPeriodMs() { - return periodMs; - } - - public boolean isRed() { - return isRed; - } - - public void setIsRed(boolean status) { - isRed = status; - } - - public static double rotationsToInches(double rotations) { - return rotations * (Constants.kDriveWheelDiameterInches * Math.PI); - } - - public static double rpmToInchesPerSecond(double rpm) { - return rotationsToInches(rpm) / 60; - } - - public static double inchesToRotations(double inches) { - return inches / (Constants.kDriveWheelDiameterInches * Math.PI); - } - - public static double inchesPerSecondToRpm(double inches_per_second) { - return inchesToRotations(inches_per_second) * 60; - } - - public double getLeftPositionWorld() { - return leftDrive1.getPositionWorld(); - } - - public double getRightPositionWorld() { - return rightDrive1.getPositionWorld(); - } - - public void updateStatus(Robot.OperationMode operationMode) { - if (operationMode == Robot.OperationMode.TEST) { - try { - SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); - SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld()); - SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld()); - SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12); - SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12); - //SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID)); - //SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID)); -// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID)); - //SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID)); - //SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID)); -// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID)); - //SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn()); - //SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn()); - SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); - //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); - SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg()); - MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); - double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; - SmartDashboard.putNumber("Gyro Delta", delta); - //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); - SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); - SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); - SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); - SmartDashboard.putNumber("Steer Output", m_steerOutput); - SmartDashboard.putNumber("Move Output", m_moveOutput); - SmartDashboard.putNumber("Steer Input", m_steerInput); - SmartDashboard.putNumber("Move Input", m_moveInput); - } - catch (Exception e) { - System.err.println("Drivetrain update status error"); - } - } - else { - - } - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java deleted file mode 100644 index 129b561..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc4388.robot.subsystems; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Subsystem; - -public class Pnumatics extends Subsystem { - - - private DoubleSolenoid speedShift; - private DoubleSolenoid Intake; - - - public Pnumatics() { - try { - speedShift = new DoubleSolenoid(1,0,1); - Intake = new DoubleSolenoid(1,4,5 ); - } - catch (Exception e) { - System.err.println("An error occurred in the Pnumatics constructor"); - } - } - - public void setShiftState(boolean state) { - if (state==true) { - speedShift.set(DoubleSolenoid.Value.kForward); - } - if (state==false) { - speedShift.set(DoubleSolenoid.Value.kReverse); - } - } - public void setIntake(boolean state) { - if (state==true) { - Intake.set(DoubleSolenoid.Value.kForward); - } - if (state==false) { - Intake.set(DoubleSolenoid.Value.kReverse); - } - } - - public void initDefaultCommand() { - } -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java deleted file mode 100644 index c217c81..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java +++ /dev/null @@ -1,228 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; -import java.util.Optional; -import java.util.Set; - -import org.usfirst.frc4388.robot.Constants; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.Timer; - -/** - * Implements an adaptive pure pursuit controller. See: - * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 - * .pdf - * - * Basically, we find a spot on the path we'd like to follow and calculate the - * wheel speeds necessary to make us land on that spot. The target spot is a - * specified distance ahead of us, and we look further ahead the greater our - * tracking error. - */ -public class AdaptivePurePursuitController { - private static final double kEpsilon = 1E-9; - - double mFixedLookahead; - Path mPath; - RigidTransform2d.Delta mLastCommand; - double mLastTime; - double mMaxAccel; - double mDt; - boolean mReversed; - double mPathCompletionTolerance; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - //motorController.configNominalOutputVoltage(+0.0f, -0.0f); - //motorController.configPeakOutputVoltage(+12.0f, -12.0f); - //motorController.setProfile(0); - motorController.config_kP(0, pidParams.kP, 0); - motorController.config_kI(0, pidParams.kI, 0); - motorController.config_kD(0, pidParams.kD, 0); - motorController.config_kF(0, pidParams.kF, 0); - motorController.configNominalOutputForward(+0.0f, 0); - motorController.configNominalOutputReverse(-0.0f, 0); - motorController.configPeakOutputForward(+1.0f, 0); - motorController.configPeakOutputReverse(-1.0f, 0); - motorController.selectProfileSlot(0, 0); - } - } - - public void setPath(double fixed_lookahead, double max_accel, Path path, - boolean reversed, double path_completion_tolerance) { - mFixedLookahead = fixed_lookahead; - mMaxAccel = max_accel; - mPath = path; - mDt = periodMs; - mLastCommand = null; - mReversed = reversed; - mPathCompletionTolerance = path_completion_tolerance; - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Speed); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Velocity, 0); - } - } - - public boolean isDone() { - double remainingLength = mPath.getRemainingLength(); - return remainingLength <= mPathCompletionTolerance; - } - - public boolean controlLoopUpdate(RigidTransform2d robot_pose) { - RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp()); - Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command); - - // Scale the command to respect the max velocity limits - double max_vel = 0.0; - max_vel = Math.max(max_vel, Math.abs(setpoint.left)); - max_vel = Math.max(max_vel, Math.abs(setpoint.right)); - if (max_vel > Constants.kPathFollowingMaxVel) { - double scaling = Constants.kPathFollowingMaxVel / max_vel; - setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling); - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - motorController.setVelocityWorld(-setpoint.right); - } - else { - motorController.setVelocityWorld(-setpoint.left); - } - } - - return isDone(); - } - - public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) { - RigidTransform2d pose = robot_pose; - if (mReversed) { - pose = new RigidTransform2d(robot_pose.getTranslation(), - robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI))); - } - - double distance_from_path = mPath.update(robot_pose.getTranslation()); - if (this.isDone()) { - return new RigidTransform2d.Delta(0, 0, 0); - } - - PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(), - distance_from_path + mFixedLookahead); - Optional circle = joinPath(pose, lookahead_point.translation); - - double speed = lookahead_point.speed; - if (mReversed) { - speed *= -1; - } - // Ensure we don't accelerate too fast from the previous command - double dt = now - mLastTime; - if (mLastCommand == null) { - mLastCommand = new RigidTransform2d.Delta(0, 0, 0); - dt = mDt; - } - double accel = (speed - mLastCommand.dx) / dt; - if (accel < -mMaxAccel) { - speed = mLastCommand.dx - mMaxAccel * dt; - } else if (accel > mMaxAccel) { - speed = mLastCommand.dx + mMaxAccel * dt; - } - - // Ensure we slow down in time to stop - // vf^2 = v^2 + 2*a*d - // 0 = v^2 + 2*a*d - double remaining_distance = mPath.getRemainingLength(); - double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance); - if (Math.abs(speed) > max_allowed_speed) { - speed = max_allowed_speed * Math.signum(speed); - } - final double kMinSpeed = 4.0; - if (Math.abs(speed) < kMinSpeed) { - // Hack for dealing with problems tracking very low speeds with - // Talons - speed = kMinSpeed * Math.signum(speed); - } - - RigidTransform2d.Delta rv; - if (circle.isPresent()) { - rv = new RigidTransform2d.Delta(speed, 0, - (circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius); - } else { - rv = new RigidTransform2d.Delta(speed, 0, 0); - } - mLastTime = now; - mLastCommand = rv; - return rv; - } - - public Set getMarkersCrossed() { - return mPath.getMarkersCrossed(); - } - - public static class Circle { - public final Translation2d center; - public final double radius; - public final boolean turn_right; - - public Circle(Translation2d center, double radius, boolean turn_right) { - this.center = center; - this.radius = radius; - this.turn_right = turn_right; - } - } - - public static Optional joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) { - double x1 = robot_pose.getTranslation().getX(); - double y1 = robot_pose.getTranslation().getY(); - double x2 = lookahead_point.getX(); - double y2 = lookahead_point.getY(); - - Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point); - double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin() - - pose_to_lookahead.getY() * robot_pose.getRotation().cos(); - if (Math.abs(cross_product) < kEpsilon) { - return Optional.empty(); - } - - double dx = x1 - x2; - double dy = y1 - y2; - double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos(); - double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin(); - - double cross_term = mx * dx + my * dy; - - if (Math.abs(cross_term) < kEpsilon) { - // Points are colinear - return Optional.empty(); - } - - return Optional.of(new Circle( - new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term), - (-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)), - .5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0)); - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java deleted file mode 100644 index 8d3fc74..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class BHRMathUtils { - - public static double normalizeAngle0To360(double currentAccumAngle) { - // reduce the angle - double normalizedAngle = currentAccumAngle % 360; - - // force it to be the positive remainder, so that 0 <= angle < 360 - normalizedAngle = (normalizedAngle + 360) % 360; - - return normalizedAngle; - } - - public static double adjustAccumAngleToDesired(double currentAccumAngle, double desiredAngle0To360) { - double normalizedAngle = normalizeAngle0To360(currentAccumAngle); - - if ( Math.abs(normalizedAngle - desiredAngle0To360) <= 180) { - double deltaAngle = normalizedAngle - desiredAngle0To360; - return currentAccumAngle - deltaAngle; - } - else { - double deltaAngle = desiredAngle0To360 - normalizedAngle; - if (deltaAngle < 0) { - deltaAngle += 360; - } - else { - deltaAngle -= 360; - } - return currentAccumAngle + deltaAngle; - } - } - - public static double adjustAccumAngleToClosest180(double currentAccumAngle) { - double normalizedAngle = Math.abs(normalizeAngle0To360(currentAccumAngle)); - - if (normalizedAngle < 90 || normalizedAngle > 270) { - return adjustAccumAngleToDesired(currentAccumAngle, 0); - } - else { - return adjustAccumAngleToDesired(currentAccumAngle, 180); - } - } - - public static void main(String[] args) { - System.out.println("Accum angle to desired 721 to 45 = " + adjustAccumAngleToDesired(721, 45)); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java deleted file mode 100644 index 52088f7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java +++ /dev/null @@ -1,65 +0,0 @@ -package org.usfirst.frc4388.utility; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -public class CANTalonEncoder extends WPI_TalonSRX -{ - private double encoderTicksToWorld; - private boolean isRight = true; - - - - public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { - this(deviceNumber, encoderTicksToWorld, false, feedbackDevice); - } - - public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) { - super(deviceNumber); - //this.setFeedbackDevice(feedbackDevice); - this.configSelectedFeedbackSensor(feedbackDevice, 0, 0); - this.encoderTicksToWorld = encoderTicksToWorld; - this.isRight = isRight; - } - - public boolean isRight() { - return isRight; - } - - public void setRight(boolean isRight) { - this.isRight = isRight; - } - - public double convertEncoderTicksToWorld(double encoderTicks) { - return encoderTicks / encoderTicksToWorld; - } - - public double convertEncoderWorldToTicks(double worldValue) { - return worldValue * encoderTicksToWorld; - } - - public void setWorld(double worldValue) { - //this.set(convertEncoderWorldToTicks(worldValue)); - this.set(getControlMode(), convertEncoderWorldToTicks(worldValue)); //TODO: change to explicit mode set? - } - - public void setPositionWorld(double worldValue) { - //this.setPosition(convertEncoderWorldToTicks(worldValue)); - this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0); //TODO: verify - } - - public double getPositionWorld() { - //return convertEncoderTicksToWorld(this.getPosition()); - return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop" - } - - public void setVelocityWorld(double worldValue) { - //this.set(convertEncoderWorldToTicks(worldValue) * 0.1); - this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1); //TODO: change to explicit mode set? - } - - public double getVelocityWorld() { - //return convertEncoderTicksToWorld(this.getSpeed() / 0.1); - return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop" - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java deleted file mode 100644 index 135bb97..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java +++ /dev/null @@ -1,199 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.io.File; -import java.io.FileReader; -import java.io.FileWriter; -import java.io.IOException; -import java.lang.reflect.Field; -import java.math.BigDecimal; -import java.util.ArrayList; -import java.util.Collection; -import java.util.HashMap; -import java.util.List; -import java.util.Set; - -import org.json.simple.JSONObject; -import org.json.simple.parser.JSONParser; -import org.json.simple.parser.ParseException; - -/** - * ConstantsBase - * - * Base class for storing robot constants. Anything stored as a public static - * field will be reflected and be able to set externally - */ -public abstract class ConstantsBase { - HashMap modifiedKeys = new HashMap(); - - public abstract String getFileLocation(); - - public static class Constant { - public String name; - public Class type; - public Object value; - - public Constant(String name, Class type, Object value) { - this.name = name; - this.type = type; - this.value = value; - } - - @Override - public boolean equals(Object o) { - String itsName = ((Constant) o).name; - Class itsType = ((Constant) o).type; - Object itsValue = ((Constant) o).value; - return o instanceof Constant && this.name.equals(itsName) && this.type.equals(itsType) - && this.value.equals(itsValue); - } - } - - public File getFile() { - String filePath = getFileLocation(); - filePath = filePath.replaceFirst("^~", System.getProperty("user.home")); - return new File(filePath); - } - - public boolean setConstant(String name, Double value) { - return setConstantRaw(name, value); - } - - public boolean setConstant(String name, Integer value) { - return setConstantRaw(name, value); - } - - public boolean setConstant(String name, String value) { - return setConstantRaw(name, value); - } - - private boolean setConstantRaw(String name, Object value) { - boolean success = false; - Field[] declaredFields = this.getClass().getDeclaredFields(); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { - try { - Object current = field.get(this); - field.set(this, value); - success = true; - if (!value.equals(current)) { - modifiedKeys.put(name, true); - } - } catch (IllegalArgumentException | IllegalAccessException e) { - System.out.println("Could not set field: " + name); - } - } - } - return success; - } - - public Object getValueForConstant(String name) throws Exception { - Field[] declaredFields = this.getClass().getDeclaredFields(); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { - try { - return field.get(this); - } catch (IllegalArgumentException | IllegalAccessException e) { - throw new Exception("Constant not found"); - } - } - } - throw new Exception("Constant not found"); - } - - public Constant getConstant(String name) { - Field[] declaredFields = this.getClass().getDeclaredFields(); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { - try { - return new Constant(field.getName(), field.getType(), field.get(this)); - } catch (IllegalArgumentException | IllegalAccessException e) { - e.printStackTrace(); - } - } - } - return new Constant("", Object.class, 0); - } - - public Collection getConstants() { - List constants = (List) getAllConstants(); - int stop = constants.size() - 1; - for (int i = 0; i < constants.size(); ++i) { - Constant c = constants.get(i); - if ("kEndEditableArea".equals(c.name)) { - stop = i; - } - } - return constants.subList(0, stop); - } - - private Collection getAllConstants() { - Field[] declaredFields = this.getClass().getDeclaredFields(); - List constants = new ArrayList(declaredFields.length); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers())) { - Constant c; - try { - c = new Constant(field.getName(), field.getType(), field.get(this)); - constants.add(c); - } catch (IllegalArgumentException | IllegalAccessException e) { - e.printStackTrace(); - } - } - } - return constants; - } - - public JSONObject getJSONObjectFromFile() throws IOException, ParseException { - File file = getFile(); - if (file == null || !file.exists()) { - return new JSONObject(); - } - FileReader reader; - reader = new FileReader(file); - JSONParser jsonParser = new JSONParser(); - return (JSONObject) jsonParser.parse(reader); - } - - public void loadFromFile() { - try { - JSONObject jsonObject = getJSONObjectFromFile(); - Set keys = jsonObject.keySet(); - for (Object o : keys) { - String key = (String) o; - Object value = jsonObject.get(o); - if (value instanceof Long && getConstant(key).type.equals(int.class)) { - value = new BigDecimal((Long) value).intValueExact(); - } - setConstantRaw(key, value); - } - } catch (IOException e) { - e.printStackTrace(); - } catch (ParseException e) { - e.printStackTrace(); - } - } - - public void saveToFile() { - File file = getFile(); - if (file == null) { - return; - } - try { - JSONObject json = getJSONObjectFromFile(); - FileWriter writer = new FileWriter(file); - for (String key : modifiedKeys.keySet()) { - try { - Object value = getValueForConstant(key); - json.put(key, value); - } catch (Exception e) { - e.printStackTrace(); - } - } - writer.write(json.toJSONString()); - writer.close(); - } catch (IOException | ParseException e) { - e.printStackTrace(); - } - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java deleted file mode 100644 index c53b589..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.usfirst.frc4388.utility; - -public interface ControlLoopable -{ - public void controlLoopUpdate(); - public void setPeriodMs(long periodMs); -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java deleted file mode 100644 index ac7be21..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java +++ /dev/null @@ -1,79 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.Timer; -import java.util.TimerTask; -import java.util.Vector; - -/** - * ControlLooper.java - *

- * Runs several ControlLoopables simultaneously with one timing loop. - * Useful for running a bunch of control loops - * with only one Thread worth of overhead. - * - * Shamelessly stolen from team 254 2015 code and then slightly modified - * - * @author Tom Bottiglieri - */ - -public class ControlLooper -{ - private Timer controlLoopTimer; - private Vector loopables = new Vector(); - private long periodMs; - private String name; - - public ControlLooper(String name, long periodMs) { - this.name = name; - this.periodMs = periodMs; - } - - private class ControlLoopTask extends TimerTask { - private ControlLooper controlLooper; - - public ControlLoopTask(ControlLooper controlLooper) { - if (controlLooper == null) { - throw new NullPointerException("Given control looper was null"); - } - this.controlLooper = controlLooper; - } - - @Override - public void run() { - controlLooper.controlLoopUpdate(); - } - - } - - public String getName() { - return name; - } - - public void start() { - if (controlLoopTimer == null) { - controlLoopTimer = new Timer(); - controlLoopTimer.schedule(new ControlLoopTask(this), 0L, periodMs); - } - } - - public void stop() { - if (controlLoopTimer != null) { - controlLoopTimer.cancel(); - controlLoopTimer = null; - } - } - - private void controlLoopUpdate() { - for (int i = 0; i < loopables.size(); ++i) { - ControlLoopable c = loopables.elementAt(i); - if (c != null) { - c.controlLoopUpdate(); - } - } - } - - public void addLoopable(ControlLoopable c) { - loopables.addElement(c); - c.setPeriodMs(periodMs); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java deleted file mode 100644 index ea3af27..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java +++ /dev/null @@ -1,894 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.awt.AWTException; -import java.awt.BasicStroke; -import java.awt.Color; -import java.awt.Dimension; -import java.awt.FontMetrics; -import java.awt.Graphics; -import java.awt.Graphics2D; -import java.awt.Image; -import java.awt.Rectangle; -import java.awt.RenderingHints; -import java.awt.Robot; -import java.awt.Stroke; -import java.awt.Toolkit; -import java.awt.datatransfer.Clipboard; -import java.awt.datatransfer.ClipboardOwner; -import java.awt.datatransfer.DataFlavor; -import java.awt.datatransfer.Transferable; -import java.awt.datatransfer.UnsupportedFlavorException; -import java.awt.event.ActionEvent; -import java.awt.event.ActionListener; -import java.awt.event.MouseAdapter; -import java.awt.event.MouseEvent; -import java.awt.geom.AffineTransform; -import java.awt.geom.Ellipse2D; -import java.awt.geom.Line2D; -import java.awt.image.BufferedImage; -import java.io.IOException; -import java.text.DecimalFormat; -import java.util.LinkedList; - -import javax.swing.JFrame; -import javax.swing.JMenuItem; -import javax.swing.JPanel; -import javax.swing.JPopupMenu; - -/** - * This class is a basic plotting class using the Java AWT interface. It has basic features which allow the user - * to plot multiple graphs on one figure, control axis dimensions, and specify colors. - * - * This is by all means not an extensive plotter, but it will help visualize data very quickly and accurately. If - * a more robust plotting function is required, the user is encouraged to use Excel or Matlab. The purpose of this - * class is to be easy to use with enought automation to have nice graphs with minimal effort, but give the user - * control over as much as possible, so they can generate the perfect chart. - * - * The plotter also features the ability to capture screen shots directly from the right-click menu, this allows - * the user to copy and paste plots into reports or other documents rather quickly. - * - * This class holds an interface similar to that of Matlab. - * - * This class currently only supports scatterd line charts. - * - * @author Kevin Harrilal - * @email kevin@team2168.org - * @version 1 - * @date 9 Sept 2014 - * - */ - -class FalconLinePlot extends JPanel implements ClipboardOwner{ - - - private static final long serialVersionUID = 3205256608145459434L; - private final int yPAD = 60; //controls how far the X- and Y- axis lines are away from the window edge - private final int xPAD = 70; //controls how far the X- and Y- axis lines are away from the window edge - - private double upperXtic; - private double lowerXtic; - private double upperYtic; - private double lowerYtic; - private boolean yGrid; - private boolean xGrid; - - private double yMax; - private double yMin; - private double xMax; - private double xMin; - - private int yticCount; - private int xticCount; - private double xTicStepSize; - private double yTicStepSize; - - boolean userSetYTic; - boolean userSetXTic; - - private String xLabel; - private String yLabel; - private String titleLabel; - protected static int count = 0; - - JPopupMenu menu = new JPopupMenu("Popup"); - - //Link List to hold all different plots on one graph. - private LinkedList link; - - - /** - * Constructor which Plots only Y-axis data. - * @param yData is a array of doubles representing the Y-axis values of the data to be plotted. - */ - public FalconLinePlot(double[] yData) - { - this(null,yData,Color.red); - } - - public FalconLinePlot(double[] yData,Color lineColor, Color marker) - { - this(null,yData,lineColor,marker); - } - - /** - * Constructor which Plots chart based on provided x and y data. X and Y arrays must be of the same length. - * @param xData is an array of doubles representing the X-axis values of the data to be plotted. - * @param yData is an array of double representing the Y-axis values of the data to be plotted. - */ - public FalconLinePlot(double[] xData, double[] yData) - { - this(xData,yData,Color.red,null); - } - - /** - * Constructor which Plots chart based on provided x and y axis data. - * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data - * is the second dimension. - */ - public FalconLinePlot(double[][] data) - { - this(getXVector(data),getYVector(data),Color.red,null); - } - -/** - * Constructor which plots charts based on provided x and y axis data in a single two dimensional array. - * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data - * is the second dimension. - * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint - * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to - * not have datapoint markers. - */ - public FalconLinePlot(double[][] data, Color lineColor, Color markerColor) - { - this(getXVector(data),getYVector(data),lineColor,markerColor); - } - - /** - * Constructor which plots charts based on provided x and y axis data provided as separate arrays. The user can also specify the color of the adjoining line. - * Data markers are not displayed. - * @param xData is an array of doubles representing the X-axis values of the data to be plotted. - * @param yData is an array of double representing the Y-axis values of the data to be plotted. - * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint - */ - public FalconLinePlot(double[] xData, double[] yData,Color lineColor) - { - this(xData,yData,lineColor,null); - } - - - - /** - * Constructor which plots charts based on provided x and y axis data, provided as separate arrays. The user - * can also specify the color of the adjoining line and the color of the datapoint maker. - * @param xData is an array of doubles representing the X-axis values of the data to be plotted. - * @param yData is an array of double representing the Y-axis values of the data to be plotted. - * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint - * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to - * not have datapoint markers. - */ - public FalconLinePlot(double[] xData, double[] yData,Color lineColor, Color markerColor) - { - xLabel = "X axis"; - yLabel = "Y axis"; - titleLabel = "Title"; - - upperXtic = -Double.MAX_VALUE; - lowerXtic = Double.MAX_VALUE; - upperYtic = -Double.MAX_VALUE; - lowerYtic = Double.MAX_VALUE; - xticCount = -Integer.MAX_VALUE; - - this.userSetXTic = false; - this.userSetYTic = false; - - link = new LinkedList(); - - addData(xData, yData, lineColor,markerColor); - - count ++; - JFrame g = new JFrame("Figure " + count); - g.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - g.add(this); - g.setSize(600,400); - g.setLocationByPlatform(true); - g.setVisible(true); - - menu(g,this); - } - - /** - * Adds a plot to an existing figure. - * @param yData is a array of doubles representing the Y-axis values of the data to be plotted. - * @param color is the color the user wishes to be displayed for the line connecting each datapoint - */ - - public void addData(double[] y, Color lineColor) - { - addData(y, lineColor, null); - } - - public void addData(double[] y, Color lineColor, Color marker) - { - //cant add y only data unless all other data is y only data - for(xyNode data: link) - if(data.x != null) - throw new Error ("All previous chart series need to have only Y data arrays"); - - addData(null,y,lineColor, marker); - } - - public void addData(double[] x, double[] y, Color lineColor) - { - addData(x,y,lineColor,null); - } - - - public void addData(double[][] data, Color lineColor) - { - addData(getXVector(data),getYVector(data),lineColor,null); - } - - public void addData(double[][] data, Color lineColor, Color marker) - { - addData(getXVector(data),getYVector(data),lineColor,marker); - } - - public void addData(double[] x, double[] y, Color lineColor, Color marker) - { - xyNode Data = new xyNode(); - - //copy y array into node - Data.y = new double[y.length]; - Data.lineColor = lineColor; - - if(marker == null) - Data.lineMarker = false; - else - { - Data.lineMarker = true; - Data.markerColor = marker; - } - for(int i=0; i list) - { - for(xyNode node: list) - { - double tempYMax = getMax(node.y); - double tempYMin = getMin(node.y); - - if(tempYMinyMax) - yMax=tempYMax; - - if(xticCount < node.y.length) - xticCount = node.y.length; - - - if(node.x != null) - { - double tempXMax = getMax(node.x); - double tempXMin = getMin(node.x); - - if(tempXMinxMax) - xMax=tempXMax; - - } - else - { - xMax=node.y.length-1; - xMin=0; - - } - - } - - } - - private double getMax(double[] data) { - double max = -Double.MAX_VALUE; - for(int i = 0; i < data.length; i++) { - if(data[i] > max) - max = data[i]; - } - return max; - } - - private double getMin(double[] data) { - double min = Double.MAX_VALUE; - for(int i = 0; i < data.length; i++) { - if(data[i] < min) - min = data[i]; - } - return min; - } - - public void setYLabel(String s) - { - yLabel = s; - } - - public void setXLabel(String s) - { - xLabel = s; - } - - public void setTitle(String s) - { - titleLabel = s; - } - - private void setYLabel(Graphics2D g2, String s) - { - FontMetrics fm = getFontMetrics(getFont()); - int width = fm.stringWidth(s); - - AffineTransform temp = g2.getTransform(); - - AffineTransform at = new AffineTransform(); - at.setToRotation(-Math.PI /2, 10, getHeight()/2+width/2); - g2.setTransform(at); - - //draw string in center of y axis - g2.drawString(s, 10, 7+getHeight()/2+width/2); - - g2.setTransform(temp); - - } - - private void setXLabel(Graphics2D g2, String s) - { - FontMetrics fm = getFontMetrics(getFont()); - int width = fm.stringWidth(s); - - g2.drawString(s, getWidth()/2-(width/2), getHeight()-10); - } - - private void setTitle(Graphics2D g2, String s) - { - FontMetrics fm = getFontMetrics(getFont()); - - String[] line = s.split("\n"); - - int height = xPAD/2 - ((line.length-1) * fm.getHeight()/2); - - for (int i=0; i - * The Type of Interpolable - * @see InterpolatingTreeMap - */ -public interface Interpolable { - /** - * Interpolates between this value and an other value according to a given - * parameter. If x is 0, the method should return this value. If x is 1, the - * method should return the other value. If 0 < x < 1, the return value - * should be interpolated proportionally between the two. - * - * @param other - * The value of the upper bound - * @param x - * The requested value. Should be between 0 and 1. - * @return Interpolable The estimated average between the surrounding - * data - */ - public T interpolate(T other, double x); -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java deleted file mode 100644 index 7c6de07..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.Map; -import java.util.TreeMap; - -/** - * Interpolating Tree Maps are used to get values at points that are not defined - * by making a guess from points that are defined. This uses linear - * interpolation. - * - * @param - * The type of the key (must implement InverseInterpolable) - * @param - * The type of the value (must implement Interpolable) - */ -public class InterpolatingTreeMap & Comparable, V extends Interpolable> - extends TreeMap { - private static final long serialVersionUID = 8347275262778054124L; - - int max_; - - public InterpolatingTreeMap(int maximumSize) { - max_ = maximumSize; - } - - public InterpolatingTreeMap() { - this(0); - } - - /** - * Inserts a key value pair, and trims the tree if a max size is specified - * - * @param key - * Key for inserted data - * @param value - * Value for inserted data - * @return the value - */ - @Override - public V put(K key, V value) { - if (max_ > 0 && max_ <= size()) { - // "Prune" the tree if it is oversize - K first = firstKey(); - remove(first); - } - - super.put(key, value); - - return value; - } - - @Override - public void putAll(Map map) { - System.out.println("Unimplemented Method"); - } - - /** - * - * @param key - * Lookup for a value (does not have to exist) - * @return V or null; V if it is Interpolable or exists, null if it is at a - * bound and cannot average - */ - public V getInterpolated(K key) { - V gotval = get(key); - if (gotval == null) { - /** Get surrounding keys for interpolation */ - K topBound = ceilingKey(key); - K bottomBound = floorKey(key); - - /** - * If attempting interpolation at ends of tree, return the nearest - * data point - */ - if (topBound == null && bottomBound == null) { - return null; - } else if (topBound == null) { - return get(bottomBound); - } else if (bottomBound == null) { - return get(topBound); - } - - /** Get surrounding values for interpolation */ - V topElem = get(topBound); - V bottomElem = get(bottomBound); - return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); - } else { - return gotval; - } - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java deleted file mode 100644 index dc9f6a4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.usfirst.frc4388.utility; - -/** - * InverseInterpolable is an interface used by an Interpolating Tree as the Key - * type. Given two endpoint keys and a third query key, an InverseInterpolable - * object can calculate the interpolation parameter of the query key on the - * interval [0, 1]. - * - * @param - * The Type of InverseInterpolable - * @see InterpolatingTreeMap - */ -public interface InverseInterpolable { - /** - * Given this point (lower), a query point (query), and an upper point - * (upper), estimate how far (on [0, 1]) between 'lower' and 'upper' the - * query point lies. - * - * @param upper - * @param query - * @return The interpolation parameter on [0, 1] representing how far - * between this point and the upper point the query point lies. - */ - public double inverseInterpolate(T upper, T query); -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java deleted file mode 100644 index e3c3910..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.usfirst.frc4388.utility; - -import org.usfirst.frc4388.robot.Constants; - -/** - * Provides forward and inverse kinematics equations for the robot modeling the - * wheelbase as a differential drive (with a corrective factor to account for - * the inherent skidding of the center 4 wheels quasi-kinematically). - */ - -public class Kinematics { - private static final double kEpsilon = 1E-9; - - /** - * Forward kinematics using only encoders, rotation is implicit (less - * accurate than below, but useful for predicting motion) - */ - public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta) { - double linear_velocity = (left_wheel_delta + right_wheel_delta) / 2; - double delta_v = (right_wheel_delta - left_wheel_delta) / 2; - double delta_rotation = delta_v * 2 * Constants.kTrackScrubFactor / Constants.kTrackEffectiveDiameter; - return new RigidTransform2d.Delta(linear_velocity, 0, delta_rotation); - } - - /** - * Forward kinematics using encoders and explicitly measured rotation (ex. - * from gyro) - */ - public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta, - double delta_rotation_rads) { - return new RigidTransform2d.Delta((left_wheel_delta + right_wheel_delta) / 2, 0, delta_rotation_rads); - } - - /** Append the result of forward kinematics to a previous pose. */ - public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta, - double right_wheel_delta, Rotation2d current_heading) { - RigidTransform2d.Delta with_gyro = forwardKinematics(left_wheel_delta, right_wheel_delta, - current_pose.getRotation().inverse().rotateBy(current_heading).getRadians()); - return current_pose.transformBy(RigidTransform2d.fromVelocity(with_gyro)); - } - - public static class DriveVelocity { - public final double left; - public final double right; - - public DriveVelocity(double left, double right) { - this.left = left; - this.right = right; - } - } - - public static DriveVelocity inverseKinematics(RigidTransform2d.Delta velocity) { - if (Math.abs(velocity.dtheta) < kEpsilon) { - return new DriveVelocity(velocity.dx, velocity.dx); - } - double delta_v = Constants.kTrackEffectiveDiameter * velocity.dtheta / (2 * Constants.kTrackScrubFactor); - return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java deleted file mode 100644 index 28dced9..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java +++ /dev/null @@ -1,137 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.subsystems.Drive; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -public class MMTalonPIDController -{ - protected static enum MMControlMode { STRAIGHT, TURN }; - public static enum MMTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY }; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected boolean useGyroLock; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - protected MMControlMode controlMode; - protected MMTalonTurnType turnType; - protected double targetValue; - - public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - private int convertInchesPerSecToTicksPer100ms(double inchesPerSec) { - return (int)Math.round(Constants.kDriveEncoderTicksPerInch * inchesPerSec / 10); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kF(0, pidParams.kF, 0); //TODO: verify want parameter slot 0, with no timeout - } - } - - public void setMMStraightTarget(double startValue, double targetValue, double maxVelocityInchesPerSec, double maxAccelerationInchesPerSecPerSec, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - controlMode = MMControlMode.STRAIGHT; - this.startGyroAngle = desiredAngle; - this.useGyroLock = useGyroLock; - this.targetValue = targetValue; - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - if (resetEncoder) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - //motorController.setMotionMagicCruiseVelocity(maxVelocity); - //motorController.setMotionMagicAcceleration(maxAcceleration); - //motorController.set(targetValue); - //motorController.changeControlMode(TalonControlMode.MotionMagic); - motorController.configMotionCruiseVelocity(convertInchesPerSecToTicksPer100ms(maxVelocityInchesPerSec), 0); - motorController.configMotionAcceleration(convertInchesPerSecToTicksPer100ms(maxAccelerationInchesPerSecPerSec), 0); - motorController.set(ControlMode.MotionMagic, targetValue); - } - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - private double calcTrackDistance(double deltaAngleDeg, MMTalonTurnType turnType, double trackWidth) { - double trackDistance = deltaAngleDeg / 360.0 * Math.PI * trackWidth; - if (turnType == MMTalonTurnType.TANK) { - return trackDistance; - } - else if (turnType == MMTalonTurnType.LEFT_SIDE_ONLY) { - return trackDistance * 2.0; - } - else if (turnType == MMTalonTurnType.RIGHT_SIDE_ONLY) { - return -trackDistance * 2.0; - } - return 0.0; - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - // Calculate the motion profile feed forward and gyro feedback terms - double rightTarget = 0.0; - double leftTarget = 0.0; - - // Update the set points - if (controlMode == MMControlMode.STRAIGHT) { - double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; - double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES); - rightTarget = targetValue + deltaDistance; - leftTarget = targetValue - deltaDistance; - - // Update the controllers with updated set points. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(rightTarget); - motorController.set(motorController.getControlMode(), rightTarget); //TODO: change to explicit mode set? - } - else { - //motorController.set(leftTarget); - motorController.set(motorController.getControlMode(), leftTarget); //TODO: change to explicit mode set? - } - } - } - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java deleted file mode 100644 index b91fc31..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java +++ /dev/null @@ -1,156 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -public class MPSoftwarePIDController -{ - public static enum MPSoftwareTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY, LEFT_ARC, RIGHT_ARC }; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected MotionProfileBoxCar mp; - protected MotionProfilePoint mpPoint; - protected boolean useGyroLock; - protected double startGyroAngle; - protected double targetGyroAngle; - protected MPSoftwareTurnType turnType; - - protected double prevError = 0.0; // the prior error (used to compute velocity) - protected double totalError = 0.0; // the sum of the errors for use in the integral calc - - public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - } - - public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPSoftwareTurnType turnType, double trackWidth) { - this.turnType = turnType; - this.startGyroAngle = startAngleDeg; - this.targetGyroAngle = targetAngleDeg; - this.useGyroLock = true; - - // Set up the motion profile - mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2); - // NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - - prevError = 0.0; - totalError = 0.0; - } - - //public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) { - // this.turnType = turnType; - // this.useGyroLock = true; - // - // // Set up the motion profile - // mp = MotionProfileCache.getInstance().getMP(key); - // this.startGyroAngle = mp.getStartDistance(); - // this.targetGyroAngle = mp.getTargetDistance(); - // - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - // - // prevError = 0.0; - // totalError = 0.0; - //} - - public boolean controlLoopUpdate() { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - mpPoint = mp.getNextPoint(mpPoint); - - // Check if we are finished - if (mpPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and error feedback terms - double error = mpPoint.position - currentGyroAngle; - - if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) { - totalError += error; - } - else { - totalError = 0; - } - - double output = pidParams.kP * error + pidParams.kI * totalError + pidParams.kD * (error - prevError) + pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity; - prevError = error; - - // Update the controllers set point. - if (turnType == MPSoftwareTurnType.TANK) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.set(output); - motorController.set(ControlMode.PercentOutput, output); - } - } - else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - } - } - else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - else { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - } - } - else if (turnType == MPSoftwareTurnType.LEFT_ARC) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(1.0 * output); - motorController.set(ControlMode.PercentOutput, 1.0 * output); - } - else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - } - } - else if (turnType == MPSoftwareTurnType.RIGHT_ARC) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - else { - //motorController.set(1.0 * output); - motorController.set(ControlMode.PercentOutput, 1.0 * output); - } - } - } - - return false; - } - - public MotionProfilePoint getCurrentPoint() { - return mpPoint; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java deleted file mode 100644 index 9a9d760..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java +++ /dev/null @@ -1,233 +0,0 @@ - -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -public class MPTalonPIDController -{ - protected static enum MPControlMode { STRAIGHT, TURN }; - public static enum MPTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY }; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected MotionProfileBoxCar mp; - protected MotionProfilePoint mpPoint; - protected boolean useGyroLock; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - protected MPControlMode controlMode; - protected MPTalonTurnType turnType; - - public MPTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - } - } - - public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2) { - setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, false); - } - - public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2, boolean resetEncoder) { - setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, resetEncoder); - } - - public void setMPStraightTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - controlMode = MPControlMode.STRAIGHT; - this.startGyroAngle = desiredAngle; - this.useGyroLock = useGyroLock; - - // Set up the motion profile - mp = new MotionProfileBoxCar(startValue, targetValue, maxVelocity, periodMs, t1, t2); - for (CANTalonEncoder motorController : motorControllers) { - if (resetEncoder) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - //motorController.set(mp.getStartDistance()); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.set(ControlMode.Position, mp.getStartDistance()); - } - } - - //public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - // controlMode = MPControlMode.STRAIGHT; - // this.startGyroAngle = desiredAngle; - // this.useGyroLock = useGyroLock; - // - // // Set up the motion profile - // mp = MotionProfileCache.getInstance().getMP(key); - // for (CANTalonEncoder motorController : motorControllers) { - // if (resetEncoder) { - // motorController.setPosition(0); - // } - // motorController.set(mp.getStartDistance()); - // motorController.changeControlMode(TalonControlMode.Position); - // } - //} - - public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPTalonTurnType turnType, double trackWidth) { - controlMode = MPControlMode.TURN; - this.turnType = turnType; - this.startGyroAngle = startAngleDeg; - this.targetGyroAngle = targetAngleDeg; - this.useGyroLock = true; - - trackDistance = calcTrackDistance(targetAngleDeg - startAngleDeg, turnType, trackWidth); - - // Set up the motion profile - mp = new MotionProfileBoxCar(0, trackDistance, maxTurnRateDegPerSec, periodMs, t1, t2); - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - - if (Math.abs(trackDistance) < 0.0001) { - trackDistance = 1; - } - } - - private double calcTrackDistance(double deltaAngleDeg, MPTalonTurnType turnType, double trackWidth) { - double trackDistance = deltaAngleDeg / 360.0 * Math.PI * trackWidth; - if (turnType == MPTalonTurnType.TANK) { - return trackDistance; - } - else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) { - return trackDistance * 2.0; - } - else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) { - return -trackDistance * 2.0; - } - return 0.0; - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - public boolean controlLoopUpdate() { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - mpPoint = mp.getNextPoint(mpPoint); - - // Check if we are finished - if (mpPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double KfLeft = 0.0; - double KfRight = 0.0; - - // Update the set points and Kf gains - if (controlMode == MPControlMode.STRAIGHT) { - double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; - if (Math.abs(mpPoint.position) > 0.001) { - //KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - //KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position; - - KfLeft = -(pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position; - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); - } - else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); - } - } - } - - else { - double mpAngle = startGyroAngle + ((targetGyroAngle - startGyroAngle) * mpPoint.position / trackDistance); - double gyroDelta = mpAngle - currentGyroAngle; - if (Math.abs(mpPoint.position) > 0.001) { - KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - } - - for (CANTalonEncoder motorController : motorControllers) { - if (turnType == MPTalonTurnType.TANK) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); - } - else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); - } - } - else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) { - if (!motorController.isRight()) { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); - } - } - else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); - } - } - } - } - - return false; - } - - public MotionProfilePoint getCurrentPoint() { - return mpPoint; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java deleted file mode 100644 index b9e8fca..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java +++ /dev/null @@ -1,113 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import jaci.pathfinder.Trajectory.Segment; - -public class MPTalonPIDPathController -{ - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected PathGenerator path; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - } - } - - public void setMPPathTarget(PathGenerator path) { - this.path = path; - path.resetCounter(); - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - Segment leftPoint = path.getLeftPoint(); - Segment rightPoint = path.getRightPoint(); - - // Check if we are finished - if (leftPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double KfLeft = 0.0; - double KfRight = 0.0; - - // Update the set points and Kf gains - double gyroDelta = -path.getHeading() - currentGyroAngle; - if (Math.abs(leftPoint.position) > 0.001) { - KfLeft = (pidParams.kA * leftPoint.acceleration + pidParams.kV * leftPoint.velocity + pidParams.kG * gyroDelta) / leftPoint.position; - KfRight = (pidParams.kA * rightPoint.acceleration + pidParams.kV * rightPoint.velocity - pidParams.kG * gyroDelta) / rightPoint.position; - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(rightPoint.position); - } - else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(leftPoint.position); - } - } - - path.incrementCounter(); - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java deleted file mode 100644 index 9863dc4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java +++ /dev/null @@ -1,111 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import jaci.pathfinder.Trajectory.Segment; - -public class MPTalonPIDPathVelocityController -{ - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected PathGenerator path; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - //motorController.configNominalOutputVoltage(+0.0f, -0.0f); - //motorController.configPeakOutputVoltage(+12.0f, -12.0f); - //motorController.setProfile(0); - motorController.config_kP(0, pidParams.kP, 0); - motorController.config_kI(0, pidParams.kI, 0); - motorController.config_kD(0, pidParams.kD, 0); - motorController.config_kF(0, pidParams.kF, 0); - motorController.configNominalOutputForward(+0.0f, 0); - motorController.configNominalOutputReverse(-0.0f, 0); - motorController.configPeakOutputForward(+1.0f, 0); - motorController.configPeakOutputReverse(-1.0f, 0); - motorController.selectProfileSlot(0, 0); - } - } - - public void setMPPathTarget(PathGenerator path) { - this.path = path; - path.resetCounter(); - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Speed); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Velocity, 0); - } - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - Segment leftPoint = path.getLeftPoint(); - Segment rightPoint = path.getRightPoint(); - - // Check if we are finished - if (leftPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double rightVelocity = rightPoint.velocity; - double leftVelocity = leftPoint.velocity; - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - motorController.setVelocityWorld(rightVelocity); - } - else { - motorController.setVelocityWorld(leftVelocity); - } - } - - path.incrementCounter(); - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java deleted file mode 100644 index eb2367c..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java +++ /dev/null @@ -1,184 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class MotionProfileBoxCar -{ - public static double DEFAULT_T1 = 200; // millisecond - public static double DEFAULT_T2 = 100; // millisecond - - private double startDistance; // any distance unit - private double targetDistance; // any distance unit - private double maxVelocity; // velocity unit consistent with targetDistance - - // Accel profile - // - // 0 T2 T1 - // | | | - // _____ - // / \ - // / \___ - // \ / - // \____/ - - private double itp; // time between points millisecond - private double t1 = DEFAULT_T1; // time when accel starts back to 0. millisecond (typically use t1 = 2 * t2) - private double t2 = DEFAULT_T2; // time when accel = max accel. millisecond - - private double t4; - private int numFilter1Boxes; - private int numFilter2Boxes; - private int numPoints; - - private int numITP; - private double filter1; - private double filter2; - private double previousPosition; - private double previousVelocity; - private double deltaFilter1; - private double[] filter2Window; - private int windowIndex; - private int pointIndex; - - public MotionProfileBoxCar(double startDistance, double targetDistance, double maxVelocity, double itp) { - this(startDistance, targetDistance, maxVelocity, itp, DEFAULT_T1, DEFAULT_T2); - } - - public MotionProfileBoxCar(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) { - this.startDistance = startDistance; - this.targetDistance = targetDistance; - this.maxVelocity = maxVelocity; - this.itp = itp; - this.t1 = t1; - this.t2 = t2; - - initializeProfile(); - } - - private void initializeProfile() { - // t4 is the time in ms it takes to get to the end point when at max velocity - t4 = Math.abs((targetDistance - startDistance)/maxVelocity) * 1000; - - // We need to make t4 an even multiple of itp - t4 = (int)(itp * Math.ceil(t4/itp)); - - // In the case where t4 is less than the accel times, we need to adjust the - // accel times down so the filters works out. Lots of ways to do this but - // to keep things simple we will make t4 slightly larger than the sum of - // the accel times. - if (t4 < t1 + t2) { - double total = t1 + t2 + t4; - double t1t2Ratio = t1/t2; - double t2Adjusted = Math.floor(total / 2 / (1 + t1t2Ratio) / itp); - if (t2Adjusted % 2 != 0) { - t2Adjusted -= 1; - } - t2 = t2Adjusted * itp; - t1 = t2 * t1t2Ratio; - t4 = total - t1 - t2; - } - - // Adjust max velocity so that the end point works out to the correct position. - maxVelocity = Math.abs((targetDistance - startDistance) / t4) * 1000; - - numFilter1Boxes = (int)Math.ceil(t1/itp); - numFilter2Boxes = (int)Math.ceil(t2/itp); - numPoints = (int)Math.ceil(t4/itp); - - numITP = numPoints + numFilter1Boxes + numFilter2Boxes; - filter1 = 0; - filter2 = 0; - previousVelocity = 0; - previousPosition = startDistance; - deltaFilter1 = 1.0/numFilter1Boxes; - filter2Window = new double[numFilter2Boxes]; - windowIndex = 0; - pointIndex = 0; - if (startDistance > targetDistance && maxVelocity > 0) { - maxVelocity = -maxVelocity; - } - } - - public MotionProfilePoint getNextPoint(MotionProfilePoint point) { - if (point == null) { - point = new MotionProfilePoint(); - } - - if (pointIndex == 0) { - point.initialize(startDistance); - pointIndex++; - return point; - } - else if (pointIndex > numITP) { - return null; - } - - int input = (pointIndex - 1) < numPoints ? 1 : 0; - if (input > 0) { - filter1 = Math.min(1, filter1 + deltaFilter1); - } - else { - filter1 = Math.max(0, filter1 - deltaFilter1); - } - - double firstFilter1InWindow = filter2Window[windowIndex]; - if (pointIndex <= numFilter2Boxes) { - firstFilter1InWindow = 0; - } - filter2Window[windowIndex] = filter1; - - filter2 += (filter1 - firstFilter1InWindow) / numFilter2Boxes; - - point.time = pointIndex * itp / 1000.0; - point.velocity = filter2 * maxVelocity; - point.position = previousPosition + (point.velocity + previousVelocity) / 2 * itp / 1000; - point.acceleration = (point.velocity - previousVelocity) / itp * 1000; - - previousVelocity = point.velocity; - previousPosition = point.position; - windowIndex++; - if (windowIndex == numFilter2Boxes) { - windowIndex = 0; - } - - pointIndex++; - - return point; - } - - public double getStartDistance() { - return startDistance; - } - - public double getTargetDistance() { - return targetDistance; - } - - public double getMaxVelocity() { - return maxVelocity; - } - - public double getItp() { - return itp; - } - - public double getT1() { - return t1; - } - - public double getT2() { - return t2; - } - - public static void main(String[] args) { - long startTime = System.nanoTime(); - - MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 96, 120, 10, 600, 300); - System.out.println("Time, Position, Velocity, Acceleration"); - MotionProfilePoint point = new MotionProfilePoint(); - while(mp.getNextPoint(point) != null) { - System.out.println(point.time + ", " + point.position + ", " + point.velocity + ", " + point.acceleration); - } - - long deltaTime = System.nanoTime() - startTime; - System.out.println("Time Box Car = " + (double)deltaTime * 1E-6 + " ms"); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java deleted file mode 100644 index 358138b..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class MotionProfilePoint { - public double time; - public double position; - public double velocity; - public double acceleration; - - public void initialize(double startPosition) { - time = 0; - position = startPosition; - velocity = 0; - acceleration = 0; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java deleted file mode 100644 index c989f65..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class PIDParams -{ - public double kP = 0; - public double kI = 0; - public double kD = 0; - public double kF = 0; - public double kA = 0; - public double kV = 0; - public double kG = 0; - public double iZone = 0; - public double rampRatePID = 0; - - public PIDParams() { - } - - public PIDParams(double kP) - { - this.kP = kP; - } - - public PIDParams(double kP, double kI, double kD) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - } - - public PIDParams(double kP, double kI, double kD, double kF) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - } - - public PIDParams(double kP, double kI, double kD, double kA, double kV) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kA = kA; - this.kV = kV; - } - - public PIDParams(double kP, double kI, double kD, double kA, double kV, double kG) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kA = kA; - this.kV = kV; - this.kG = kG; - } - - public PIDParams(double kP, double kI, double kD, double kA, double kV, double kG, double iZone) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kA = kA; - this.kV = kV; - this.kG = kG; - this.iZone = iZone; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java deleted file mode 100644 index 163c52d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java +++ /dev/null @@ -1,260 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; -import java.util.HashSet; -import java.util.Iterator; -import java.util.List; -import java.util.Optional; -import java.util.Set; - -import org.usfirst.frc4388.utility.Path.Waypoint; - -/** - * A Path is a recording of the path that the robot takes. Path objects consist - * of a List of Waypoints that the robot passes by. Using multiple Waypoints in - * a Path object and the robot's current speed, the code can extrapolate future - * Waypoints and predict the robot's motion. It can also dictate the robot's - * motion along the set path. - */ -public class Path { - protected static final double kSegmentCompletePercentage = .99; - - protected List mWaypoints; - protected List mSegments; - protected Set mMarkersCrossed; - - /** - * A point along the Path, which consists of the location, the speed, and a - * string marker (that future code can identify). Paths consist of a List of - * Waypoints. - */ - public static class Waypoint { - public final Translation2d position; - public final double speed; - public final Optional marker; - - public Waypoint(Translation2d position, double speed) { - this.position = position; - this.speed = speed; - this.marker = Optional.empty(); - } - - public Waypoint(Translation2d position, double speed, String marker) { - this.position = position; - this.speed = speed; - this.marker = Optional.of(marker); - } - } - - public Path(List waypoints) { - mMarkersCrossed = new HashSet(); - mWaypoints = waypoints; - mSegments = new ArrayList(); - for (int i = 0; i < waypoints.size() - 1; ++i) { - mSegments.add( - new PathSegment(waypoints.get(i).position, waypoints.get(i + 1).position, waypoints.get(i).speed)); - } - // The first waypoint is already complete - if (mWaypoints.size() > 0) { - Waypoint first_waypoint = mWaypoints.get(0); - if (first_waypoint.marker.isPresent()) { - mMarkersCrossed.add(first_waypoint.marker.get()); - } - mWaypoints.remove(0); - } - } - - /** - * @param An - * initial position - * @return Returns the distance from the position to the first point on the - * path - */ - public double update(Translation2d position) { - double rv = 0.0; - for (Iterator it = mSegments.iterator(); it.hasNext();) { - PathSegment segment = it.next(); - PathSegment.ClosestPointReport closest_point_report = segment.getClosestPoint(position); - if (closest_point_report.index >= kSegmentCompletePercentage) { - it.remove(); - if (mWaypoints.size() > 0) { - Waypoint waypoint = mWaypoints.get(0); - if (waypoint.marker.isPresent()) { - mMarkersCrossed.add(waypoint.marker.get()); - } - mWaypoints.remove(0); - } - } else { - if (closest_point_report.index > 0.0) { - // Can shorten this segment - segment.updateStart(closest_point_report.closest_point); - } - // We are done - rv = closest_point_report.distance; - // ...unless the next segment is closer now - if (it.hasNext()) { - PathSegment next = it.next(); - PathSegment.ClosestPointReport next_closest_point_report = next.getClosestPoint(position); - if (next_closest_point_report.index > 0 - && next_closest_point_report.index < kSegmentCompletePercentage - && next_closest_point_report.distance < rv) { - next.updateStart(next_closest_point_report.closest_point); - rv = next_closest_point_report.distance; - mSegments.remove(0); - if (mWaypoints.size() > 0) { - Waypoint waypoint = mWaypoints.get(0); - if (waypoint.marker.isPresent()) { - mMarkersCrossed.add(waypoint.marker.get()); - } - mWaypoints.remove(0); - } - } - } - break; - } - } - return rv; - } - - public Set getMarkersCrossed() { - return mMarkersCrossed; - } - - public double getRemainingLength() { - double length = 0.0; - for (int i = 0; i < mSegments.size(); ++i) { - length += mSegments.get(i).getLength(); - } - return length; - } - - /** - * @param The - * robot's current position - * @param A - * specified distance to predict a future waypoint - * @return A segment of the robot's predicted motion with start/end points - * and speed. - */ - public PathSegment.Sample getLookaheadPoint(Translation2d position, double lookahead_distance) { - if (mSegments.size() == 0) { - return new PathSegment.Sample(new Translation2d(), 0); - } - - // Check the distances to the start and end of each segment. As soon as - // we find a point > lookahead_distance away, we know the right point - // lies somewhere on that segment. - Translation2d position_inverse = position.inverse(); - if (position_inverse.translateBy(mSegments.get(0).getStart()).norm() >= lookahead_distance) { - // Special case: Before the first point, so just return the first - // point. - return new PathSegment.Sample(mSegments.get(0).getStart(), mSegments.get(0).getSpeed()); - } - for (int i = 0; i < mSegments.size(); ++i) { - PathSegment segment = mSegments.get(i); - double distance = position_inverse.translateBy(segment.getEnd()).norm(); - if (distance >= lookahead_distance) { - // This segment contains the lookahead point - Optional intersection_point = getFirstCircleSegmentIntersection(segment, position, - lookahead_distance); - if (intersection_point.isPresent()) { - return new PathSegment.Sample(intersection_point.get(), segment.getSpeed()); - } else { - System.out.println("ERROR: No intersection point?"); - } - } - } - // Special case: After the last point, so extrapolate forward. - PathSegment last_segment = mSegments.get(mSegments.size() - 1); - PathSegment new_last_segment = new PathSegment(last_segment.getStart(), last_segment.interpolate(10000), - last_segment.getSpeed()); - Optional intersection_point = getFirstCircleSegmentIntersection(new_last_segment, position, - lookahead_distance); - if (intersection_point.isPresent()) { - return new PathSegment.Sample(intersection_point.get(), last_segment.getSpeed()); - } else { - System.out.println("ERROR: No intersection point anywhere on line?"); - return new PathSegment.Sample(last_segment.getEnd(), last_segment.getSpeed()); - } - } - - static Optional getFirstCircleSegmentIntersection(PathSegment segment, Translation2d center, - double radius) { - double x1 = segment.getStart().getX() - center.getX(); - double y1 = segment.getStart().getY() - center.getY(); - double x2 = segment.getEnd().getX() - center.getX(); - double y2 = segment.getEnd().getY() - center.getY(); - double dx = x2 - x1; - double dy = y2 - y1; - double dr_squared = dx * dx + dy * dy; - double det = x1 * y2 - x2 * y1; - - double discriminant = dr_squared * radius * radius - det * det; - if (discriminant < 0) { - // No intersection - return Optional.empty(); - } - - double sqrt_discriminant = Math.sqrt(discriminant); - Translation2d pos_solution = new Translation2d( - (det * dy + (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(), - (-det * dx + Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY()); - Translation2d neg_solution = new Translation2d( - (det * dy - (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(), - (-det * dx - Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY()); - - // Choose the one between start and end that is closest to start - double pos_dot_product = segment.dotProduct(pos_solution); - double neg_dot_product = segment.dotProduct(neg_solution); - if (pos_dot_product < 0 && neg_dot_product >= 0) { - return Optional.of(neg_solution); - } else if (pos_dot_product >= 0 && neg_dot_product < 0) { - return Optional.of(pos_solution); - } else { - if (Math.abs(pos_dot_product) <= Math.abs(neg_dot_product)) { - return Optional.of(pos_solution); - } else { - return Optional.of(neg_solution); - } - } - } - - public static void addCircleArc(List waypoints, double radius, double angleDeg, int numPoints, String endMarker ) { - Waypoint last = waypoints.get(waypoints.size() - 1); - - double centerX = last.position.x_; - double centerY = radius; - - double deltaAngle = angleDeg / numPoints; - double currentAngle = deltaAngle; - for (int i = 0; i < numPoints; i++ ) { - double x = radius * Math.sin(Math.toRadians(currentAngle)) + centerX; - double y = centerY - radius * Math.cos(Math.toRadians(currentAngle)); - - if (i == numPoints - 1 && endMarker != null) { - Waypoint point = new Waypoint(new Translation2d(x, y), last.speed, endMarker); - waypoints.add(point); - } - else { - Waypoint point = new Waypoint(new Translation2d(x, y), last.speed); - waypoints.add(point); - } - - currentAngle += deltaAngle; - } - } - - public static void main(String[] args) { - - List waypoints = new ArrayList<>(); - waypoints.add(new Waypoint(new Translation2d(0, 0), 40.0)); - waypoints.add(new Waypoint(new Translation2d(-35, 0), 40.0)); - Path.addCircleArc(waypoints, 30.0, -45.0, 10, "hopperSensorOn"); - waypoints.add(new Waypoint(new Translation2d(-85, 30), 40.0)); - - for (int i = 0; i < waypoints.size(); i++) { - Waypoint curPoint = waypoints.get(i); - System.out.println("x = " + curPoint.position.x_ + ", y = " + curPoint.position.y_); - } - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java deleted file mode 100644 index f99df73..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java +++ /dev/null @@ -1,232 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.awt.Color; - -import org.usfirst.frc4388.robot.subsystems.Drive; - -import jaci.pathfinder.Pathfinder; -import jaci.pathfinder.Trajectory; -import jaci.pathfinder.Trajectory.Segment; -import jaci.pathfinder.Waypoint; -import jaci.pathfinder.modifiers.TankModifier; - -public class PathGenerator { - - private Segment[] centerPoints; - private Segment[] leftPoints; - private Segment[] rightPoints; - private int curIndex = 0; - - public PathGenerator(Waypoint[] points, double timeStep, double maxVelocity, double maxAccel, double maxJerk) { - - boolean negativeFlag = false; - for (int i = 0; i < points.length; i++) { - if (points[i].x < 0) { - negativeFlag = true; - } - } - if (negativeFlag == true) { - for (int i = 0; i < points.length; i++) { - points[i].x = -(points[i].x); - } - } - - Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_LOW, timeStep, maxVelocity, maxAccel, maxJerk); - Trajectory trajectory = Pathfinder.generate(points, config); - centerPoints = trajectory.segments; - - TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES); - leftPoints = modifier.getLeftTrajectory().segments; - rightPoints = modifier.getRightTrajectory().segments; - - if (negativeFlag == true) { - for (int i = 0; i < centerPoints.length; i++) { - Segment curSeg = centerPoints[i]; - curSeg.x = -(curSeg.x); - curSeg.y = -(curSeg.y); - curSeg.jerk = -(curSeg.jerk); - curSeg.acceleration = -(curSeg.acceleration); - curSeg.velocity = -(curSeg.velocity); - curSeg.position = -(curSeg.position); - curSeg.heading = -(curSeg.heading); - curSeg = leftPoints[i]; - curSeg.x = -(curSeg.x); - curSeg.y = -(curSeg.y); - curSeg.jerk = -(curSeg.jerk); - curSeg.acceleration = -(curSeg.acceleration); - curSeg.velocity = -(curSeg.velocity); - curSeg.position = -(curSeg.position); - curSeg.heading = -(curSeg.heading); - curSeg = rightPoints[i]; - curSeg.x = -(curSeg.x); - curSeg.y = -(curSeg.y); - curSeg.jerk = -(curSeg.jerk); - curSeg.acceleration = -(curSeg.acceleration); - curSeg.velocity = -(curSeg.velocity); - curSeg.position = -(curSeg.position); - curSeg.heading = -(curSeg.heading); - } - } - -// TankModifier2 modifier2 = new TankModifier2(); -// modifier2.modify(centerPoints, Drive.TRACK_WIDTH_INCHES); -// leftPoints = modifier2.getLeftSegments(); -// rightPoints = modifier2.getRightSegments(); - } - - public Segment getLeftPoint() { - return (curIndex < leftPoints.length) ? leftPoints[curIndex] : null; - } - - public Segment getRightPoint() { - return (curIndex < rightPoints.length) ? rightPoints[curIndex] : null; - } - - public Segment getCenterPoint() { - return (curIndex < centerPoints.length) ? centerPoints[curIndex] : null; - } - - public Double getHeading() { - if (curIndex < centerPoints.length) { - double heading = Math.toDegrees(centerPoints[curIndex].heading); - if (heading > 180) { - heading -= 360; - } - else if (heading < -180) { - heading += 360; - } - return heading; - } - return null; - } - - public void incrementCounter() { - curIndex++; - } - - public void resetCounter() { - curIndex =0; - } - - public Segment[] getCenterPoints() { - return centerPoints; - } - - public Segment[] getLeftPoints() { - return leftPoints; - } - - public Segment[] getRightPoints() { - return rightPoints; - } - - public static void main(String[] args) { - Waypoint[] points = new Waypoint[] { - new Waypoint(0, 0, 0), - new Waypoint(78, 20, Pathfinder.d2r(32)) - }; - - PathGenerator path = new PathGenerator(points, 0.01, 120, 200.0, 700.0); - Segment[] centerSegments = path.getCenterPoints(); - Segment[] leftSegments = path.getLeftPoints(); - Segment[] rightSegments = path.getRightPoints(); - - double[] heading = new double[centerSegments.length]; - double[] centerX = new double[centerSegments.length]; - double[] centerY = new double[centerSegments.length]; - double[] centerAccel = new double[centerSegments.length]; - double[] centerVelocity = new double[centerSegments.length]; - double[] centerPosition = new double[centerSegments.length]; - double[] leftX = new double[leftSegments.length]; - double[] leftY = new double[leftSegments.length]; - double[] leftAccel = new double[leftSegments.length]; - double[] leftVelocity = new double[leftSegments.length]; - double[] leftPosition = new double[leftSegments.length]; - double[] rightX = new double[rightSegments.length]; - double[] rightY = new double[rightSegments.length]; - double[] rightAccel = new double[rightSegments.length]; - double[] rightVelocity = new double[rightSegments.length]; - double[] rightPosition = new double[rightSegments.length]; - - path.resetCounter(); - for (int i = 0; i < centerSegments.length; i++) { - heading[i] = path.getHeading(); - centerX[i] = path.getCenterPoint().x; - centerY[i] = path.getCenterPoint().y; - centerAccel[i] = path.getCenterPoint().acceleration; - centerVelocity[i] = path.getCenterPoint().velocity; - centerPosition[i] = path.getCenterPoint().position; - leftX[i] = path.getLeftPoint().x; - leftY[i] = path.getLeftPoint().y; - leftAccel[i] = path.getLeftPoint().acceleration; - leftVelocity[i] = path.getLeftPoint().velocity; - leftPosition[i] = path.getLeftPoint().position; - rightX[i] = path.getRightPoint().x; - rightY[i] = path.getRightPoint().y; - rightAccel[i] = path.getRightPoint().acceleration; - rightVelocity[i] = path.getRightPoint().velocity; - rightPosition[i] = path.getRightPoint().position; - path.incrementCounter(); - } - - FalconLinePlot fig4 = new FalconLinePlot(centerX); - fig4.yGridOn(); - fig4.xGridOn(); - fig4.setYLabel("X (in)"); - fig4.setXLabel("time"); - fig4.setTitle("Position Profile X"); - fig4.addData(rightX, Color.magenta); - fig4.addData(leftX, Color.blue); - - FalconLinePlot fig0 = new FalconLinePlot(centerY); - fig0.yGridOn(); - fig0.xGridOn(); - fig0.setYLabel("Y (in)"); - fig0.setXLabel("time"); - fig0.setTitle("Position Profile Y"); - fig0.addData(rightY, Color.magenta); - fig0.addData(leftY, Color.blue); - - FalconLinePlot fig1 = new FalconLinePlot(centerPosition); - fig1.yGridOn(); - fig1.xGridOn(); - fig1.setYLabel("Position (in)"); - fig1.setXLabel("time (seconds)"); - fig1.setTitle("Position Profile for Left and Right Wheels \n Left = Blue, Right = Magenta"); - fig1.addData(rightPosition, Color.magenta); - fig1.addData(leftPosition, Color.blue); - - FalconLinePlot fig2 = new FalconLinePlot(centerVelocity, Color.green, Color.green); - fig2.yGridOn(); - fig2.xGridOn(); - fig2.setYLabel("Velocity (in/sec)"); - fig2.setXLabel("time (seconds)"); - fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Blue, Right = Magenta"); - fig2.addData(rightVelocity, Color.magenta); - fig2.addData(leftVelocity, Color.blue); - - FalconLinePlot fig3 = new FalconLinePlot(centerAccel, Color.green, Color.green); - fig3.yGridOn(); - fig3.xGridOn(); - fig3.setYLabel("Accel (in/sec^2)"); - fig3.setXLabel("time (seconds)"); - fig3.setTitle("Accel Profile for Left and Right Wheels \n Left = Blue, Right = Magenta"); - fig3.addData(rightAccel, Color.magenta); - fig3.addData(leftAccel, Color.blue); - - FalconLinePlot fig5 = new FalconLinePlot(heading); - fig5.yGridOn(); - fig5.xGridOn(); - fig5.setYLabel("Heading"); - fig5.setXLabel("time"); - fig5.setTitle("Heading Profile"); - - FalconLinePlot fig6 = new FalconLinePlot(centerX, centerY); - fig6.yGridOn(); - fig6.xGridOn(); - fig6.setYLabel("Y"); - fig6.setXLabel("X"); - fig6.setTitle("XY Profile"); - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java deleted file mode 100644 index df9499d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java +++ /dev/null @@ -1,89 +0,0 @@ -package org.usfirst.frc4388.utility; - -/** - * A PathSegment consists of two Translation2d objects (the start and end - * points) as well as the speed of the robot. - * - */ -public class PathSegment { - protected static final double kEpsilon = 1E-9; - - public static class Sample { - public final Translation2d translation; - public final double speed; - - public Sample(Translation2d translation, double speed) { - this.translation = translation; - this.speed = speed; - } - } - - protected double mSpeed; - protected Translation2d mStart; - protected Translation2d mEnd; - protected Translation2d mStartToEnd; // pre-computed for efficiency - protected double mLength; // pre-computed for efficiency - - public static class ClosestPointReport { - public double index; // Index of the point on the path segment (not - // clamped to [0, 1]) - public double clamped_index; // As above, but clamped to [0, 1] - public Translation2d closest_point; // The result of - // interpolate(clamped_index) - public double distance; // The distance from closest_point to the query - // point - } - - public PathSegment(Translation2d start, Translation2d end, double speed) { - mEnd = end; - mSpeed = speed; - updateStart(start); - } - - public void updateStart(Translation2d new_start) { - mStart = new_start; - mStartToEnd = mStart.inverse().translateBy(mEnd); - mLength = mStartToEnd.norm(); - } - - public double getSpeed() { - return mSpeed; - } - - public Translation2d getStart() { - return mStart; - } - - public Translation2d getEnd() { - return mEnd; - } - - public double getLength() { - return mLength; - } - - // Index is on [0, 1] - public Translation2d interpolate(double index) { - return mStart.interpolate(mEnd, index); - } - - public double dotProduct(Translation2d other) { - Translation2d start_to_other = mStart.inverse().translateBy(other); - return mStartToEnd.getX() * start_to_other.getX() + mStartToEnd.getY() * start_to_other.getY(); - } - - public ClosestPointReport getClosestPoint(Translation2d query_point) { - ClosestPointReport rv = new ClosestPointReport(); - if (mLength > kEpsilon) { - double dot_product = dotProduct(query_point); - rv.index = dot_product / (mLength * mLength); - rv.clamped_index = Math.min(1.0, Math.max(0.0, rv.index)); - rv.closest_point = interpolate(rv.index); - } else { - rv.index = rv.clamped_index = 0.0; - rv.closest_point = new Translation2d(mStart); - } - rv.distance = rv.closest_point.inverse().translateBy(query_point).norm(); - return rv; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java deleted file mode 100644 index c48b552..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.usfirst.frc4388.utility; - -/** - * Represents a 2d pose (rigid transform) containing translational and - * rotational elements. - * - * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) - */ -public class RigidTransform2d implements Interpolable { - private final static double kEps = 1E-9; - - // Movement along an arc at constant curvature and velocity. We can use - // ideas from "differential calculus" to create new RigidTransform2d's from - // a Delta. - public static class Delta { - public final double dx; - public final double dy; - public final double dtheta; - - public Delta(double dx, double dy, double dtheta) { - this.dx = dx; - this.dy = dy; - this.dtheta = dtheta; - } - } - - protected Translation2d translation_; - protected Rotation2d rotation_; - - public RigidTransform2d() { - translation_ = new Translation2d(); - rotation_ = new Rotation2d(); - } - - public RigidTransform2d(Translation2d translation, Rotation2d rotation) { - translation_ = translation; - rotation_ = rotation; - } - - public RigidTransform2d(RigidTransform2d other) { - translation_ = new Translation2d(other.translation_); - rotation_ = new Rotation2d(other.rotation_); - } - - public static RigidTransform2d fromTranslation(Translation2d translation) { - return new RigidTransform2d(translation, new Rotation2d()); - } - - public static RigidTransform2d fromRotation(Rotation2d rotation) { - return new RigidTransform2d(new Translation2d(), rotation); - } - - /** - * Obtain a new RigidTransform2d from a (constant curvature) velocity. See: - * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp - */ - public static RigidTransform2d fromVelocity(Delta delta) { - double sin_theta = Math.sin(delta.dtheta); - double cos_theta = Math.cos(delta.dtheta); - double s, c; - if (Math.abs(delta.dtheta) < kEps) { - s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta; - c = .5 * delta.dtheta; - } else { - s = sin_theta / delta.dtheta; - c = (1.0 - cos_theta) / delta.dtheta; - } - return new RigidTransform2d(new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s), - new Rotation2d(cos_theta, sin_theta, false)); - } - - public Translation2d getTranslation() { - return translation_; - } - - public void setTranslation(Translation2d translation) { - translation_ = translation; - } - - public Rotation2d getRotation() { - return rotation_; - } - - public void setRotation(Rotation2d rotation) { - rotation_ = rotation; - } - - /** - * Transforming this RigidTransform2d means first translating by - * other.translation and then rotating by other.rotation - * - * @param other - * The other transform. - * @return This transform * other - */ - public RigidTransform2d transformBy(RigidTransform2d other) { - return new RigidTransform2d(translation_.translateBy(other.translation_.rotateBy(rotation_)), - rotation_.rotateBy(other.rotation_)); - } - - /** - * The inverse of this transform "undoes" the effect of translating by this - * transform. - * - * @return The opposite of this transform. - */ - public RigidTransform2d inverse() { - Rotation2d rotation_inverted = rotation_.inverse(); - return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted); - } - - /** - * Do linear interpolation of this transform (there are more accurate ways - * using constant curvature, but this is good enough). - */ - @Override - public RigidTransform2d interpolate(RigidTransform2d other, double x) { - if (x <= 0) { - return new RigidTransform2d(this); - } else if (x >= 1) { - return new RigidTransform2d(other); - } - return new RigidTransform2d(translation_.interpolate(other.translation_, x), - rotation_.interpolate(other.rotation_, x)); - } - - @Override - public String toString() { - return "T:" + translation_.toString() + ", R:" + rotation_.toString(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java deleted file mode 100644 index 4da86a3..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java +++ /dev/null @@ -1,124 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.text.DecimalFormat; - -/** - * A rotation in a 2d coordinate frame represented a point on the unit circle - * (cosine and sine). - * - * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) - */ -public class Rotation2d implements Interpolable { - protected static final double kEpsilon = 1E-9; - - protected double cos_angle_; - protected double sin_angle_; - - public Rotation2d() { - this(1, 0, false); - } - - public Rotation2d(double x, double y, boolean normalize) { - cos_angle_ = x; - sin_angle_ = y; - if (normalize) { - normalize(); - } - } - - public Rotation2d(Rotation2d other) { - cos_angle_ = other.cos_angle_; - sin_angle_ = other.sin_angle_; - } - - public static Rotation2d fromRadians(double angle_radians) { - return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false); - } - - public static Rotation2d fromDegrees(double angle_degrees) { - return fromRadians(Math.toRadians(angle_degrees)); - } - - /** - * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this - * object we might accumulate rounding errors. Normalizing forces us to - * re-scale the sin and cos to reset rounding errors. - */ - public void normalize() { - double magnitude = Math.hypot(cos_angle_, sin_angle_); - if (magnitude > kEpsilon) { - sin_angle_ /= magnitude; - cos_angle_ /= magnitude; - } else { - sin_angle_ = 0; - cos_angle_ = 1; - } - } - - public double cos() { - return cos_angle_; - } - - public double sin() { - return sin_angle_; - } - - public double tan() { - if (cos_angle_ < kEpsilon) { - if (sin_angle_ >= 0.0) { - return Double.POSITIVE_INFINITY; - } else { - return Double.NEGATIVE_INFINITY; - } - } - return sin_angle_ / cos_angle_; - } - - public double getRadians() { - return Math.atan2(sin_angle_, cos_angle_); - } - - public double getDegrees() { - return Math.toDegrees(getRadians()); - } - - /** - * We can rotate this Rotation2d by adding together the effects of it and - * another rotation. - * - * @param other - * The other rotation. See: - * https://en.wikipedia.org/wiki/Rotation_matrix - * @return This rotation rotated by other. - */ - public Rotation2d rotateBy(Rotation2d other) { - return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_, - cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true); - } - - /** - * The inverse of a Rotation2d "undoes" the effect of this rotation. - * - * @return The opposite of this rotation. - */ - public Rotation2d inverse() { - return new Rotation2d(cos_angle_, -sin_angle_, false); - } - - @Override - public Rotation2d interpolate(Rotation2d other, double x) { - if (x <= 0) { - return new Rotation2d(this); - } else if (x >= 1) { - return new Rotation2d(other); - } - double angle_diff = inverse().rotateBy(other).getRadians(); - return this.rotateBy(Rotation2d.fromRadians(angle_diff * x)); - } - - @Override - public String toString() { - final DecimalFormat fmt = new DecimalFormat("#0.000"); - return "(" + fmt.format(getDegrees()) + " deg)"; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java deleted file mode 100644 index 7bd6abd..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java +++ /dev/null @@ -1,114 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -public class SoftwarePIDController -{ - protected ArrayList motorControllers; - protected PIDParams pidParams; - protected boolean useGyroLock; - protected double targetGyroAngle; - protected MPSoftwareTurnType turnType; - - protected double minTurnOutput = 0.002; - protected double maxError; - protected double maxPrevError; - protected double prevError = 0.0; // the prior error (used to compute velocity) - protected double totalError = 0.0; // the sum of the errors for use in the integral calc - - public SoftwarePIDController( PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - } - - - public void setPIDTurnTarget(double targetAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { - this.turnType = turnType; - this.targetGyroAngle = targetAngleDeg; - this.useGyroLock = true; - this.maxError = maxError; - this.maxPrevError = maxPrevError; - - // NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - - prevError = 0.0; - totalError = 0.0; - - } - - public boolean controlLoopUpdate() { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - // Calculate the motion profile feed forward and error feedback terms - double error = targetGyroAngle - currentGyroAngle; - double deltaLastError = (error - prevError); - - // Check if we are finished - if (Math.abs(error) < maxError && Math.abs(deltaLastError) < maxPrevError) { - return true; - } - - - if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) { - totalError += error; - } - else { - totalError = 0; - } - - double output = pidParams.kP * error + pidParams.kI * totalError + pidParams.kD * deltaLastError; - double turnBoost = output < 0 ? -minTurnOutput : minTurnOutput; - output += turnBoost; - - prevError = error; - - // Update the controllers set point. - if (turnType == MPSoftwareTurnType.TANK) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.set(output); - motorController.set(ControlMode.PercentOutput, output); - } - } - else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - } - } - else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - else { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - } - } - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java deleted file mode 100644 index 383acd7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java +++ /dev/null @@ -1,85 +0,0 @@ - -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -import edu.wpi.first.wpilibj.Timer; - -public class SoftwarePIDPositionController -{ - //protected ArrayList motorControllers; - protected WPI_TalonSRX motorController; - protected PIDParams pidParams; - - protected PIDParams PValue; - - protected double targetEncoderPosition; - - protected double minTurnOutput = 0.002; - protected double maxError; - protected double minError; - protected double maxPrevError; ///?? - protected double prevError = 0.0; // the prior error (used to compute velocity) - protected double totalError = 0.0; // the sum of the errors for use in the integral calc - - public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft) - { - this.motorController = elevatorLeft; - setP(PValue); - } - - public void setP(PIDParams PValue) - { - this.PValue = PValue; - } - - public void setPIDPositionTarget(double targetPosition, double maxError, double minError) - { - this.targetEncoderPosition = targetPosition; - - this.maxError = maxError; - this.minError = minError; - //this.maxPrevError = maxPrevError; - - prevError = 0.0; - totalError = 0.0; - } - - public boolean controlLoopUpdate() - { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentEncoderPosition) - { - // Calculate the motion profile feed forward and error feedback terms - double error = targetEncoderPosition - currentEncoderPosition; - //double deltaLastError = (error - prevError); - - //Updating the error - //totalError += error; - - // Check if we are finished - if (Math.abs(error) < maxError && Math.abs(error) > minError) - { - //Robot.elevator.holdInPos(); - - return true; - } - - double output = pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError; - - prevError = error; - - // Update the controllers set point. - motorController.set(ControlMode.PercentOutput, output); - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java deleted file mode 100644 index 05417a8..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java +++ /dev/null @@ -1,104 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.text.DecimalFormat; - -/** - * A translation in a 2d coordinate frame. Translations are simply shifts in an - * (x, y) plane. - */ -public class Translation2d implements Interpolable { - protected double x_; - protected double y_; - - public Translation2d() { - x_ = 0; - y_ = 0; - } - - public Translation2d(double x, double y) { - x_ = x; - y_ = y; - } - - public Translation2d(Translation2d other) { - x_ = other.x_; - y_ = other.y_; - } - - /** - * The "norm" of a transform is the Euclidean distance in x and y. - * - * @return Sqrt(x^2 + y^2) - */ - public double norm() { - return Math.hypot(x_, y_); - } - - public double getX() { - return x_; - } - - public double getY() { - return y_; - } - - public void setX(double x) { - x_ = x; - } - - public void setY(double y) { - y_ = y; - } - - /** - * We can compose Translation2d's by adding together the x and y shifts. - * - * @param other - * The other translation to add. - * @return The combined effect of translating by this object and the other. - */ - public Translation2d translateBy(Translation2d other) { - return new Translation2d(x_ + other.x_, y_ + other.y_); - } - - /** - * We can also rotate Translation2d's. See: - * https://en.wikipedia.org/wiki/Rotation_matrix - * - * @param rotation - * The rotation to apply. - * @return This translation rotated by rotation. - */ - public Translation2d rotateBy(Rotation2d rotation) { - return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos()); - } - - /** - * The inverse simply means a Translation2d that "undoes" this object. - * - * @return Translation by -x and -y. - */ - public Translation2d inverse() { - return new Translation2d(-x_, -y_); - } - - @Override - public Translation2d interpolate(Translation2d other, double x) { - if (x <= 0) { - return new Translation2d(this); - } else if (x >= 1) { - return new Translation2d(other); - } - return extrapolate(other, x); - } - - public Translation2d extrapolate(Translation2d other, double x) { - return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_); - } - - @Override - public String toString() { - final DecimalFormat fmt = new DecimalFormat("#0.000"); - return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")"; - } -} diff --git a/2019robot/vendordeps/PathfinderOLD.json b/2019robot/vendordeps/PathfinderOLD.json deleted file mode 100644 index 2551e4d..0000000 --- a/2019robot/vendordeps/PathfinderOLD.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "fileName": "PathfinderOLD.json", - "name": "PathfinderOld", - "version": "2019.1.11", - "uuid": "7194a2d4-2860-4bcc-86c0-97879737d875", - "mavenUrls": [ - "https://dev.imjac.in/maven" - ], - "jsonUrl": "https://dev.imjac.in/maven/jaci/pathfinder/PathfinderOLD-latest.json", - "cppDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-Core", - "version": "2019.1.11", - "libName": "pathfinder", - "configuration": "native_pathfinder_old", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-FRCSupport", - "version": "2019.1.11", - "libName": "pathfinder_frc", - "configuration": "native_pathfinder_old", - "headerClassifier": "headers", - "binaryPlatforms": [] - } - ], - "javaDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-Java", - "version": "2019.1.11" - }, - { - "groupId": "jaci.jniloader", - "artifactId": "JNILoader", - "version": "1.0.1" - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-FRCSupport", - "version": "2019.1.11" - } - ], - "jniDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-JNI", - "version": "2019.1.11", - "isJar": true, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-CoreJNI", - "version": "2019.1.11", - "isJar": true, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - } - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/Phoenix.json b/2019robot/vendordeps/Phoenix.json deleted file mode 100644 index d4da1ce..0000000 --- a/2019robot/vendordeps/Phoenix.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix", - "version": "5.12.0", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "http://devsite.ctr-electronics.com/maven/release/" - ], - "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.12.0" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.12.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.12.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.12.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.12.0", - "libName": "CTRE_PhoenixCore", - "headerClassifier": "headers" - } - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/REVRobotics.json b/2019robot/vendordeps/REVRobotics.json deleted file mode 100644 index d997798..0000000 --- a/2019robot/vendordeps/REVRobotics.json +++ /dev/null @@ -1,32 +0,0 @@ -{ - "fileName": "REVRobotics.json", - "name": "REVRobotics", - "version": "1.0.26", - "uuid": "c16ed09f-23df-4beb-87e8-460bd7fa9924", - "mavenUrls": [ - "http://www.revrobotics.com/content/sw/max/sdk/maven/" - ], - "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-java", - "version": "1.0.26" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-cpp", - "version": "1.0.26", - "libName": "libSparkMax", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - } - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/cw.json b/2019robot/vendordeps/cw.json deleted file mode 100644 index 2c3329e..0000000 --- a/2019robot/vendordeps/cw.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "fileName": "cw.json", - "name": "cw", - "version": "3.1.344", - "uuid":"13", - "mavenUrls": [ - "https://mvnrepository.com" - ], - "jsonUrl": "file:///C:/dev/cw.json", - "javaDependencies": [ - { - "groupId": "com.googlecode.json-simple", - "artifactId": "json-simple", - "version": "1.1.1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/navx_frc.json b/2019robot/vendordeps/navx_frc.json deleted file mode 100644 index 80defba..0000000 --- a/2019robot/vendordeps/navx_frc.json +++ /dev/null @@ -1,33 +0,0 @@ -{ - "fileName": "navx_frc.json", - "name": "KauaiLabs_navX_FRC", - "version": "3.1.344", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "mavenUrls": [ - "https://repo1.maven.org/maven2/" - ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2019/navx_frc.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-java", - "version": "3.1.344" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-cpp", - "version": "3.1.344", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - } - ] -} \ No newline at end of file