From caa182fa28ba29110d94199c9fe317e8f4f7b91a Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Mon, 14 Jan 2019 19:51:11 -0700 Subject: [PATCH] initial commit hitormiss --- 2019robot/.gitignore | 160 ++++ 2019robot/.vscode/launch.json | 21 + 2019robot/.vscode/settings.json | 14 + 2019robot/.wpilib/wpilib_preferences.json | 6 + 2019robot/build.gradle | 62 ++ 2019robot/gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 55741 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 | 163 ++++ .../org/usfirst/frc4388/robot/RobotMap.java | 36 + .../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 +++ .../frc4388/robot/commands/ElevatorBasic.java | 99 ++ .../robot/commands/ElevatorSetSpeed.java | 44 + .../robot/commands/InitiateClimber.java | 38 + .../frc4388/robot/subsystems/Climber.java | 68 ++ .../frc4388/robot/subsystems/Drive.java | 863 +++++++++++++++++ .../frc4388/robot/subsystems/Elevator.java | 450 +++++++++ .../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/navx_frc.json | 33 + 64 files changed, 7496 insertions(+) create mode 100644 2019robot/.gitignore create mode 100644 2019robot/.vscode/launch.json create mode 100644 2019robot/.vscode/settings.json create mode 100644 2019robot/.wpilib/wpilib_preferences.json create mode 100644 2019robot/build.gradle create mode 100644 2019robot/gradle/wrapper/gradle-wrapper.jar create mode 100644 2019robot/gradle/wrapper/gradle-wrapper.properties create mode 100644 2019robot/gradlew create mode 100644 2019robot/gradlew.bat create mode 100644 2019robot/settings.gradle create mode 100644 2019robot/src/main/deploy/example.txt create mode 100644 2019robot/src/main/java/buttons/XBoxTriggerButton.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java create mode 100644 2019robot/vendordeps/PathfinderOLD.json create mode 100644 2019robot/vendordeps/Phoenix.json create mode 100644 2019robot/vendordeps/navx_frc.json diff --git a/2019robot/.gitignore b/2019robot/.gitignore new file mode 100644 index 0000000..61f25ce --- /dev/null +++ b/2019robot/.gitignore @@ -0,0 +1,160 @@ +# 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 new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/2019robot/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // 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 new file mode 100644 index 0000000..860e319 --- /dev/null +++ b/2019robot/.vscode/settings.json @@ -0,0 +1,14 @@ +{ + "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 new file mode 100644 index 0000000..95c6671 --- /dev/null +++ b/2019robot/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2019", + "teamNumber": 4388 +} \ No newline at end of file diff --git a/2019robot/build.gradle b/2019robot/build.gradle new file mode 100644 index 0000000..f15e84c --- /dev/null +++ b/2019robot/build.gradle @@ -0,0 +1,62 @@ +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' + compile pathfinder() +} + +// 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 new file mode 100644 index 0000000000000000000000000000000000000000..457aad0d98108420a977756b7145c93c8910b076 GIT binary patch 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 literal 0 HcmV?d00001 diff --git a/2019robot/gradle/wrapper/gradle-wrapper.properties b/2019robot/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..d08253c --- /dev/null +++ b/2019robot/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +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 new file mode 100644 index 0000000..af6708f --- /dev/null +++ b/2019robot/gradlew @@ -0,0 +1,172 @@ +#!/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 new file mode 100644 index 0000000..6d57edc --- /dev/null +++ b/2019robot/gradlew.bat @@ -0,0 +1,84 @@ +@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 new file mode 100644 index 0000000..b0f4d48 --- /dev/null +++ b/2019robot/settings.gradle @@ -0,0 +1,25 @@ +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 new file mode 100644 index 0000000..70c79b6 --- /dev/null +++ b/2019robot/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000..7abfe7c --- /dev/null +++ b/2019robot/src/main/java/buttons/XBoxTriggerButton.java @@ -0,0 +1,61 @@ +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 new file mode 100644 index 0000000..d8f08dc --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java @@ -0,0 +1,12 @@ +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 new file mode 100644 index 0000000..040282a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java @@ -0,0 +1,205 @@ + +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 new file mode 100644 index 0000000..96a6f89 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -0,0 +1,72 @@ + +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 kElevatorEncoderTickesPerRev = 256; + public static double kElevatorInchesOfTravelPerRev = 3.75; + public static double kElevatorEncoderTicksPerInch = 126.36; + public static double kElevatorBasicPercentOutputUp = -0.85; + public static double kElevatorBasicPercentOutputDown =.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 new file mode 100644 index 0000000..91b4cae --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java @@ -0,0 +1,22 @@ +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 new file mode 100644 index 0000000..4fc1133 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -0,0 +1,101 @@ + + + +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 new file mode 100644 index 0000000..36fea3e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -0,0 +1,163 @@ + +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 Elevator elevator = new Elevator(); + + + + 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(elevator); + + + 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); + elevator.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 new file mode 100644 index 0000000..64e015f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -0,0 +1,36 @@ + +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; + + //Elevator Motors + public static final int ELEVATOR_MOTOR1_ID = 6; + public static final int ELEVATOR_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/DriveAbsoluteTurnMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java new file mode 100644 index 0000000..5c25540 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java @@ -0,0 +1,42 @@ +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 new file mode 100644 index 0000000..9d2d3e4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java @@ -0,0 +1,38 @@ +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 new file mode 100644 index 0000000..0bc270b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java @@ -0,0 +1,38 @@ +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 new file mode 100644 index 0000000..d3e8293 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java @@ -0,0 +1,40 @@ +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 new file mode 100644 index 0000000..a477dc7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java @@ -0,0 +1,103 @@ +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 new file mode 100644 index 0000000..21fede7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java @@ -0,0 +1,115 @@ +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 new file mode 100644 index 0000000..e205818 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java @@ -0,0 +1,60 @@ +// 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 new file mode 100644 index 0000000..82223d9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java @@ -0,0 +1,151 @@ +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/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java new file mode 100644 index 0000000..df2b718 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java @@ -0,0 +1,99 @@ +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 ElevatorBasic 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 ElevatorBasic(double targetHeightInchesAboveFloor) { + requires(Robot.elevator); + m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; + } + + // Called just before this Command runs the first time + protected void initialize() + { + Robot.elevator.setControlMode(DriveControlMode.RAW); + + double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); + // 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.kElevatorBasicPercentOutputUp; + } + else { + m_speed = Constants.kElevatorBasicPercentOutputDown; + } + 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.elevator.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.elevator.getElevatorHeightInchesAboveFloor(); + 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.elevator.rawSetOutput(0.0); + + Robot.elevator.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/ElevatorSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java new file mode 100644 index 0000000..7553d48 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java @@ -0,0 +1,44 @@ +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 ElevatorSetSpeed extends Command { + + private double RaiseSpeed; + + // Constructor with speed + public ElevatorSetSpeed(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/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java new file mode 100644 index 0000000..8a6e37d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java @@ -0,0 +1,38 @@ +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/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..3cacf76 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,68 @@ + + + + +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 new file mode 100644 index 0000000..b984af3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -0,0 +1,863 @@ + +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); + + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + //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/Elevator.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java new file mode 100644 index 0000000..4a17e98 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java @@ -0,0 +1,450 @@ +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 Elevator extends Subsystem implements ControlLoopable +{ + //PID encoder and motor + private CANTalonEncoder elevatorRight; + private WPI_TalonSRX elevatorLeft; + + //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.kElevatorEncoderTicksPerInch; + + //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 elevatorControlMode = false; + // Motor controllers + //private ArrayList motorControllers = new ArrayList(); + + public Elevator() + { + try + { + //PID elevator encoder and talon + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID); + + elevatorRight.setInverted(false); + + //Setting left elevator motor as follower + elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID()); + elevatorLeft.setInverted(false); + elevatorLeft.setNeutralMode(NeutralMode.Brake); + elevatorRight.setNeutralMode(NeutralMode.Brake); + elevatorRight.setSensorPhase(true); + //Limit Switch Left + //elevatorLeft.overrideLimitSwitchesEnable(true); + elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + //Limit Switch Right + //elevatorRight.overrideLimitSwitchesEnable(true); + //elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + + //Change This boi + + // elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here + //elevatorRight.configReverseSoftLimitThreshold(5, 0); + //elevatorRight.configForwardSoftLimitEnable(true, 0); + //elevatorRight.configReverseSoftLimitEnable(true, 0); + + //sos + //elevatorRight.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 Elevator 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 setElevatorMode() + { + setControlMode(DriveControlMode.JOYSTICK); + } + + public void resetElevatorEncoder() + { + elevatorRight.setSelectedSensorPosition(0, 0, 0); + } + + public void moveElevatorXbox() + { + double moveElevatorInput; + double elevatorSafeZone = 15; + + double elevatorPos = getEncoderElevatorPosition(); + + moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis(); + + //double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY); + + boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); + boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + + if(elevatorTuningPressed == true) + { + elevatorRight.set(moveElevatorInput * 0.5); + } + else if(elevatorTuningPressed == false) + { + elevatorRight.set(moveElevatorInput); + } + + /* + if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) + { + elevatorRight.set(moveElevatorInput); + } + else if(elevatorPos > elevatorSafeZone) + { + elevatorRight.set(moveElevatorInput * 0.65); + + + if(holdButtonPressed == true) + { + elevatorRight.set(-0.43 * (0.2)); + } + else if(holdButtonPressed == false) + { + elevatorRight.set(moveElevatorInput * 0.75); + } + + } + + else if(elevatorPos < 0) + { + elevatorRight.set(moveElevatorInput * 0.75); + } + */ + } + + +// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range + + + //PID encoder position + public double getEncoderElevatorPosition() + { + return elevatorRight.getPositionWorld(); + } + + public double getElevatorHeightInchesAboveFloor() + { + return elevatorRight.getPositionWorld(); + } + + public synchronized void setControlMode(DriveControlMode controlMode) + { + this.controlMode = controlMode; + + isFinished = false; + } + /* + public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); + } + + public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); + } + + public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); + } + + public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); + } + */ + public void rawSetOutput(double output){ + elevatorRight.set(/*ControlMode.PercentOutput,*/ output); + } + + public void holdInPos() + { + elevatorRight.set(-0.43 * 0.2); + } + + public void stopMotors() + { + elevatorRight.set(0); + } + + public void isSwitchPressed() + { + pressed = false; + isPressed = elevatorRight.getSensorCollection(); + + if(isPressed.isFwdLimitSwitchClosed() == true) + { + if (controlMode == DriveControlMode.JOYSTICK) { + Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS); + } + pressed = true; + } + else + { + if (controlMode == DriveControlMode.STOP_MOTORS){ + { + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + } + + pressed = false; + } + } + + } + + //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; + + + + + + @Override + public void controlLoopUpdate() + { + if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) + { + moveElevatorXbox(); + } + else if (!isFinished) + { + //PID control mode + if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) + { + isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) + { + isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) + { + isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + } + /* + else if(controlMode == DriveControlMode.RAW) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + } + */ + } + } + + @Override + public void initDefaultCommand() + { + } + + @Override + public void periodic() + { + // isSwitchPressed(); + } + + @Override + public void setPeriodMs(long periodMs) + { + //PID controller + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight); + + 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("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor()); + //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/Pnumatics.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java new file mode 100644 index 0000000..129b561 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java @@ -0,0 +1,46 @@ +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 new file mode 100644 index 0000000..c217c81 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java @@ -0,0 +1,228 @@ +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 new file mode 100644 index 0000000..8d3fc74 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java @@ -0,0 +1,48 @@ +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 new file mode 100644 index 0000000..52088f7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java @@ -0,0 +1,65 @@ +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 new file mode 100644 index 0000000..135bb97 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java @@ -0,0 +1,199 @@ +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 new file mode 100644 index 0000000..c53b589 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java @@ -0,0 +1,7 @@ +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 new file mode 100644 index 0000000..ac7be21 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java @@ -0,0 +1,79 @@ +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 new file mode 100644 index 0000000..ea3af27 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java @@ -0,0 +1,894 @@ +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 new file mode 100644 index 0000000..7c6de07 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java @@ -0,0 +1,91 @@ +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 new file mode 100644 index 0000000..dc9f6a4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 0000000..e3c3910 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java @@ -0,0 +1,59 @@ +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 new file mode 100644 index 0000000..28dced9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java @@ -0,0 +1,137 @@ +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 new file mode 100644 index 0000000..b91fc31 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java @@ -0,0 +1,156 @@ +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 new file mode 100644 index 0000000..9a9d760 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java @@ -0,0 +1,233 @@ + +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 new file mode 100644 index 0000000..b9e8fca --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java @@ -0,0 +1,113 @@ +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 new file mode 100644 index 0000000..9863dc4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java @@ -0,0 +1,111 @@ +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 new file mode 100644 index 0000000..eb2367c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java @@ -0,0 +1,184 @@ +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 new file mode 100644 index 0000000..358138b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java @@ -0,0 +1,15 @@ +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 new file mode 100644 index 0000000..c989f65 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java @@ -0,0 +1,62 @@ +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 new file mode 100644 index 0000000..163c52d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java @@ -0,0 +1,260 @@ +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 new file mode 100644 index 0000000..f99df73 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java @@ -0,0 +1,232 @@ +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 new file mode 100644 index 0000000..df9499d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java @@ -0,0 +1,89 @@ +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 new file mode 100644 index 0000000..c48b552 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java @@ -0,0 +1,131 @@ +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 new file mode 100644 index 0000000..4da86a3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java @@ -0,0 +1,124 @@ +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 new file mode 100644 index 0000000..7bd6abd --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java @@ -0,0 +1,114 @@ +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 new file mode 100644 index 0000000..383acd7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java @@ -0,0 +1,85 @@ + +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 new file mode 100644 index 0000000..05417a8 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java @@ -0,0 +1,104 @@ +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 new file mode 100644 index 0000000..2551e4d --- /dev/null +++ b/2019robot/vendordeps/PathfinderOLD.json @@ -0,0 +1,85 @@ +{ + "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 new file mode 100644 index 0000000..a1654ec --- /dev/null +++ b/2019robot/vendordeps/Phoenix.json @@ -0,0 +1,87 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.12.1", + "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.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.12.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.12.1", + "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.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.12.1", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file diff --git a/2019robot/vendordeps/navx_frc.json b/2019robot/vendordeps/navx_frc.json new file mode 100644 index 0000000..80defba --- /dev/null +++ b/2019robot/vendordeps/navx_frc.json @@ -0,0 +1,33 @@ +{ + "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